Actionlib 与KUKA youbot机械臂

版权声明:本文为博主原创文章,如需转载,请附上文章原文地址。 https://blog.csdn.net/yaked/article/details/45098549


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的信息,最下面还真有两个,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中的仿真图(视频地址)

点击打开链接

没有更多推荐了,返回首页