Actionlib 与KUKA youbot机械臂

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.继续找资料

http://wiki.ros.org/pr2_controllers/Tutorials/Moving%20the%20arm%20using%20the%20Joint%20Trajectory%20Action

这个地方有关于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中的仿真图(视频地址)

点击打开链接

其他参考： 

https://www.cnblogs.com/CZM-/p/6283108.html