做机械臂导航时遇到的问题7：正向运动学求解：在关节空间进行规划

有时已经知道机械臂目标位姿的各关节信息，对于这种情况，我们用moveit可以很简单的将机械臂移动到目标位姿。
本篇主要是参考《ROS by example （volume 2）》里的实例，在pi_robot代码的基础上修改。
还有一些资料:http://www.360doc.com/content/16/0825/16/7821691_585865831.shtml

一、配置moveit关节控制器
ArbotiX的包提供了一个JTAC（Joint Trajectory Action Controller），它一方面直接控制伺服电机，另一方面使用Ros中的FollowJointTrajectoryAction去接收程序发过来的joint轨迹目标。
所以：需要做的有2点：
（1）配置一个底层硬件实际用到的JTAC。 这个JTAC会负责接收FollowJointTrajectoryAction内容。
（2）配置一个moveit！插件，这个插件负责将JTAC与moveit！之间的通信抽象化。 这个插件叫moveit_simple_controller_manager，它可以配合FollowJointTrajectoryAction工作。
需要我们自己做的事情就是配置参数将它们联系起来（topic和actions的名字）
1、创建controllers.yaml文件

controller_list:
  - name: right_arm_controller
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    default: true
    joints:
      - joint_1
      - joint_2
      - joint_3

这个文件放置在moveit配置文件的config目录下，我们用来测试的机械臂没有抓取的手爪，只用到了right_arm_controller这个控制器。

注意：这个right_arm_controller控制器命名可以修改，但是与当初我们测试arbotix时的配置文件有关。我们当初学习arbotix节点控制伺服电机时，为arbotix配置了文件，名为：my_robot_arbotix.yaml,在里面我们命名了一些控制器controller，这里的controller命名、action_name必须要与moveit配置里的controllers.yaml文件中设置的action_ns相匹配。

2、创建控制器管理器启动文件robot_arm_moveit_controller_manager.launch.xml

这个文件放置在moveit配置文件的launch目录下,他会读取我们的controllers.yaml中设置好的控制器参数。

<launch>

  <!-- Set the param that trajectory_execution_manager needs to find the controller plugin -->
  <arg name="moveit_controller_manager" default="moveit_simple_controller_manager/MoveItSimpleControllerManager" />
  <param name="moveit_controller_manager" value="$(arg moveit_controller_manager)"/>


  <!-- load controller_list -->


<arg name="use_controller_manager" default="true" />


<!-- Load joint controller configurations from YAML file to parameter server -->


  <rosparam file="$(find arm_moveit_config)/config/controllers.yaml"/>


</launch>

二、关节空间的正运动学规划
1、用arbotix关节控制器启动虚拟机械臂模型

xs@xs-PC:~/catkin_ws2/src/mybot_description/launch$ roslaunch my_robot_arbotix_ok.launch 

文件代码内容如下：（我是在教材例子中进行修改的，与机械臂控制无关的部分，比如right_gripper_controller，已经被注释掉了）
大致内容是;
（1）加载urdf模型
（2）启动arbotix，包括上面我们提到的配置的一些控制器
（3）启动tf节点，这里是我参考其他的文件时加上的。我在调试时，没有这个tf节点，规划过程中可以plan（）出规划结果，但是excute（）失败，加上之后就可以了，具体原因还不清楚，找到后，我会再补充。目前猜测可能是odom坐标系与机械臂的base_link之间缺少tf关系。
（4）启动robot_state_publisher

<launch>
   <!-- Make sure we are not using simulated time -->
   <param name="/use_sim_time" value="false" />
  
   <!-- Launch the arbotix driver in fake mode by default -->
   <arg name="sim" default="true" />
   
   <!-- If using a real controller, look on /dev/ttyUSB0 by default -->
   <arg name="port" default="/dev/ttyUSB0" />
   
   <!-- Load the URDF/Xacro model of our robot -->
   <param name="robot_description" textfile="/home/xs/catkin_ws2/src/mybot_description/urdf/robot_arm.urdf" />
   
   <!-- Bring up the arbotix driver with a configuration file appropriate to the robot -->
   <node name="arbotix" pkg=