Actionlib 與KUKA youbot機械臂
阿新 • • 發佈：2019-02-10
1.目前已經獲得的情報
這篇文章是在對youbot機械臂進行dynamic 控制之後，我們的控制從關節角度控制到dynamic控制一路過來，但是這還是不過癮，比較笨拙，完全需要人遙控，關節角控制，一次輸一個點位還得重新編譯、執行。dynamic控制每個關節角，調節滑塊位置，機器人關節動作。但是必須小心，因為可能自碰撞。對於機器人正解，我的理解是經過轉移矩陣可以得到末端的位置和姿態，關於這個實驗室的師兄用matlab機器人工具箱已經做過，但是matlab和程式，和ROS怎麼結合呢？ROS官網有兩種辦法，一種是常規的Kinematic，涉及正反解，現有的庫是KDL（Kinematics and Dynamatics Library）；另一種是基於Action library，它的好處是帶有反饋，傳送一個指令，比如關節運動45度，機器人每次步進都得檢測有沒有到達45度，沒有的話接著走，到達了就停止。而actionlib就提供了這樣的反饋機制，可以自動去檢查執行指令後的位置。
在上圖中的youbot_driver提供的topic中可以看到有幾個關於follow_joint_trajectory的topic，此外根據
rosdriver的啟動資訊，parameter裡有一個/trajectoryActionServerEnable的引數，檢視後發現預設是啟動的，為此我就依據action來做文章。
官網actionlib指南，建議先自己跟著做一遍，加深理解
先來看節點圖（server端），不知你有沒有注意，goal的箭頭是指向節點的，跟youbot_driver的一樣。也就是說只要有個client一直給他關於這個topic的資訊就可以達到控制的目的了。
action（client端）
由此，我們可以有個結論，載入了action模組，他就有這5個topic。看名字，我們首先要分析的就是goal。
檢視一下topic的type和資料結構
rostopic type /arm_1/arm_controller/goalrostopic pub /arm_1/arm_controller/goal control_msgs/FollowJointTrajectoryActionGoal 雙擊tab鍵，自動補全格式
這個結構還是挺複雜的，也就是說不能通過topic pub 的辦法來和它通訊了。
2.繼續找資料
這個地方有關於Joint Trajectory Action的說明，不過它是用的PR2的，沒辦法，硬著頭皮上了。
往下看，你會發現它就是一個關於Action Client的c++程式，主體結構不用變，改joint名字，改action server的名字，手臂只有五個joint哦。編譯通過，跑起來，以為可以，結果：悲劇了.
原來是我們的topic名字也不一樣，一個是control_msgs/JointTrajectoryActionGoal,而youbot這邊是control_msgs/FollowJointTrajectoryActionGoal, 改唄。
查查官網control_msgs的資訊，最下面還真有兩個，JointTrajectory和FollowJointTrajectory。
然後，在命令列裡，看看control_msgs裡有什麼東西，因為你會發現，官網這裡查到的是沒有最後那個Goal的，那程式裡為什麼會有呢
這和我們之前說的很像，actionlib這種結構都有Goal， Result，Feedback等。
那我又要好奇一下了，既然不一樣，那不一樣在什麼地方呢，看看結構再說
其實FollowJointTrajectoryActonGoal就是多了一些tolerance（誤差範圍），path tolerance、goal tolerance和goal time tolerance。
一個簡單的action client控制手臂直立起來。
//
// Simple demo program that calls the youBot ROS wrapper with a trajectory
//
#include "ros/ros.h"
#include "actionlib/client/simple_action_client.h"
#include "control_msgs/FollowJointTrajectoryAction.h"
int main(int argc, char **argv) {
ros::init(argc, argv, "youbot_ros_simple_trajectory");
ros::NodeHandle n;
actionlib::SimpleActionClient<control_msgs::FollowJointTrajectoryAction> ac("arm_1/arm_controller/follow_joint_trajectory", true);
ROS_INFO("Waiting for action server to start.");
ac.waitForServer();
ROS_INFO("Action server started, sending trajectory.");
// setup trajectory message
control_msgs::FollowJointTrajectoryGoal msg;
// some arbitrary points of a not too useful trajectory
const int nPoints = 1;
double values[nPoints][5] = {
// {0.11, 0.11, -0.10, 0.11, 0.13},// Home
{2.562, 1.0488, -2.435, 1.73184, 0.13}, // Candle
};
// set values for all points of trajectory
for (int p = 0; p < nPoints; p++)
{ // iterate over all points
trajectory_msgs::JointTrajectoryPoint point;
for (int i = 0; i < 5; i++)
{ // 5 DOF
point.positions.push_back(values[p][i]);
point.velocities.push_back(0.03); // 0.1
point.accelerations.push_back(0);
}
point.time_from_start = ros::Duration((p+1)*5.0);
msg.trajectory.points.push_back(point);
}
// set joint names
for (int i = 0; i < 5; i++) {
std::stringstream jointName;
jointName << "arm_joint_" << (i + 1);
msg.trajectory.joint_names.push_back(jointName.str());
}
// fill message header and sent it out
msg.trajectory.header.frame_id = "base_link";
msg.trajectory.header.stamp = ros::Time::now();
ac.sendGoal(msg);
// wait for reply that action was completed (or cancel after 10 sec)
ac.waitForResult(ros::Duration(10));
return 0;
}特別注意的的地方是timefromstart，我這裡是手動給的值。
3.結尾
我們給了四個點，讓youbot機械臂途徑四個點，並限定速度、加速度、時間，是不是想起什麼了，機器人學裡的路徑規劃問題，五次多項式、拋物線等規劃，其實這裡我們就是按照五次多項式來規劃的，但是是隨意設定的速度和加速度，先加速，再勻速，再勻減速。程式如下：
//http://wiki.ros.org/pr2_controllers/Tutorials/Moving%20the%20arm%20using%20the%20Joint%20Trajectory%20Action
//2015年04月16日 20時34分43秒
#include <ros/ros.h>
#include <control_msgs/FollowJointTrajectoryAction.h>
#include <control_msgs/JointTolerance.h>
#include <actionlib/client/simple_action_client.h>
typedef actionlib::SimpleActionClient< control_msgs::FollowJointTrajectoryAction > TrajClient;
class RobotArm
{
private:
// Action client for the joint trajectory action
// used to trigger the arm movement action
TrajClient* traj_client_;
public:
//! Initialize the action client and wait for action server to come up
RobotArm()
{
// tell the action client that we want to spin a thread by default
traj_client_ = new TrajClient("arm_1/arm_controller/follow_joint_trajectory", true);
// wait for action server to come up
while(!traj_client_->waitForServer(ros::Duration(5.0))){
ROS_INFO("Waiting for the joint_trajectory_action server");
}
}
//! Clean up the action client
~RobotArm()
{
delete traj_client_;
}
//! Sends the command to start a given trajectory
void startTrajectory(control_msgs::FollowJointTrajectoryGoal goal)
{
// When to start the trajectory: 1s from now
goal.trajectory.header.stamp = ros::Time::now() + ros::Duration(1.0);
traj_client_->sendGoal(goal);
}
//! Generates a simple trajectory with two waypoints, used as an example
/*! Note that this trajectory contains two waypoints, joined together
as a single trajectory. Alternatively, each of these waypoints could
be in its own trajectory - a trajectory can have one or more waypoints
depending on the desired application.
*/
control_msgs::FollowJointTrajectoryGoal armExtensionTrajectory()
{
//our goal variable
control_msgs::FollowJointTrajectoryGoal goal;
// First, the joint names, which apply to all waypoints
goal.trajectory.joint_names.push_back("arm_joint_1");
goal.trajectory.joint_names.push_back("arm_joint_2");
goal.trajectory.joint_names.push_back("arm_joint_3");
goal.trajectory.joint_names.push_back("arm_joint_4");
goal.trajectory.joint_names.push_back("arm_joint_5");
// goal.trajectory.joint_names.push_back("r_wrist_flex_joint");
// goal.trajectory.joint_names.push_back("r_wrist_roll_joint");
// We will have two waypoints in this goal trajectory
goal.trajectory.points.resize(4);
// First trajectory point
// Positions
int ind = 0;
goal.trajectory.points[ind].positions.resize(5);
goal.trajectory.points[ind].positions[0] = 0.15;
goal.trajectory.points[ind].positions[1] = 0.15;
goal.trajectory.points[ind].positions[2] = -0.21;
goal.trajectory.points[ind].positions[3] = 0.15;
goal.trajectory.points[ind].positions[4] = 0.15;
// goal.trajectory.points[ind].positions[5] = 0.0;
// goal.trajectory.points[ind].positions[6] = 0.0;
// Velocities
goal.trajectory.points[ind].velocities.resize(5);
for (size_t a = 0; a < 5; ++a)
{
goal.trajectory.points[ind].velocities[a] = 0.0;
}
goal.trajectory.points[ind].accelerations.resize(5);
for (size_t b = 0; b < 5; ++b)
{
goal.trajectory.points[ind].accelerations[b] = 0.001;
}
// To be reached 1 second after starting along the trajectory
goal.trajectory.points[ind].time_from_start = ros::Duration(5.0);
// goal.goal_tolerance.push_back('arm_joint_1', 0.1, 0.01, 0.001);
// control_msgs::JointTolerance_<std::allocator<void> > jtolerance;
// jtolerance.name nm;
// nm = "arm_joint_1";
// goal.goal_tolerance.push_back(&jtolerance);
control_msgs::JointTolerance jt;
jt.name = "arm_joint_1";
jt.position = 0.1;
jt.velocity = 0.01;
jt.acceleration = 0.001;
goal.goal_tolerance.push_back(jt);
// std::cout<< "______________________jt_____________"<<jt;
// const control_msgs::JointTolerance_<std::allocator<void> >& t = n ;
//
//
// goal.goal_tolerance.push_back( t);
// goal.goal_tolerance.push_back( const control_msgs::JointTolerance_<std::allocator<void> > 'arm_joint_1'&);
// note: void std::vector<_Tp, _Alloc>::push_back(const value_type&) [with _Tp = control_msgs::JointTolerance_<std::allocator<void> >, _Alloc = std::allocator<control_msgs::JointTolerance_<std::allocator<void> > >, std::vector<_Tp, _Alloc>::value_type = control_msgs::JointTolerance_<std::allocator<void> >]
///usr/include/c++/4.6/bits/stl_vector.h:826:7: note: no known conversion for argument 1 from ‘double’ to ‘const value_type& {aka const control_msgs::JointTolerance_<std::allocator<void> >&}’
// goal.control_msgs::FollowJointTrajectoryGoal_<std::allocator<void> >::goal_tolerance
// goal.goal_tolerance.position = 0.1;
// goal.goal_tolerance.velocity = 0.01;
// goal.goal_tolerance.acceleration = 0.001;
// std::cout<< "name1"<<goal.goal_tolerance;
// 2 trajectory point
// Positions
ind += 1;
goal.trajectory.points[ind].positions.resize(5);
goal.trajectory.points[ind].positions[0] = 2.562;
goal.trajectory.points[ind].positions[1] = 1.05;
goal.trajectory.points[ind].positions[2] = -2.43;
goal.trajectory.points[ind].positions[3] = 1.73;
goal.trajectory.points[ind].positions[4] = 0.12;
// Velocities
goal.trajectory.points[ind].velocities.resize(5);
for (size_t c = 0; c < 5; ++c)
{
goal.trajectory.points[ind].velocities[c] = 0.005;
}
// Acceleration
goal.trajectory.points[ind].accelerations.resize(5);
for (size_t d = 0; d < 5; ++d)
{
goal.trajectory.points[ind].accelerations[d] = 0.0;
}
// To be reached 1 second after starting along the trajectory
goal.trajectory.points[ind].time_from_start = ros::Duration(20.0);
// 3 trajectory point
// Positions
ind += 1;
goal.trajectory.points[ind].positions.resize(5);
goal.trajectory.points[ind].positions[0] = 2.94961;
goal.trajectory.points[ind].positions[1] = 1.352;
goal.trajectory.points[ind].positions[2] = -2.591;
goal.trajectory.points[ind].positions[3] = 0.1;
goal.trajectory.points[ind].positions[4] = 0.12;
// Velocities
goal.trajectory.points[ind].velocities.resize(5);
for (size_t e = 0; e < 5; ++e)
{
goal.trajectory.points[ind].velocities[e] = 0.005;
}
// Acceleration
goal.trajectory.points[ind].accelerations.resize(5);
for (size_t f = 0; f < 5; ++f)
{
goal.trajectory.points[ind].accelerations[f] = 0.0;
}
// To be reached 1 second after starting along the trajectory
goal.trajectory.points[ind].time_from_start = ros::Duration(30.0);
// 4 trajectory point
// Positions
ind += 1;
goal.trajectory.points[ind].positions.resize(5);
goal.trajectory.points[ind].positions[0] = 0.11;
goal.trajectory.points[ind].positions[1] = 0.11;
goal.trajectory.points[ind].positions[2] = -0.11;
goal.trajectory.points[ind].positions[3] = 0.11;
goal.trajectory.points[ind].positions[4] = 0.12;
// Velocities
goal.trajectory.points[ind].velocities.resize(5);
for (size_t j = 0; j < 5; ++j)
{
goal.trajectory.points[ind].velocities[j] = 0.0;
}
// Acceleration
goal.trajectory.points[ind].accelerations.resize(5);
for (size_t h = 0; h < 5; ++h)
{
goal.trajectory.points[ind].accelerations[h] = -0.001;
}
// To be reached 2 seconds after starting along the trajectory
goal.trajectory.points[ind].time_from_start = ros::Duration(40.0);
//we are done; return the goal
return goal;
}
//! Returns the current state of the action
actionlib::SimpleClientGoalState getState()
{
return traj_client_->getState();
}
};
int main(int argc, char** argv)
{
// Init the ROS node
ros::init(argc, argv, "youbot_arm_action_trajectory");
RobotArm arm;
// Start the trajectory
arm.startTrajectory(arm.armExtensionTrajectory());
// Wait for trajectory completion
while(!arm.getState().isDone() && ros::ok())
{
usleep(50000);
}
}4. 總結
actionlib 其實是基於Server/Client模式，它最大的好處就是，也是我們之前反覆提過的，他有result、有feedback而不用你經常去檢查結果，只要你檢視一下相關topic就就行了。另外，
1.關於軌跡規劃，並不是像我們這裡測試用，任意給的一些數值，是要花些功夫在裡面的。
2.最後程式，其實還有很大的改進的地方，比如速度加速度那些
這裡都設成了一樣的，你可以不必用那個for迴圈，就按照一個一個來處理。
3. 觀察tolerance這裡我只給了Joint1的tolerance，你可以修改試試，給定其他關節的，還有path_tolerance、goal_time_tolerance等。
在Riz中的模擬圖(視訊地址)