5. Moveit moveit编程与空间规划

1.添加配置文件——controllers.yaml

关联了arm_controller控制器名称，follow_joint_trajectory命名空间，FollowJointTrajectory的action类型（规划轨迹是以action发布的），是否为默认控制器，与该规划组所包含的关节。

controller_list:
  - name: arm_controller
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    default: true
    joints:
      - joint1
      - joint2
      - joint3
      - joint4
      - joint5
      - joint6
      
  - name: gripper_controller
    action_ns: gripper_action
    type: GripperCommand
    default: true
    joints:
      - finger_joint1

注意！这个是后加的！这个个控制器是对标ArbotiX的，在配置助手创建config文件时，并不知道ArbotiX这个控制器，用的是moveit中的虚拟控制器，其中虚拟控制器如下，注意区分！细节在书中P323.

controller_list:
  - name: fake_arm_controller
    type: $(arg fake_execution_type)
    joints:
      - joint1
      - joint2
      - joint3
      - joint4
      - joint5
      - joint6
  - name: fake_gripper_controller
    type: $(arg fake_execution_type)
    joints:
      - finger_joint1
initial:  # Define initial robot poses.
  - group: arm
    pose: home

2.修改启动文件的launch文件

因为默认的是加载ros_controllers.yaml——这个是moveit配置助手的默认生成的，而我要用的是关于ArbotiX的控制器，故需要进行修改，将ros_去掉。修改后如下。同时注意<arg name=" " default=" ">  $(arg moveit_controller_manager)的方式。

<launch>
  <!-- Define the controller manager plugin to use for trajectory execution -->
  <param name="moveit_controller_manager" value="moveit_simple_controller_manager/MoveItSimpleControllerManager" />

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

3.关节空间规划

其launch文件

<launch>
    <!-- 不使用仿真时间 -->
    <param name="/use_sim_time" value="false" />

    <!-- 启动 arbotix driver-->
    <arg name="sim" default="true" />

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

    <node name="arbotix" pkg="arbotix_python" type="arbotix_driver" output="screen">
        <rosparam file="$(find marm_description)/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_opening: 0.06
            joint: finger_joint1
        </rosparam>
    </node>

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

    <include file="$(find marm_moveit_config)/launch/move_group.launch" />

    <!-- 启动rviz可视化界面 -->
    <node name="rviz" pkg="rviz" type="rviz" args="-d $(find marm_planning)/config/pick_and_place.rviz" required="true" />

</launch>

与之前的没有加入moveit控制器的launch文件区别

<launch>
    <!-- 不使用仿真时间 -->
    <param name="/use_sim_time" value="false" />

    <!-- 启动arbotix driver-->
    <arg name="sim" default="true" />

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

    <node name="arbotix" pkg="arbotix_python" type="arbotix_driver" output="screen">
        <rosparam file="$(find marm_description)/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_opening: 0.06
            joint: finger_joint1
        </rosparam>
    </node>

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

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

</launch>

区别：

（1）rviz节点不同，之前是urdf.rviz,目前是pick_and_place.rviz.

（2）加入了move_group.launch,其中书中所说，此处是为了加载之前修改过控制器的插件，即Moveit控制器。

4.开始运动测试

（1）正运动学

roslaunch marm_planning arm_planning.launch
roscore
rosrun marm_planning moveit_fk_demo.py

 marm_planning moveit_fk_demo.py

#!/usr/bin/env python
# -*- coding: utf-8 -*-

import rospy, sys
import moveit_commander
from control_msgs.msg import GripperCommand

class MoveItFkDemo:
    def __init__(self):
        # 初始化move_group的API
        moveit_commander.roscpp_initialize(sys.argv)

        # 初始化ROS节点
        rospy.init_node('moveit_fk_demo', anonymous=True)
 
        # 初始化需要使用move group控制的机械臂中的arm group
        arm = moveit_commander.MoveGroupCommander('arm')
        
        # 初始化需要使用move group控制的机械臂中的gripper group
        gripper = moveit_commander.MoveGroupCommander('gripper')
        
        # 设置机械臂和夹爪的允许误差值
        arm.set_goal_joint_tolerance(0.1)
        gripper.set_goal_joint_tolerance(0.001)
        
        # 控制机械臂先回到初始化位置
        joint_positions = [0.1145, 0.4544, 0.4777, 0.2171, -0.5156, 0.3817]
        # arm.set_named_target('home')
        arm.set_joint_value_target(joint_positions)
        arm.go()
        rospy.sleep(2)



        # 控制机械臂先回到初始化位置
        joint_positions = [0.2545, 0.5644, 0.5877, 0.3171, -0.6156, 0.4817]
        # arm.set_named_target('home')
        arm.set_joint_value_target(joint_positions)
        arm.go()
        rospy.sleep(2)

        # 控制机械臂先回到初始化位置
        joint_positions = [0.3545, 0.6644, 0.6877, 0.4171, -0.7156, 0.5817]
        # arm.set_named_target('home')
        arm.set_joint_value_target(joint_positions)
        arm.go()
        rospy.sleep(2)


        arm.set_named_target('forward')
        arm.go()
        rospy.sleep(5)
          
        # 设置夹爪的目标位置，并控制夹爪运动
        gripper.set_joint_value_target([0.01])
        gripper.go()
        rospy.sleep(2)
         

         
        # 控制机械臂完成运动
        # arm.go()
        # rospy.sleep(5)
        
        # 关闭并退出moveit
        moveit_commander.roscpp_shutdown()
        moveit_commander.os._exit(0)

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

注意正运动学幅度不能太大，特别容易规划不出来！

会出现报错，规划失败。ABORTED: No motion plan found. No execution attempted.

（2）逆运动学

rosrun marm_planning moveit_ik_demo.py

运行上述python脚本。

 逆运动学没发现规划出现问题。相比于正确运动学要更加稳定。

#!/usr/bin/env python
# -*- coding: utf-8 -*-

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):
        # 初始化move_group的API
        moveit_commander.roscpp_initialize(sys.argv)
        
        # 初始化ROS节点
        rospy.init_node('moveit_ik_demo')
                
        # 初始化需要使用move group控制的机械臂中的arm group
        arm = moveit_commander.MoveGroupCommander('arm')
                
        # 获取终端link的名称
        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)
               
        # 设置机械臂工作空间中的目标位姿，位置使用x、y、z坐标描述，
        # 姿态使用四元数描述，基于base_link坐标系
        target_pose = PoseStamped()
        target_pose.header.frame_id = reference_frame
        target_pose.header.stamp = rospy.Time.now()     
        target_pose.pose.position.x = 0.191995
        target_pose.pose.position.y = 0.213868
        target_pose.pose.position.z = 0.520436
        target_pose.pose.orientation.x = 0.911822
        target_pose.pose.orientation.y = -0.0269758
        target_pose.pose.orientation.z = 0.285694
        target_pose.pose.orientation.w = -0.293653
        
        # 设置机器臂当前的状态作为运动初始状态
        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)
         
        # 控制机械臂终端向右移动5cm
        arm.shift_pose_target(1, -0.05, end_effector_link)
        arm.go()
        rospy.sleep(1)
  
        # 控制机械臂终端反向旋转90度
        arm.shift_pose_target(3, -1.57, end_effector_link)
        arm.go()
        rospy.sleep(1)
           
        # 控制机械臂回到初始化位置
        arm.set_named_target('home')
        arm.go()

        # 关闭并退出moveit
        moveit_commander.roscpp_shutdown()
        moveit_commander.os._exit(0)

if __name__ == "__main__":
    MoveItIkDemo()