ROS学习笔记（二）体验使用MoveIt！控制自己的仿真机械臂

学习资料：胡春旭《ROS机械人开发实践》
笔记内容：参考第十章《MoceIt！ 机械臂控制》，创建一个自己的6R机械臂，完成创建机械臂模型、配置文件、ArbotiX关节控制、正逆运动学控制等
（初学ROS，仅供参考）

(一） 创建xacro机械臂模型

Ctrl+Alt+T新建一个终端，创建一个名为“my_sixr_robot”的功能包，输入以下代码：

cd ~/catkin_ws/src
catkin_create_pkg my_sixr_robot urdf xacro
cd my_sixr_robot
mkdir urdf
cd urdf
gedit arm.xacro

创建基座、夹爪和一个如下图所示的腕部分离6R机械臂

在打开的arm.xacro文件中输入以下代码：

<?xml version="1.0"?>
<robot name="arm" xmlns:xacro="http://www.ros.org/wiki/xacro">

<xacro:property name="M_PI" value="3.141593"/>

<xacro:property name="base_link_radius" value="1"/>
<xacro:property name="base_link_length" value="0.3"/>
<xacro:property name="arm0_link_radius" value="0.4"/>
<xacro:property name="arm0_link_length" value="0.5"/>
<xacro:property name="arm1_link_radius" value="0.4"/>
<xacro:property name="arm1_link_length" value="2"/>
<xacro:property name="arm2_link_radius" value="0.4"/>
<xacro:property name="arm2_link_length" value="2"/>
<xacro:property name="arm3_link_radius" value="0.4"/>
<xacro:property name="arm3_link_length" value="0.2"/>
<xacro:property name="arm4_link_radius" value="0.4"/>
<xacro:property name="arm4_link_length" value="2"/>
<xacro:property name="arm5_link_radius" value="0.4"/>
<xacro:property name="arm5_link_length" value="0.2"/>
<xacro:property name="arm6_link_radius" value="0.4"/>
<xacro:property name="arm6_link_length" value="2"/>
<xacro:property name="finger_base_link_radius" value="0.4"/>
<xacro:property name="finger_base_link_length" value="0.2"/>
<xacro:property name="left_finger_link_length" value="0.1"/>
<xacro:property name="left_finger_link_width" value="0.1"/>
<xacro:property name="left_finger_link_height" value="0.4"/>
<xacro:property name="right_finger_link_length" value="0.1"/>
<xacro:property name="right_finger_link_width" value="0.1"/>
<xacro:property name="right_finger_link_height" value="0.4"/>


<xacro:macro name="inertial_matrix" params="mass">
  <inertial>
    <mass value="${mass}"/>
    <inertia ixx="1" ixy="0" ixz="0" iyy="0.5" iyz="0.0" izz="1"/>
    </inertial>
</xacro:macro>

<material name="White">
  <color rgba="1 1 1 1"/>
</material>

<material name="Blue">
  <color rgba="0 0 1 1"/>
</material> 

<link name="base_link">
  <visual>
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <geometry>
       <cylinder radius="${base_link_radius}" length="${base_link_length}"/>
    </geometry>
    <material name="White"/>
  </visual>
  <collision>
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <geometry>
      <cylinder radius="${base_link_radius}" length="${base_link_length-0.2}"/>
    </geometry>
  </collision>
  <xacro:inertial_matrix mass="10"/>
</link>

<link name="arm0_link">
  <visual>
    <origin xyz="0 0 ${1*(arm0_link_length)/2}" rpy="0 0 0"/>
    <geometry>
       <cylinder radius="${arm0_link_radius}" length="${arm0_link_length}"/>
    </geometry>
    <material name="White"/> 
  </visual>
  <collision>
    <origin xyz="0 0 ${1*(arm0_link_length)/2}" rpy="0 0 0"/>
    <geometry>
       <cylinder radius="${arm0_link_radius}" length="${arm0_link_length}"/>
    </geometry>
  </collision>
  <xacro:inertial_matrix mass="1"/>
</link>

<link name="arm1_link">
  <visual>
    <origin xyz="0 0 ${arm1_link_length/2}" rpy="0 0 0"/>
    <geometry>
       <cylinder radius="${arm1_link_radius}" length="${arm1_link_length}"/>
    </geometry>
    <material name="Blue"/> 
  </visual>
  <collision>
    <origin xyz="0 0 ${arm1_link_length/2}" rpy="0 0 0"/>
    <geometry>
       <cylinder radius="${arm1_link_radius}" length="${arm1_link_length-2*arm1_link_radius}"/>
    </geometry>
  </collision>
  <xacro:inertial_matrix mass="2"/>
</link>

<link name="arm2_link">
  <visual>
    <origin xyz="${1*arm2_link_length/2} 0 0" rpy="0 ${M_PI/2} 0"/>
    <geometry>
       <cylinder radius="${arm2_link_radius}" length="${arm2_link_length}"/>
    </geometry>
    <material name="Blue"/> 
  </visual>
  <collision>
    <origin xyz="${1*arm2_link_length/2} 0 0" rpy="0 ${M_PI/2} 0"/>
    <geometry>
       <cylinder radius="${arm2_link_radius}" length="${arm2_link_length-2*arm2_link_radius}"/>
    </geometry>
  </collision>
  <xacro:inertial_matrix mass="2"/>
</link>

<link name="arm3_link">
  <visual>
    <origin xyz="${1*arm3_link_length/2} 0 0" rpy="0 ${M_PI/2} 0"/>
    <geometry>
       <cylinder radius="${arm3_link_radius}" length="${arm3_link_length}"/>
    </geometry>
    <material name="Blue"/> 
  </visual>
  <xacro:inertial_matrix mass="0.2"/>
</link>

<link name="arm4_link">
  <visual>
    <origin xyz="0 0 ${1*arm4_link_length/2}" rpy="0 0 0"/>
    <geometry>
       <cylinder radius="${arm4_link_radius}" length="${arm4_link_length}"/>
    </geometry>
    <material name="Blue"/> 
  </visual>
  <collision>
    <origin xyz="0 0 ${1*arm4_link_length/2}" rpy="0 0 0"/>
    <geometry>
       <cylinder radius="${arm4_link_radius}" length="${arm4_link_length-2*arm4_link_radius}"/>
    </geometry>
  </collision>
  <xacro:inertial_matrix mass="2"/>
</link>

<link name="arm5_link">
  <visual>
    <origin xyz="0 ${-1*arm5_link_length/2} 0" rpy="${M_PI/2} 0 0"/>
    <geometry>
       <cylinder radius="${arm5_link_radius}" length="${arm5_link_length}"/>
    </geometry>
    <material name="Blue"/> 
  </visual>
  <xacro:inertial_matrix mass="0.2"/>
</link>

<link name="arm6_link">
  <visual>
    <origin xyz="0 0 ${arm6_link_length/2}" rpy="0 0 0"/>
    <geometry>
       <cylinder radius="${arm6_link_radius}" length="${arm6_link_length}"/>
    </geometry>
    <material name="Blue"/> 
  </visual>
  <collision>
    <origin xyz="0 0 ${arm6_link_length/2}" rpy="0 0 0"/>
    <geometry>
       <cylinder radius="${arm6_link_radius}" length="${arm6_link_length-2*arm6_link_radius}"/>
    </geometry>
  </collision>
  <xacro:inertial_matrix mass="2"/>
</link>

<link name="finger_base_link">
  <visual>
    <origin xyz="0 0 ${finger_base_link_length/2}" rpy="0 0 0"/>
    <geometry>
       <cylinder radius="${finger_base_link_radius}" length="${finger_base_link_length}"/>
    </geometry>
    <material name="White"/> 
  </visual>
  <collision>
    <origin xyz="0 0 ${finger_base_link_length/2}" rpy="0 0 0"/>
    <geometry>
       <cylinder radius="${finger_base_link_radius}" length="${finger_base_link_length}"/>
    </geometry>
  </collision>
  <xacro:inertial_matrix mass="0.5"/>
</link>

<link name="left_finger_link">
  <visual>
    <origin xyz="0 0 ${left_finger_link_height/2}" rpy="0 0 0"/>
    <geometry>
       <box size="${left_finger_link_length} ${left_finger_link_width} ${left_finger_link_height}"/>
    </geometry>
    <material name="White"/> 
  </visual>
  <collision>
    <origin xyz="0 0 ${left_finger_link_height/2}" rpy="0 0 0"/>
    <geometry>
       <box size="${left_finger_link_length} ${left_finger_link_width} ${left_finger_link_height}"/>
    </geometry>
  </collision>
  <xacro:inertial_matrix mass="0.3"/>
</link>

<link name="right_finger_link">
  <visual>
    <origin xyz="0 0 ${right_finger_link_height/2}" rpy="0 0 0"/>
    <geometry>
       <box size="${right_finger_link_length} ${right_finger_link_width} ${right_finger_link_height}"/>
    </geometry>
    <material name="White"/> 
  </visual>
  <collision>
    <origin xyz="0 0 ${right_finger_link_height/2}" rpy="0 0 0"/>
    <geometry>
       <box size="${right_finger_link_length} ${right_finger_link_width} ${right_finger_link_height}"/>
    </geometry>
  </collision>
  <xacro:inertial_matrix mass="0.3"/>
</link>

<link name="grasping_frame_link"/>

<joint name="base_joint" type="fixed">
  <parent link="base_link"/>
  <child link="arm0_link"/>
  <origin xyz="0 0 ${base_link_length/2}" rpy="0 0 0"/>
</joint>

<joint name="arm01_joint" type="revolute">
  <parent link="arm0_link"/>
  <child link="arm1_link"/>
  <origin xyz="0 0 ${arm0_link_length}" rpy="0 0 0"/>
  <axis xyz="0 0 1"/>
  <limit effort="300" velocity="1" lower="${-1*M_PI}" upper="${M_PI}"/>
  <dynamics damping="50" friction="1"/>
</joint>

<joint name="arm12_joint" type="revolute">
  <parent link="arm1_link"/>
  <child link="arm2_link"/>
  <origin xyz="0 0 ${arm1_link_length}" rpy="${-1*M_PI/2} 0 0"/>
  <axis xyz="0 0 1"/>
  <limit effort="300" velocity="1" lower="${-1*M_PI/2}" upper="${M_PI/2}"/>
  <dynamics damping="50" friction="1"/>
</joint>

<joint name="arm23_joint" type="revolute">
  <parent link="arm2_link"/>
  <child link="arm3_link"/>
  <origin xyz="${arm2_link_length} 0 0" rpy="0 0 0"/>
  <axis xyz="0 0 1"/>
  <limit effort="300" velocity="1" lower="${-1*M_PI/2}" upper="${M_PI/2}"/>
  <dynamics damping="50" friction="1"/>
</joint>

<joint name="arm34_joint" type="revolute">
  <parent link="arm3_link"/>
  <child link="arm4_link"/>
  <origin xyz="${arm3_link_length} 0 0" rpy="${M_PI/2} 0 ${M_PI/2}"/>
  <axis xyz="0 0 1"/>
  <limit effort="300" velocity="1" lower="${-1*M_PI/2}" upper="${M_PI/2}"/>
  <dynamics damping="50" friction="1"/>
</joint>

<joint name="arm45_joint" type="revolute">
  <parent link="arm4_link"/>
  <child link="arm5_link"/>
  <origin xyz="0 0 ${arm4_link_length}" rpy="${-1*M_PI/2} 0 0"/>
  <axis xyz="0 0 1"/>
  <limit effort="300" velocity="1" lower="${-1*M_PI/2}" upper="${M_PI/2}"/>
  <dynamics damping="50" friction="1"/>
</joint>

<joint name="arm56_joint" type="revolute">
  <parent link="arm5_link"/>
  <child link="arm6_link"/>
  <origin xyz="0 ${-1*arm5_link_length} 0" rpy="${M_PI/2} 0 0"/>
  <axis xyz="0 0 1"/>
  <limit effort="300" velocity="1" lower="${-1*M_PI/2}" upper="${M_PI/2}"/>
  <dynamics damping="50" friction="1"/>
</joint>

<joint name="finger_base_joint" type="fixed">
  <parent link="arm6_link"/>
  <child link="finger_base_link"/>
  <origin xyz="0 0 ${arm6_link_length}" rpy="0 0 0"/>
</joint>

<joint name="left_finger_joint" type="fixed">
  <parent link="finger_base_link"/>
  <child link="left_finger_link"/>
  <origin xyz="${finger_base_link_radius/2} 0 ${finger_base_link_length}" rpy="0 0 0"/>
</joint>

<joint name="right_finger_joint" type="prismatic">
  <parent link="finger_base_link"/>
  <child link="right_finger_link"/>
  <origin xyz="-${finger_base_link_radius/2} 0 ${finger_base_link_length}" rpy="0 0 0"/>
  <axis xyz="1 0 0"/>
  <limit effort="300" velocity="1" lower="0" upper="${finger_base_link_radius-right_finger_link_length}"/>
  <dynamics damping="50" friction="1"/>
</joint>

<joint name="grasping_frame_joint" type="fixed">
  <parent link="finger_base_link"/>
  <child link="grasping_frame_link"/>
  <origin xyz="0 0 ${finger_base_link_length}" rpy="0 0 0"/>
</joint>

<gazebo reference="base_link">
  <material>Gazebo/White</material>
</gazebo>

<gazebo reference="arm0_link">
  <material>Gazebo/White</material>
</gazebo>

<gazebo reference="arm1_link">
  <material>Gazebo/Blue</material>
</gazebo>

<gazebo reference="arm2_link">
  <material>Gazebo/Blue</material>
</gazebo>

<gazebo reference="arm3_link">
  <material>Gazebo/Blue</material>
</gazebo>

<gazebo reference="arm4_link">
  <material>Gazebo/Blue</material>
</gazebo>

<gazebo reference="arm5_link">
  <material>Gazebo/Blue</material>
</gazebo>

<gazebo reference="arm6_link">
  <material>Gazebo/Blue</material>
</gazebo>

<gazebo reference="finger_base_link">
  <material>Gazebo/White</material>
</gazebo>

<gazebo reference="left_finger_link">
  <material>Gazebo/White</material>
</gazebo>

<gazebo reference="right_finger_link">
  <material>Gazebo/White</material>
</gazebo>

<xacro:macro name="transmission_block" params="joint_name">
  <transmission name="tran1">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="${joint_name}">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor1">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
</xacro:macro>

<xacro:transmission_block joint_name="base_joint"/>
<xacro:transmission_block joint_name="arm01_joint"/>
<xacro:transmission_block joint_name="arm12_joint"/>
<xacro:transmission_block joint_name="arm23_joint"/>
<xacro:transmission_block joint_name="arm34_joint"/>
<xacro:transmission_block joint_name="arm45_joint"/>
<xacro:transmission_block joint_name="arm56_joint"/>
<xacro:transmission_block joint_name="finger_base_joint"/>
<xacro:transmission_block joint_name="left_finger_joint"/>
<xacro:transmission_block joint_name="right_finger_joint"/>

<gazebo>
  <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
    <robotNamespace>/arm/</robotNamespace>
  </plugin>
</gazebo>

</robot>

(二） 显示机器人模型

在终端中输入以下代码：

cd ~/catkin_ws/src/my_sixr_robot
mkdir launch
cd launch
gedit display_sixr_robot.launch

在打开的display_sixr_robot.launch文件中输入以下内容：

<launch>
  <arg name="model"/>

  <param name="robot_description" command="$(find xacro)/xacro --inorder $(find my_sixr_robot)/urdf/arm.xacro"/>

  <param name="use_gui" value="true"/>
 
<node name="joint_state_publisher_gui" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui"/>

  <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher"/>

  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find my_sixr_robot)/urdf.rviz" required="true" />

</launch>

launch文件将模型放置到rviz中可视化显示，从而验证设计的正确性。
在终端输入以下代码：

roslaunch my_sixr_robot display_sixr_robot.launch

出现以下窗口：

将Fixed Frame选项中的map改为base_link

点击左下角“ADD”选择“RobotModel”后点击“OK”

将出现创建的6R机械臂

滑动gui窗口横条，可以改变关节角度，验证模型的正确性

(三） 使用Setup Assistant配置机械臂

完成机器人模型创建之后，需要用Setup Assistant工具完成一些配置工作。Setup Assistant会根据用户导入的机器人模型生成SRDF文件，从而创建一个MoveIt！配置的功能包，完成机器人的配置、可视化和仿真工作。
（配置步骤省略，参考《ROS机器人开发实践》10.4节）
配置完成后，会生成了一个名为“sixr_robot_moveit_config"的功能包，它包含大部分MoveIt！启动所需要的配置文件和启动文件，以及包含一个简单的演示demo，可以用来测试配置是否成功，使用以下命令运行：

roslaunch my_sixr_robot_config demo.launch

在这个程序中可以进行随即目标规划、初始位置更新、碰撞检测等

(四） 用命令行测试MoveIt!

除了使用MoveIt！中的MotionPlanning测试机械臂功能之外，还可以用命令行测试MoceIt！（参考《ROS进阶实例》11.14节）
运行demo文件：

roslaunch my_sixr_robot_config demo.launch

在命令行里用moveit_commander_cmdline.py工具和MoveIt！进行交互。启动MoveIt！命令行界面：

rosrun moveit_commander moveit_commander_cmdline.py

所有MoveIt！里的命令都是针对规划组的，这些规划组定义在当前使用的机器人的配置文件里，选择arm组：

use arm

（1）将机械臂移动到随机关节角度：

go rand

（2）查看当前关节角度和末端执行器的位姿：

current

（3）修改目标关节角度
将当前配置记录到一个名为‘c’的变量里

record c

改变目标角度

c[1] = 0.2

将目标设置到修改后的配置：

goal = c

将机械臂移动到目标配置：

go goal

(4) 调用设置助手定义过的预设位姿

go home

（5） 查看更多解析器支持命令

help

(五）添加ArbotiX关节控制器

MoveIt！默认生成的demo中所使用的控制器功能有限，可以使用其他控制器插件实现驱动机器人模型的功能。ArbotiX功能包中提供了Joint Trajectory Action Controllers插件，可以用来驱动真实机器人的每个舵机关节，实现旋转运动。
（1） 添加YAML格式配置文件

roscd my_sixr_robot
mkdir config
cd config
gedit arm.yaml

在打开的arm.yaml文件中输入以下内容：

joints: {
  arm01_joint: {id: 1, neutral: 205, max_angle: 169.6, min_angle: -169.6, max_speed: 90},
  arm12_joint: {id: 2, max_angle: 134.6, min_angle: -134.6, max_speed: 90},
  arm23_joint: {id: 3, max_angle: 134.6, min_angle: -134.6, max_speed: 90},
  arm34_joint: {id: 4, max_angle: 134.6, min_angle: -134.6, max_speed: 90},
  arm45_joint: {id: 5, max_angle: 134.6, min_angle: -134.6, max_speed: 90},
  arm56_joint: {id: 6, max_angle: 360, min_angle: -360, max_speed: 90},
  right_finger_joint: {id: 7, max_speed: 90}
}
controllers: {
  arm_controllers: {type: follow_controller, joints:[arm01_joint, arm12_joint, arm23_joint, arm34_joint,arm45_joint,arm56_joint], action_name: arm_controller/follow_joint_trajectory, onboard: False},
  gripper_controllers: {type: follow_controller, joints:[right_finger_joint], action_name: gripper_controller/follow_joint_trajectory, onboard: False}
}

（2） 编写运行ArbotiX节点的.launch文件

roscd my_sixr_robot/launch
gedit fake_arm.launch

在打开的fake_arm.launch文件中输入以下内容：

<launch>
  <param name="/use_sim_time" value="false" />

  <!-- Load the URDF/Xacro model of our robot -->
  <arg name="urdf_file" default="$(find xacro)/xacro.py '$(find my_sixr_robot)/urdf/arm.xacro'" />

  <param name="robot_description" command="$(arg urdf_file)" />
    
  <node name="arbotix" pkg="arbotix_python" type="arbotix_driver" output="screen" clear_params="true">
      <rosparam file="$(find my_sixr_robot)/config/arm.yaml" command="load" />
      <param name="sim" value="true"/>
  </node>

  
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher">
      <param name="publish_frequency" type="double" value="20.0" />
  </node>


 <node name="rviz" pkg="rviz" type="rviz" args="-d $(find my_sixr_robot)/urdf.rviz" required="true" />
  
</launch>

（3）编写发布action的例程

roscd my_sixr_robot
mkdir scripts
cd scripts
gedit trajectory_demo.py

在打开的trajectory_demo.py文件中输入以下内容：

#! /usr/bin/env python
import rospy
import actionlib

from control_msgs.msg import FollowJointTrajectoryAction, FollowJointTrajectoryGoal
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint

class TrajectoryDemo():
  def __init__(self):
    rospy.init_node('trajectory_demo')

    reset = rospy.get_param('~reset', False)
  
    arm_joints= ['arm01_joint',
                 'arm12_joint',
                 'arm23_joint',
                 'arm34_joint',
                 'arm45_joint',
                 'arm56_joint']
    
    if reset:
      arm_goal = [0,0,0,0,0,0]
    else:
      arm_goal = [-0.3,-1.0,0.5,0.8,0.9,0]

    rospy.loginfo('Waiting for arm trajectory controller...')
    arm_client = actionlib.SimpleActionClient('arm_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
    arm_client.wait_for_server()
    rospy.loginfo('...connected.')

    arm_trajectory = JointTrajectory()
    arm_trajectory.joint_names = arm_joints
    arm_trajectory.points.append(JointTrajectoryPoint())
    arm_trajectory.points[0].positions = arm_goal
    arm_trajectory.points[0].velocities = [0.0 for i in arm_joints]
    arm_trajectory.points[0].accelerations = [0.0 for i in arm_joints]
    arm_trajectory.points[0].time_from_start = rospy.Duration(3.0)

    rospy.loginfo('Moving the arm to goal position...')

    arm_goal = FollowJointTrajectoryGoal()
    arm_goal.trajectory = arm_trajectory
    arm_goal.goal_time_tolerance = rospy.Duration(0.0)
    arm_client.send_goal(arm_goal)

    arm_client.wait_for_result(rospy.Duration(5.0))

    rospy.loginfo('...done')

if __name__ == '__main__':
    try:
      TrajectoryDemo()
    except rospy.ROSInterruptException:
      pass

（4）运行例程

roslaunch my_sixr_robot fake_arm.launch

按照之前的方法显示模型之后，新建终端

rosrun my_sixr_robot trajectory_demo.py _reset:=False

机器人将运动到指定位置
（如果显示找不到文件：

roscd my_sixr_robot/scripts
sudo chmod +x trajectory_demo.py

再重新运行.py程序即可）

rosrun my_sixr_robot trajectory_demo.py _reset:=True

机器人将回到初始位置

(六）配置MoveIt！关节控制器

roscd sixr_robot_moveit_config/config
gedit controllers.yaml

在打开的controllers.yaml文件中输入以下内容：

controller_list:
  - name: arm_controller
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    defalut: true
    joints:
      - arm01_joint
      - arm12_joint
      - arm23_joint
      - arm34_joint
      - arm45_joint
      - arm56_joint

  - name: gripper_controller
    action_ns: gripper_action
    type: GripperCommand
    defalut: true
    joints:
      - right_finger_joint

进入sixr_robot_moveit_config/launch文件夹，修改arm_moveit_controller_manager.launch.xml中内容如下：

  <!-- loads moveit_controller_manager on the parameter server which is taken as argument 
    if no argument is passed, moveit_simple_controller_manager will be set -->
  <arg name="moveit_controller_manager" default="moveit_simple_controller_manager/MoveItSimpleControllerManager" />
  <param name="moveit_controller_manager" value="$(arg moveit_controller_manager)"/>

  <!-- loads ros_controllers to the param server -->
  <rosparam file="$(find sixr_robot_moveit_config)/config/controllers.yaml"/>
</launch>

(七）关节空间规划

关节空间规划是以关节角度为控制量的机器人运动，机器人状态使用各轴位置来描述，在指定运动目标的机器人状态后，通过控制各轴运动来到达目标位置。
（1） 创建.launch文件启动所需要的各种节点

cd ~/catkin_ws/src
catkin_create_pkg sixr_robot_planning
cd sixr_robot_planning
mkdir launch
cd launch
gedit arn_planning.launch

在打开的arm_planning.launch文件中输入以下内容：

<launch>

  <param name="/use_sim_time" value="false" />

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

  <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find my_sixr_robot)/urdf/arm.xacro'" />

  <node name="arbotix" pkg="arbotix_python" type="arbotix_driver" output="screen">
    <rosparam file="$(find my_sixr_robot)/config/arm.yaml" command="load" />
    <param name="sim" value="true" />
  </node>

  <node name="gripper_controller" pkg="arbotix_controllers" type="gripper_controller">
    <rosparam>
      model: singlesided
      invert: false
      center: 0.0
      pad_width: 0.004
      finger_length: 0.08
      min_opening: 0.0
      max_openning: 0.6
      joint: right_finger_joint
    </rosparam>
  </node>

  <node pkg="robot_state_publisher" type="robot_state_publisher" name="rob_st_pub" />

  <include file="$(find sixr_robot_moveit_config)/launch/move_group.launch" />
 
  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find my_3Rrobot)/urdf.rviz" required="true" />

  </launch>

（2）编写关节空间规划程序

roscd sixr_robot_planning
mkdir scripts
cd scripts
gedit moveit_fk_demo.py

在打开的moveit_fk_demo.py文件中输入以下内容：

#! /usr/bin/env python
import rospy, sys
import moveit_commander
from control_msgs.msg import GripperCommand
 
class MoveItFkDemo:
    def __init__(self):
 
        moveit_commander.roscpp_initialize(sys.argv)
 
       
        rospy.init_node('moveit_fk_demo', anonymous=True)       
 
       
        arm = moveit_commander.MoveGroupCommander('arm')
        gripper = moveit_commander.MoveGroupCommander('gripper')
        
        arm.set_goal_joint_tolerance(0.001)
        gripper.set_goal_joint_tolerance(0.01)
        
        arm.set_max_acceleration_scaling_factor(0.5)
        arm.set_max_velocity_scaling_factor(0.5)
        gripper.set_max_acceleration_scaling_factor(0.5)
        gripper.set_max_velocity_scaling_factor(0.5)
        
        gripper.set_joint_value_target([0])
        gripper.go()  
        rospy.sleep(1)

        arm.set_named_target('home')
        arm.go()  
        rospy.sleep(2)

        joint_positions = [-1, -1, -1, -1,-1,1]
        arm.set_joint_value_target(joint_positions)  
        arm.go()   
        rospy.sleep(1)
         
        gripper.set_joint_value_target([0.02])
        gripper.go()  
        rospy.sleep(1)  
        
        moveit_commander.roscpp_shutdown()
        moveit_commander.os._exit(0)
 
if __name__ == "__main__":
    try:
        MoveItFkDemo()
    except rospy.ROSInterruptException:
        pass

然后进入sixr_robot_moveit_config/config文件夹，打开opml_planning.yaml文件，修改以下两处内容，将None改成RRTConnect


最后使用如下命令实现机械臂关节空间下的运动测试：

roslaunch sixr_robot_planning arm_planning.launch
rosrun sixr_robot_planning moveit_fk_demo.py

运行成功之后，在rviz的界面中可以看到机械臂夹爪首先完成了闭合动作，然后机械臂运动到指定位姿。

(八）工作空间规划

roscd sixr_robot_planning
mkdir scripts
cd scripts
gedit moveit_ik_demo.py

在打开的gedit move_ik_demo.py文件中输入以下内容：

#! /usr/bin/env python
import rospy, sys
import moveit_commander
from moveit_msgs.msg import RobotTrajectory
from trajectory_msgs.msg import JointTrajectoryPoint

from geometry_msgs.msg import PoseStamped, Pose
from tf.transformations import euler_from_quaternion, quaternion_from_euler
 
class MoveItIkDemo:
    def __init__(self):
 
        moveit_commander.roscpp_initialize(sys.argv)
 
       
        rospy.init_node('moveit_ik_demo')       
 
       
        arm = moveit_commander.MoveGroupCommander('arm')

        end_effector_link = arm.get_end_effector_link()

        reference_frame = 'base_link'
        arm.set_pose_reference_frame(reference_frame)

        arm.allow_replanning(True)

        arm.set_goal_position_tolerance(0.01)
        arm.set_goal_orientation_tolerance(0.05)

        arm.set_named_target('home')
        arm.go()  
        rospy.sleep(2)

        target_pose = PoseStamped()
        target_pose.header.frame_id = reference_frame
        target_pose.header.stamp = rospy.Time.now()
        target_pose.pose.position.x = 0.68
        target_pose.pose.position.y = 2.07
        target_pose.pose.position.z = 7.06
        target_pose.pose.orientation.x = -0.56 
        target_pose.pose.orientation.y = -0.198
        target_pose.pose.orientation.z = -0.460
        target_pose.pose.orientation.w = 0.66

        arm.set_start_state_to_current_state()    
         
        arm.set_pose_target(target_pose, end_effector_link)

        traj = arm.plan()  
        
        arm.execute(traj)
        rospy.sleep(1)

        arm.shift_pose_target(1, 1, end_effector_link)
        arm.go()
        rospy.sleep(1)

        arm.shift_pose_target(5, 0.55, end_effector_link)
        arm.go()
        rospy.sleep(1)

        arm.set_named_target('home')
        arm.go()          
 
 
if __name__ == "__main__":
    MoveItIkDemo()

使用如下命令实现机械臂工作空间下的运动测试：

roslaunch sixr_robot_planning arm_planning.launch
rosrun sixr_robot_planning moveit_ik_demo.py

运行成功之后，在rviz的界面中可以看到机械臂按设置运动

(九）笛卡尔运动规划

roscd sixr_robot_planning
mkdir scripts
cd scripts
gedit moveit_cartesian_demo.py

在打开的gedit move_catesian_demo.py文件中输入以下内容：

#! /usr/bin/env python
import rospy, sys
import moveit_commander
from moveit_commander import MoveGroupCommander
from geometry_msgs.msg import Pose
from copy import deepcopy

class MoveItCartesianDemo:
    def __init__(self):
        moveit_commander.roscpp_initialize(sys.argv)
        rospy.init_node('moveit_cartesian_demo', anonymous=True)
        cartesian = rospy.get_param('~cartesian', True)
        arm = MoveGroupCommander('arm')
        arm.allow_replanning(True)
        arm.set_pose_reference_frame('base_link')
        arm.set_goal_position_tolerance(0.01)
        arm.set_goal_orientation_tolerance(0.1)
        
        end_effector_link = arm.get_end_effector_link()

        arm.set_named_target('foward')
        arm.go()

        start_pose = arm.get_current_pose(end_effector_link).pose
    
        waypoints = []
 
        if cartesian:
            waypoints.append(start_pose)

        wpose = deepcopy(start_pose)
        wpose.position.x +=0.5
        wpose.position.y +=0.5
        wpose.position.z -=0.5

        if cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            arm.set_pose_target(wpose)
            arm.go()
            rospy.sleep(1)

        wpose.position.x += 0.5
        wpose.position.y += 0.5
        wpose.position.z -= 1
   
        if cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            arm.set_pose_target(wpose)
            arm.go()
            rospy.sleep(1)

        if cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            arm.set_pose_target(start_pose)
            arm.go()
            rospy.sleep(1)
  
        if cartesian:
            fraction = 0.0
            maxtries = 100
            attempts = 0
    
            arm.set_start_state_to_current_state()

            while fraction <1.0 and attempts < maxtries:
                (plan, fraction) = arm.compute_cartesian_path(
                                        waypoints,
                                        0.01,
                                        0.0,
                                        True)
                attempts += 1
                if attempts == 0:
                    rospy.loginfo("Still trying after " + str(attempts) + " str(attempts) + "" attempts ...")

            if fraction == 1.0:
                rospy.loginfo("Path computed successfully. Moving the arm.")
                arm.execute(plan)
                rospy.loginfo("Path execution complete.")

            else:
                rospy.loginfo("Path planning failed with only " + str(fraction) + "success after " + str(maxtries) + " attempts.")

        arm.set_named_target('home')
        arm.go()
        rospy.sleep(1)

        moveit_commander.roscpp_shutdown()
        moveit_commander.os._exit(0)

if __name__ == "__main__":
    try:
        MoveItCartesianDemo()
    except rospy.ROSInterruptException:
        pass
           

使用如下命令实现机械臂工作空间下的运动测试：

roslaunch sixr_robot_planning arm_planning.launch
rosrun sixr_robot_planning moveit_cartesian_demo.py

运行成功之后，在rviz的界面中可以看到机械臂按设置运动