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指南，建议先自己跟着做一遍，加深理解
http://wiki.ros.org/actionlib/Tutorials
先来看节点图（server端），不知你有没有注意，goal的箭头是指向节点的，跟youbot_driver的一样。也就是说只要有个client一直给他关于这个topic的信息就可以达到控制的目的了。
action（client端）
由此，我们可以有个结论，加载了action模块，他就有这5个topic。看名字，我们首先要分析的就是goal。
查看一下topic的type和数据结构
rostopic type /arm_1/arm_controller/goal
查看数据结构，这里有个小技巧
rostopic 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的信息，http://wiki.ros.org/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中的仿真图(视频地址)
来源：CSDN
作者：yaked
链接：https://blog.csdn.net/yaked/article/details/45098549