版权声明：本文为博主原创文章，未经博主允许不得转载。 https://blog.csdn.net/lxn9492878lbl/article/details/84795591
关节控制，需要的功能包有臂，头，手臂，手指，末端执行器这些包。话题和动作都可以实现这个功能。
１臂的控制
/torso_controller/command (trajectory_msgs/JointTrajectory)
其中括号里的是消息类型，前面的是话题
２头的控制
/head_controller/command (trajectory_msgs/JointTrajectory)
３手臂的控制
/arm_controller/command (trajectory_msgs/JointTrajectory)
４手指的控制
/hand_controller/command (trajectory_msgs/JointTrajectory)
5执行器的控制
/parallel_gripper_controller/command (trajectory_msgs/JointTrajectory)
感觉还是很对称的，很好记
启动
启动的命令我在之前介绍过，都是相同的
运行节点
重点也是这个节点，可以稍作修改实现你自己的想要到达位置的功能。
rosrun tiago_trajectory_controller run_traj_control
下面贴出官网里的代码，
http://wiki.ros.org/Robots/TIAGo/Tutorials/trajectory_controller
// C++ standard headers
2 #include <exception>
3 #include <string>
4
5 // Boost headers
6 #include <boost/shared_ptr.hpp>
7
8 // ROS headers
9 #include <ros/ros.h>
10 #include <actionlib/client/simple_action_client.h>
11 #include <control_msgs/FollowJointTrajectoryAction.h>
12 #include <ros/topic.h>
13
14
15 // Our Action interface type for moving TIAGo's head, provided as a typedef for convenience
16 typedef actionlib::SimpleActionClient<control_msgs::FollowJointTrajectoryAction> arm_control_client;
17 typedef boost::shared_ptr< arm_control_client> arm_control_client_Ptr;
18
19
20 // Create a ROS action client to move TIAGo's arm
21 void createArmClient(arm_control_client_Ptr& actionClient)
22 {
23 ROS_INFO("Creating action client to arm controller ...");
24
25 actionClient.reset( new arm_control_client("/arm_controller/follow_joint_trajectory") );
26
27 int iterations = 0, max_iterations = 3;
28 // Wait for arm controller action server to come up
29 while( !actionClient->waitForServer(ros::Duration(2.0)) && ros::ok() && iterations < max_iterations )
30 {
31 ROS_DEBUG("Waiting for the arm_controller_action server to come up");
32 ++iterations;
33 }
34
35 if ( iterations == max_iterations )
36 throw std::runtime_error("Error in createArmClient: arm controller action server not available");
37 }
38
39
40 // Generates a simple trajectory with two waypoints to move TIAGo's arm
41 void waypoints_arm_goal(control_msgs::FollowJointTrajectoryGoal& goal)
42 {
43 // The joint names, which apply to all waypoints
44 goal.trajectory.joint_names.push_back("arm_1_joint");
45 goal.trajectory.joint_names.push_back("arm_2_joint");
46 goal.trajectory.joint_names.push_back("arm_3_joint");
47 goal.trajectory.joint_names.push_back("arm_4_joint");
48 goal.trajectory.joint_names.push_back("arm_5_joint");
49 goal.trajectory.joint_names.push_back("arm_6_joint");
50 goal.trajectory.joint_names.push_back("arm_7_joint");
51
52 // Two waypoints in this goal trajectory
53 goal.trajectory.points.resize(2);
54
55 // First trajectory point
56 // Positions
57 int index = 0;
58 goal.trajectory.points[index].positions.resize(7);
59 goal.trajectory.points[index].positions[0] = 0.2;
60 goal.trajectory.points[index].positions[1] = 0.0;
61 goal.trajectory.points[index].positions[2] = -1.5;
62 goal.trajectory.points[index].positions[3] = 1.94;
63 goal.trajectory.points[index].positions[4] = -1.57;
64 goal.trajectory.points[index].positions[5] = -0.5;
65 goal.trajectory.points[index].positions[6] = 0.0;
66 // Velocities
67 goal.trajectory.points[index].velocities.resize(7);
68 for (int j = 0; j < 7; ++j)
69 {
70 goal.trajectory.points[index].velocities[j] = 1.0;
71 }
72 // To be reached 2 second after starting along the trajectory
73 goal.trajectory.points[index].time_from_start = ros::Duration(2.0);
74
75 // Second trajectory point
76 // Positions
77 index += 1;
78 goal.trajectory.points[index].positions.resize(7);
79 goal.trajectory.points[index].positions[0] = 2.5;
80 goal.trajectory.points[index].positions[1] = 0.2;
81 goal.trajectory.points[index].positions[2] = -2.1;
82 goal.trajectory.points[index].positions[3] = 1.9;
83 goal.trajectory.points[index].positions[4] = 1.0;
84 goal.trajectory.points[index].positions[5] = -0.5;
85 goal.trajectory.points[index].positions[6] = 0.0;
86 // Velocities
87 goal.trajectory.points[index].velocities.resize(7);
88 for (int j = 0; j < 7; ++j)
89 {
90 goal.trajectory.points[index].velocities[j] = 0.0;
91 }
92 // To be reached 4 seconds after starting along the trajectory
93 goal.trajectory.points[index].time_from_start = ros::Duration(4.0);
94 }
95
96
97 // Entry point
98 int main(int argc, char** argv)
99 {
100 // Init the ROS node
101 ros::init(argc, argv, "run_traj_control");
102
103 ROS_INFO("Starting run_traj_control application ...");
104
105 // Precondition: Valid clock
106 ros::NodeHandle nh;
107 if (!ros::Time::waitForValid(ros::WallDuration(10.0))) // NOTE: Important when using simulated clock
108 {
109 ROS_FATAL("Timed-out waiting for valid time.");
110 return EXIT_FAILURE;
111 }
112
113 // Create an arm controller action client to move the TIAGo's arm
114 arm_control_client_Ptr ArmClient;
115 createArmClient(ArmClient);
116
117 // Generates the goal for the TIAGo's arm
118 control_msgs::FollowJointTrajectoryGoal arm_goal;
119 waypoints_arm_goal(arm_goal);
120
121 // Sends the command to start the given trajectory 1s from now
122 arm_goal.trajectory.header.stamp = ros::Time::now() + ros::Duration(1.0);
123 ArmClient->sendGoal(arm_goal);
124
125 // Wait for trajectory execution
126 while(!(ArmClient->getState().isDone()) && ros::ok())
127 {
128 ros::Duration(4).sleep(); // sleep for four seconds
129 }
130
131 return EXIT_SUCCESS;
132 }
注意
这个代码是控制手臂的，其他的应该没有，读了下