Chapter5-MoveIt机械臂控制

Yann 4/27/2019, ros

# What is MoveIt！

# MoveIt！系统架构

# move_group

核心节点，可以综合其他独立的功能组件为用户提供ROS中的动作指令和服务

本身不具备丰富的功能，主要完成各功能包、插件的集成

通过消息或服务的方式接收服务器发布的点云信息、关节状态信息、以及机器人的TF坐标变换
需要ROS参数服务器提供机器人的运动学参数

# motion_planner

# 规划场景

为机器人创建一个工作环境，包括外界环境中的桌面、工件等物体

# 运动学求解器

运动学插件允许开发者灵活选择多种可供使用的运动学求解器

默认为从Orocos项目中移植过来的KDL
可以选择自己的运动学求解器

# 碰撞检测

使用CollisionWorld对象进行碰撞检测
采用FCL(Flexible Collision Library)功能包实现
为减少计算量，在MoveIt！Setup Assistant中设置免检冲突矩阵(ACM)进行优化

# 如何使用MoveIt！

# 创建机械臂模型

报错

解决Could not find a package configuration file provided by “manipulation_msgs“

https://blog.csdn.net/weixin_43738246/article/details/115751901?ops_request_misc=%257B%2522request%255Fid%2522%253A%2522162659565416780262583241%2522%252C%2522scm%2522%253A%252220140713.130102334.pc%255Fall.%2522%257D&request_id=162659565416780262583241&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2~all~first_rank_v2~rank_v29-1-115751901.first_rank_v2_pc_rank_v29&utm_term=+Could+not+find+a+package+configuration+file+provided+by+%22manipulation_msgs%22&spm=1018.2226.3001.4187

ros编译时报Invoking "make -j4 -l4" failed

https://www.cnblogs.com/dayspring/p/13521510.html

‘file‘ object has no attribute ‘push

https://blog.csdn.net/CH_monsy/article/details/108129073

ModuleNotFoundError: No module named 'em'

https://blog.csdn.net/Chen1189/article/details/107327167

pip3 install empy

# `arm.xaro`

宏定义

颜色、机器人尺寸、惯性矩阵

<!-- Defining the colors used in this robot -->
    <material name="Black">
        <color rgba="0 0 0 1"/>
    </material>

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

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

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

    <!-- Constants -->
    <xacro:property name="M_PI" value="3.14159"/>

    <!-- link1 properties -->
    <xacro:property name="link1_width" value="0.03" />
    <xacro:property name="link1_len" value="0.10" />

    <!-- link2 properties -->
    <xacro:property name="link2_width" value="0.03" />
    <xacro:property name="link2_len" value="0.14" />

    <!-- link3 properties -->
    <xacro:property name="link3_width" value="0.03" />
    <xacro:property name="link3_len" value="0.22" />

    <!-- link4 properties -->
    <xacro:property name="link4_width" value="0.025" />
    <xacro:property name="link4_len" value="0.06" />

    <!-- link5 properties -->
    <xacro:property name="link5_width" value="0.03" />
    <xacro:property name="link5_len" value="0.06" />

    <!-- link6 properties -->
    <xacro:property name="link6_width" value="0.04" />
    <xacro:property name="link6_len" value="0.02" />

    <!-- Left gripper -->
    <xacro:property name="left_gripper_len" value="0.08" />
    <xacro:property name="left_gripper_width" value="0.01" />
    <xacro:property name="left_gripper_height" value="0.01" />

    <!-- Right gripper -->
    <xacro:property name="right_gripper_len" value="0.08" />
    <xacro:property name="right_gripper_width" value="0.01" />
    <xacro:property name="right_gripper_height" value="0.01" />

    <!-- Gripper frame -->
    <xacro:property name="grasp_frame_radius" value="0.001" />

    <!-- Inertial matrix -->
    <xacro:macro name="inertial_matrix" params="mass">
        <inertial>
            <mass value="${mass}" />
            <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="0.5" iyz="0.0" izz="1.0" />
        </inertial>
    </xacro:macro>

创建六轴机械臂模型

    <!-- ///////////////////////////////////////   bottom_joint   ////////////////////////////////////////// -->
    <joint name="bottom_joint" type="fixed">
        <origin xyz="0 0 0" rpy="0 0 0" />
        <parent link="base_link"/>
        <child link="bottom_link"/>
    </joint>
    <link name="bottom_link">
        <visual>
              <origin xyz=" 0 0 -0.02"  rpy="0 0 0"/>
                  <geometry>
                       <box size="1 1 0.02" />
                  </geometry>
              <material name="Brown" />
        </visual>
        <collision>
            <origin xyz=" 0 0 -0.02"  rpy="0 0 0"/>
            <geometry>
                <box size="1 1 0.02" />
            </geometry>
        </collision>
        <xacro:inertial_matrix mass="500"/>
    </link>

    <!-- /////////////////////////////////////   BASE LINK    ////////////////////////////////////////////// -->
    <link name="base_link">
        <visual>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <geometry>
                <box size="0.1 0.1 0.04" />
            </geometry>
            <material name="White" />
        </visual>
        <collision>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <geometry>
                <box size="0.1 0.1 0.04" />
            </geometry>
        </collision>
        <xacro:inertial_matrix mass="1"/>
    </link>

    <joint name="joint1" type="revolute">
        <parent link="base_link"/>
        <child link="link1"/>
        <origin xyz="0 0 0.02" rpy="0 ${M_PI/2} 0" />
        <axis xyz="-1 0 0" />
        <limit effort="300" velocity="1" lower="-2.96" upper="2.96"/>
        <dynamics damping="50" friction="1"/>
    </joint>

    <!-- /////////////////////////////////////   LINK1  ////////////////////////////////////////////// -->
    <link name="link1" >
        <visual>
            <origin xyz="-${link1_len/2} 0 0" rpy="0 ${M_PI/2} 0" />
            <geometry>
                <cylinder radius="${link1_width}" length="${link1_len}"/>
            </geometry>
            <material name="Blue" />
        </visual>
        <collision>
            <origin xyz="-${link1_len/2} 0 0" rpy="0 ${M_PI/2} 0" />
            <geometry>
                <cylinder radius="${link1_width}" length="${link1_len}"/>
            </geometry>
        </collision>
        <xacro:inertial_matrix mass="1"/>
    </link>

    <joint name="joint2" type="revolute">
        <parent link="link1"/>
        <child link="link2"/>
        <origin xyz="-${link1_len} 0 0.0" rpy="-${M_PI/2} 0 ${M_PI/2}" />
        <axis xyz="1 0 0" />
        <limit effort="300" velocity="1" lower="-2.35" upper="2.35" />
        <dynamics damping="50" friction="1"/>
    </joint>

    <!-- ///////////////////////////////////////   LINK2  ////////////////////////////////////////////// -->
    <link name="link2" >
        <visual>
            <origin xyz="0 0 ${link2_len/2}" rpy="0 0 0" />
            <geometry>
                <cylinder radius="${link2_width}" length="${link2_len}"/>
            </geometry>
            <material name="White" />
        </visual>

        <collision>
            <origin xyz="0 0 ${link2_len/2}" rpy="0 0 0" />
            <geometry>
                <cylinder radius="${link2_width}" length="${link2_len}"/>
            </geometry>
        </collision>
        <xacro:inertial_matrix mass="1"/>
    </link>

    <joint name="joint3" type="revolute">
        <parent link="link2"/>
        <child link="link3"/>
        <origin xyz="0 0 ${link2_len}" rpy="0 ${M_PI} 0" />
        <axis xyz="-1 0 0" />
        <limit effort="300" velocity="1" lower="-2.62" upper="2.62" />
        <dynamics damping="50" friction="1"/>
    </joint>

    <!-- /////////////////////////////////   LINK3  ///////////////////////////////////////////////////// -->
    <link name="link3" >
        <visual>
            <origin xyz="0 0 -${link3_len/2}" rpy="0 0 0" />
            <geometry>
                <cylinder radius="${link3_width}" length="${link3_len}"/>
            </geometry>
            <material name="Blue" />
        </visual>
        <collision>
            <origin xyz="0 0 -${link3_len/2}" rpy="0 0 0" />
            <geometry>
                <cylinder radius="${link3_width}" length="${link3_len}"/>
            </geometry>
        </collision>
        <xacro:inertial_matrix mass="1"/>
    </link>

    <joint name="joint4" type="revolute">
        <parent link="link3"/>
        <child link="link4"/>
        <origin xyz="0.0 0.0 -${link3_len}" rpy="0 ${M_PI/2} ${M_PI}" />
        <axis xyz="1 0 0" />
        <limit effort="300" velocity="1" lower="-2.62" upper="2.62" />
        <dynamics damping="50" friction="1"/>
    </joint>

    <!-- ///////////////////////////////////   LINK4  //////////////////////////////////////////////// -->
    <link name="link4" >
        <visual>
            <origin xyz="${link4_len/2} 0 0" rpy="0 ${M_PI/2} 0" />
            <geometry>
                <cylinder radius="${link4_width}" length="${link4_len}"/>
            </geometry>
            <material name="Black" />
        </visual>
        <collision>
            <origin xyz="${link4_len/2} 0 0" rpy="0 ${M_PI/2} 0" />
            <geometry>
                <cylinder radius="${link4_width}" length="${link4_len}"/>
            </geometry>
        </collision>
        <xacro:inertial_matrix mass="1"/>
    </link>

    <joint name="joint5" type="revolute">
        <parent link="link4"/>
        <child link="link5"/>
        <origin xyz="${link4_len} 0.0 0.0" rpy="0 ${M_PI/2} 0" />
        <axis xyz="1 0 0" />
        <limit effort="300" velocity="1" lower="-2.62" upper="2.62" />
        <dynamics damping="50" friction="1"/>
    </joint>

    <!-- //////////////////////////////////   LINK5  ///////////////////////////////////////////////// -->
    <link name="link5">
        <visual>
            <origin xyz="0 0 ${link4_len/2}" rpy="0 0 0" />
            <geometry>
                <cylinder radius="${link5_width}" length="${link5_len}"/>
            </geometry>
            <material name="White" />
        </visual>
        <collision>
            <origin xyz="0 0 ${link4_len/2} " rpy="0 0 0" />
            <geometry>
                <cylinder radius="${link5_width}" length="${link5_len}"/>
            </geometry>
        </collision>
        <xacro:inertial_matrix mass="1"/>
    </link>

    <joint name="joint6" type="revolute">
        <parent link="link5"/>
        <child link="link6"/>
        <origin xyz="0 0 ${link4_len}" rpy="${1.5*M_PI} -${M_PI/2} 0" />
        <axis xyz="1 0 0" />
        <limit effort="300" velocity="1" lower="-6.28" upper="6.28" />
        <dynamics damping="50" friction="1"/>
    </joint>

    <!-- ////////////////////////////////   LINK6  ///////////////////////////////////////////////// -->
    <link name="link6">
        <visual>
            <origin xyz="${link6_len/2} 0 0 " rpy="0 ${M_PI/2} 0" />
            <geometry>
                <cylinder radius="${link6_width}" length="${link6_len}"/>
            </geometry>
            <material name="Blue" />
        </visual>
        <collision>
            <origin xyz="${link6_len/2} 0 0" rpy="0 ${M_PI/2} 0" />
            <geometry>
                <cylinder radius="${link6_width}" length="${link6_len}"/>
            </geometry>
        </collision>
        <xacro:inertial_matrix mass="1"/>
    </link>

    <joint name="finger_joint1" type="prismatic">
        <parent link="link6"/>
        <child link="gripper_finger_link1"/>
        <origin xyz="0.0 0 0" />
        <axis xyz="0 1 0" />
        <limit effort="100" lower="0" upper="0.06" velocity="1.0"/>
        <dynamics damping="50" friction="1"/>
    </joint>

    <!-- //////////////////////////////////////   gripper   ////////////////////////////////////////////// -->
    <!-- LEFT GRIPPER AFT LINK -->
    <link name="gripper_finger_link1">
        <visual>
            <origin xyz="0.04 -0.03 0"/>
            <geometry>
                <box size="${left_gripper_len} ${left_gripper_width} ${left_gripper_height}" />
            </geometry>
            <material name="White" />
        </visual>
        <xacro:inertial_matrix mass="1"/>
    </link>

    <joint name="finger_joint2" type="fixed">
        <parent link="link6"/>
        <child link="gripper_finger_link2"/>
        <origin xyz="0.0 0 0" />
    </joint>

    <!-- RIGHT GRIPPER AFT LINK -->
    <link name="gripper_finger_link2">
        <visual>
            <origin xyz="0.04 0.03 0"/>
            <geometry>
                <box size="${right_gripper_len} ${right_gripper_width} ${right_gripper_height}" />
            </geometry>
            <material name="White" />
        </visual>
        <xacro:inertial_matrix mass="1"/>
    </link>

    <!-- Grasping frame -->
    <link name="grasping_frame"/>

    <joint name="grasping_frame_joint" type="fixed">
      <parent link="link6"/>
      <child link="grasping_frame"/>
      <origin xyz="0.08 0 0" rpy="0 0 0"/>
    </joint>

加入gazebo属性

   <!-- /////////////////////////////////   Gazebo   ////////////////////////////////////// -->
    <gazebo reference="bottom_link">
        <material>Gazebo/White</material>
    </gazebo>
    <gazebo reference="base_link">
        <material>Gazebo/White</material>
    </gazebo>
    <gazebo reference="link1">
        <material>Gazebo/Blue</material>
    </gazebo>
    <gazebo reference="link2">
        <material>Gazebo/White</material>
    </gazebo>
    <gazebo reference="link3">
        <material>Gazebo/Blue</material>
    </gazebo>
    <gazebo reference="link4">
        <material>Gazebo/Black</material>
    </gazebo>
    <gazebo reference="link5">
        <material>Gazebo/White</material>
    </gazebo>
    <gazebo reference="link6">
        <material>Gazebo/Blue</material>
    </gazebo>
    <gazebo reference="gripper_finger_link1">
        <material>Gazebo/White</material>
    </gazebo>
    <gazebo reference="gripper_finger_link2">
        <material>Gazebo/White</material>
    </gazebo>
    
    <!-- Transmissions for ROS Control -->
    <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="joint1"/>
    <xacro:transmission_block joint_name="joint2"/>
    <xacro:transmission_block joint_name="joint3"/>
    <xacro:transmission_block joint_name="joint4"/>
    <xacro:transmission_block joint_name="joint5"/>
    <xacro:transmission_block joint_name="joint6"/>
    <xacro:transmission_block joint_name="finger_joint1"/>
    <!-- ros_control plugin -->
    <gazebo>
        <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
            <robotNamespace>/arm</robotNamespace>
        </plugin>
    </gazebo>

显示机器人模型view_arm.launch

<launch>
    <arg name="model" />
    <!-- 加载机器人模型参数 -->
    <param name="robot_description" command="$(find xacro)/xacro --inorder $(find marm_description)/urdf/arm.xacro" />

    <!-- 设置GUI参数，显示关节控制插件 -->
    <param name="use_gui" value="true"/>

    <!-- 运行joint_state_publisher节点，发布机器人的关节状态  -->
    <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
    
    <!-- 运行robot_state_publisher节点，发布tf  -->
    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />

    <!-- 运行rviz可视化界面 -->
    <node name="rviz" pkg="rviz" type="rviz" args="-d $(find marm_description)/urdf.rviz" required="true" />
</launch>
roslaunch marm_description view_arm.launch

# 使用setup assistant配置机械臂

sudo apt-get install ros-melodic-moveit-*
roscore
rosrun moveit_setup_assistant moveit_setup_assistant

报错：libmoveit_kinematics_base.so.1.0.8: cannot open shared object file: No such file or directory

https://blog.csdn.net/fightingforcv/article/details/51850123?ops_request_misc=&request_id=&biz_id=102&utm_term=error%20while%20loading%20shared%20lib&utm_medium=distribute.pc_search_result.none-task-blog-2~all~sobaiduweb~default-7-.nonecase&spm=1018.2226.3001.4187

# 启动MoveIt！

roslaunch marm_moveit_config demo.launch

# 配置文件

配置文件	作用
`arm.srdf`	Semantic Robot Description Fromat，机械臂参数、夹具参数、规划组、自定义位姿……
`fake_controllers.yaml`	虚拟控制器参数控制 MoveIt！机械臂的运动轨迹
`joint_limits.yaml`	设置机器人运动关节的速度限位，速度、加速度
`kinematics.yaml`	运动学求解器
`ompl_planning.yaml`	MoveIt！中默认使用的运动规划库

# `arm.srdf`

规划组

    <group name="arm">
        <chain base_link="base_link" tip_link="grasping_frame" />
    </group>
    <group name="gripper">
        <link name="gripper_finger_link1" />
        <link name="gripper_finger_link2" />
        <joint name="finger_joint1" />
        <joint name="finger_joint2" />
    </group>

自定义位姿

    <group_state name="home" group="arm">
        <joint name="joint1" value="0" />
        <joint name="joint2" value="0" />
        <joint name="joint3" value="0" />
        <joint name="joint4" value="0" />
        <joint name="joint5" value="0" />
        <joint name="joint6" value="0" />
    </group_state>
    <group_state name="forward" group="arm">
        <joint name="joint1" value="0" />
        <joint name="joint2" value="0.4132" />
        <joint name="joint3" value="0.7486" />
        <joint name="joint4" value="0.2015" />
        <joint name="joint5" value="-0.4031" />
        <joint name="joint6" value="0" />
    </group_state>

机械臂终端

<end_effector name="robot_gripper" parent_link="grasping_frame" group="gripper" />

碰撞矩阵

<disable_collisions link1="base_link" link2="bottom_link" reason="Adjacent" />
 <disable_collisions link1="base_link" link2="link1" reason="Adjacent" />
 <disable_collisions link1="bottom_link" link2="link1" reason="Never" />
 <disable_collisions link1="bottom_link" link2="link2" reason="Never" />
 <disable_collisions link1="gripper_finger_link1" link2="link2" reason="Never" />
 <disable_collisions link1="gripper_finger_link1" link2="link5" reason="Default" />
 <disable_collisions link1="gripper_finger_link1" link2="link6" reason="Adjacent" />
 <disable_collisions link1="gripper_finger_link2" link2="link2" reason="Never" />
 <disable_collisions link1="gripper_finger_link2" link2="link5" reason="Default" />
 <disable_collisions link1="gripper_finger_link2" link2="link6" reason="Adjacent" />
 <disable_collisions link1="link1" link2="link2" reason="Adjacent" />
 <disable_collisions link1="link1" link2="link4" reason="Never" />
 <disable_collisions link1="link1" link2="link5" reason="Never" />
 <disable_collisions link1="link2" link2="link3" reason="Adjacent" />
 <disable_collisions link1="link2" link2="link4" reason="Never" />
 <disable_collisions link1="link2" link2="link5" reason="Never" />
 <disable_collisions link1="link2" link2="link6" reason="Never" />
 <disable_collisions link1="link3" link2="link4" reason="Adjacent" />
 <disable_collisions link1="link4" link2="link5" reason="Adjacent" />
 <disable_collisions link1="link5" link2="link6" reason="Adjacent" />

fake_controllers.yaml
controller_list:
  - name: fake_arm_controller
    joints:
      - joint1
      - joint2
      - joint3
      - joint4
      - joint5
      - joint6
  - name: fake_gripper_controller
    joints:
      - finger_joint1
joint_limits.yaml
# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed
# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration]
# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits]
joint_limits:
  finger_joint1:
    has_velocity_limits: true
    max_velocity: 1
    has_acceleration_limits: false
    max_acceleration: 0
  joint1:
    has_velocity_limits: true
    max_velocity: 1
    has_acceleration_limits: false
    max_acceleration: 0
  joint2:
    has_velocity_limits: true
    max_velocity: 1
    has_acceleration_limits: false
    max_acceleration: 0
  joint3:
    has_velocity_limits: true
    max_velocity: 1
    has_acceleration_limits: false
    max_acceleration: 0
  joint4:
    has_velocity_limits: true
    max_velocity: 1
    has_acceleration_limits: false
    max_acceleration: 0
  joint5:
    has_velocity_limits: true
    max_velocity: 1
    has_acceleration_limits: false
    max_acceleration: 0
  joint6:
    has_velocity_limits: true
    max_velocity: 1
    has_acceleration_limits: false
    max_acceleration: 0
kinematics.yaml
arm:
  kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
  kinematics_solver_search_resolution: 0.005
  kinematics_solver_timeout: 0.05
  kinematics_solver_attempts: 3

# MoveIt!编程基础

# 关节空间规划例程

roslaunch marm_moveit_config demo.launch
rosrun marm_planning moveit_fk.py

# 工作空间规划例程

roslaunch marm_moveit_config demo.launch
rosrun marm_planning moveit_ik.py

![图片](data:image/svg+xml,%3C%3Fxml version='1.0' encoding='UTF-8'%3F%3E%3Csvg width='1px' height='1px' viewBox='0 0 1 1' version='1.1' xmlns='http://www.w3.org/2000/svg' xmlns:xlink='http://www.w3.org/1999/xlink'%3E%3Ctitle%3E%3C/title%3E%3Cg stroke='none' stroke-width='1' fill='none' fill-rule='evenodd' fill-opacity='0'%3E%3Cg transform='translate(-249.000000, -126.000000)' fill='%23FFFFFF'%3E%3Crect x='249' y='126' width='1' height='1'%3E%3C/rect%3E%3C/g%3E%3C/g%3E%3C/svg%3E)

# 笛卡尔路径规划例程

roslaunch marm_moveit_config demo.launch
rosrun marm_planning moveit_cartesian_demo.py _cartesian:=True
rosrun marm_planning moveit_cartesian_demo.py _cartesian:=False

# 自主避障规划例程

roslaunch marm_moveit_config demo.launch
rosrun marm_planning moveit_obstacles_demo.py

← Chapter4-常用组件-TF Chapter6-Ubuntu16.04+Kinetic环境安装 →

Yann