[Morpheus Chair] MoveIt! for your Robot Arm | S4.Ep.3

MoveIt! for your Robot Arm

Written by Miguel Angel

19/07/2019

Following up our previous post about how to control a Robot Arm with ROS, today we are going to learn how to set up a MoveIt package for controlling the arm.

Remember that if you are new to ROS, we recommend you taking the ROS Basics in 5 Days (Python) or ROS Basics in 5 Days (C++) depending on which programming language you prefer.

What we will learn

In short, in this post we will learn the following:

  • How to create a basic MoveIt package for your own robot
  • Test that package moving and planning with moveit.

Special Thanks to Clarkson University and specially James Carrol and its team for lending us the physical Open Manipulator robot.

Getting the code

We are going to create the MoveIt package which uses some URDF files. In order to get the URDF files, please get the following ROSject: http://www.rosject.io/l/b368f5c/

You can open it by clicking on the Open button or you can download it by clicking on the download button, the ones pointed below by the red and green arrows respectively.

 

The ROSJect mentioned here basically contains the following git repositories on it:

Open the Notebook

When you open a ROSject, by default you have the Jupyter Notebook automatically open, but if that doesn’t happen, you can manually open it by clicking on Tools -> Jupyter Notebook as shown in the image below:

 

Now on the Jupyter Notebook window let’s click openmanipulator_morpheus_chair_notebooks, then click on Ep3_MoveIt!_First_Steps.ipynb to open it. That notebook contains the instructions we are going to follow in this post.

The URDF file we are going to use

If you opened or downloaded the ROSject you will see that we have a package called open_manipulator_support_description with the .xacro file called open_manipulator_support.urdf.xacro. If you opened the ROSject on n ROSDS (ROS Development Studio), the full path to that file is: ~/simulation_ws/src/open_manipulator_tc/open_manipulator_support_description/urdf

The content of open_manipulator_support.urdf.xacro is:

<?xml version="1.0"?>
<!-- Open_Manipulator Chain -->
<robot name="open_manipulator" xmlns:xacro="http://www.ros.org/wiki/xacro">

<!-- Import all Gazebo-customization elements, including Gazebo colors -->
  <xacro:include filename="$(find open_manipulator_support_description)/urdf/open_manipulator.gazebo.xacro"/>
<!-- Import Rviz colors -->
  <xacro:include filename="$(find open_manipulator_support_description)/urdf/materials.xacro" />
<!-- Import inertial Properties-->
  <xacro:include filename="$(find open_manipulator_support_description)/urdf/inertial_properties.xacro" />
<!-- Import inertial Properties-->
  <xacro:include filename="$(find open_manipulator_support_description)/urdf/mass_properties.xacro" />

 <!-- World -->
   <link name="world">
   </link>

<!-- World fixed joint -->
   <joint name="world_fixed" type="fixed">
     <parent link="world"/>
     <child link="base_link"/>
   </joint>

<!-- Base Link -->
  <link name="base_link">
	 <inertial>
      <mass value="${bl_mass}" />
      <origin xyz="${bl_cmx} ${bl_cmy} ${bl_cmz}"/>
      <inertia
        ixx="${bl_ixx}"
        ixy="${bl_ixy}"
        ixz="${bl_ixz}"
        iyy="${bl_iyy}"
        iyz="${bl_iyz}"
        izz="${bl_izz}" />
    </inertial>
	<visual>
		<geometry>
        	<mesh filename="package://open_manipulator_support_description/meshes/base_link.STL"/>
		</geometry>
		<material name="grey"/>
	</visual>
	<collision>
      <geometry>
        <mesh filename="package://open_manipulator_support_description/meshes/base_link.STL" />
      </geometry>
    </collision>
  </link>


<!-- Link 1 -->
  <link name="link1">
    <visual>
      <geometry>
        <mesh filename="package://open_manipulator_support_description/meshes/link1.STL"/>
      </geometry>
      <material name="grey"/>
    </visual>

    <collision>
      <geometry>
        <mesh filename="package://open_manipulator_support_description/meshes/link1.STL"/>
      </geometry>
    </collision>

	<inertial>
      <mass value="${l1_mass}" />
      <origin xyz="${l1_cmx} ${l1_cmy} ${l1_cmz}"/>
      <inertia
        ixx="${l1_ixx}"
        ixy="${l1_ixy}"
        ixz="${l1_ixz}"
        iyy="${l1_iyy}"
        iyz="${l1_iyz}"
        izz="${l1_izz}" />
    </inertial>
  </link>

<!-- Joint 1 -->
  <joint name="id_1" type="revolute">
    <parent link="base_link"/>
    <child link="link1"/>
    <origin xyz="0 0 0.0405"/>
    <axis xyz="0 0 1"/>
    <limit velocity="2.8" effort="3.1" lower="${-1.1*pi}" upper="${0.9*pi}" />
  </joint>

<!-- Transmission 1 -->
  <transmission name="tran1">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="id_1">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor1">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>

<!--  Link 2-->
  <link name="link2">

	<visual>
      <geometry>
        <mesh filename="package://open_manipulator_support_description/meshes/link2.STL"/>
      </geometry>
      <material name="grey"/>
    </visual>

    <collision>
      <geometry>
        <mesh filename="package://open_manipulator_support_description/meshes/link2.STL"/>
      </geometry>
    </collision>

    <inertial>
      <mass value="${l2_mass}" />
      <origin xyz="${l2_cmx} ${l2_cmy} ${l2_cmz}"/>
      <inertia
        ixx="${l2_ixx}"
        ixy="${l2_ixy}"
        ixz="${l2_ixz}"
        iyy="${l2_iyy}"
        iyz="${l2_iyz}"
        izz="${l2_izz}" />
    </inertial>
  </link>

<!--  Joint 2 -->
  <joint name="id_2" type="revolute">
    <parent link="link1"/>
    <child link="link2"/>
    <origin xyz="0 0 0.056"/>
    <axis xyz="1 0 0"/>
    <limit velocity="2.9" effort="9.9" lower="-2.0" upper="2.0" />
  </joint>

<!-- Transmission 2 -->
  <transmission name="tran2">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="id_2">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor2">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>

<!--  Link 3 -->
  <link name="link3">
    <visual>
      <geometry>
        <mesh filename="package://open_manipulator_support_description/meshes/link3.STL"/>
      </geometry>
      <material name="grey"/>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://open_manipulator_support_description/meshes/link3.STL"/>
      </geometry>
    </collision>

    <inertial>
      <mass value="${l3_mass}" />
      <origin xyz="${l3_cmx} ${l3_cmy} ${l3_cmz}"/>
      <inertia
        ixx="${l3_ixx}"
        ixy="${l3_ixy}"
        ixz="${l3_ixz}"
        iyy="${l3_iyy}"
        iyz="${l3_iyz}"
        izz="${l3_izz}" />
    </inertial>
  </link>

<!--  Joint 3 -->
  <joint name="id_3" type="revolute">
    <parent link="link2"/>
    <child link="link3"/>
    <origin xyz="0 0 .1935" />
    <axis xyz="1 0 0"/>
    <limit velocity="2.9" effort="9.9" lower="-2.5" upper="2.5" />
  </joint>

<!-- Transmission 3 -->
  <transmission name="tran3">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="id_3">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor3">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>

<!--  Link 4 -->
  <link name="link4">
    <visual>
      <geometry>
        <mesh filename="package://open_manipulator_support_description/meshes/link4.STL"/>
      </geometry>
      <material name="grey"/>
    </visual>

    <collision>
      <geometry>
        <mesh filename="package://open_manipulator_support_description/meshes/link4.STL"/>
      </geometry>
    </collision>

    <inertial>
      <mass value="${l4_mass}" />
      <origin xyz="${l4_cmx} ${l4_cmy} ${l4_cmz}"/>
      <inertia
        ixx="${l4_ixx}"
        ixy="${l4_ixy}"
        ixz="${l4_ixz}"
        iyy="${l4_iyy}"
        iyz="${l4_iyz}"
        izz="${l4_izz}" />
    </inertial>

  </link>

<!--  Joint 4-->
  <joint name="id_4" type="revolute">
    <parent link="link3"/>
    <child link="link4"/>
    <origin xyz="0 0 0.201"/>
    <axis xyz="0 0 1"/>
    <limit velocity="2.8" effort="3.1" lower="${-pi}" upper="${pi}" />
  </joint>

<!-- Transmission 4 -->
  <transmission name="tran4">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="id_4">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor4">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>

<!--  Link 5 -->
  <link name="link5">
    <visual>
      <geometry>
        <mesh filename="package://open_manipulator_support_description/meshes/link5.STL"/>
      </geometry>
      <material name="grey"/>
    </visual>

    <collision>
      <geometry>
        <mesh filename="package://open_manipulator_support_description/meshes/link5.STL"/>
      </geometry>
    </collision>

    <inertial>
      <mass value="${l5_mass}" />
      <origin xyz="${l5_cmx} ${l5_cmy} ${l5_cmz}"/>
      <inertia
        ixx="${l5_ixx}"
        ixy="${l5_ixy}"
        ixz="${l5_ixz}"
        iyy="${l5_iyy}"
        iyz="${l5_iyz}"
        izz="${l5_izz}" />
    </inertial>
  </link>

<!--  Joint 5-->
  <joint name="id_5" type="revolute">
    <parent link="link4"/>
    <child link="link5"/>
    <origin xyz="0 0 0.0405"/>
    <axis xyz="1 0 0"/>
    <limit velocity="2.8" effort="3.1" lower="${-pi/2}" upper="2.2" />
  </joint>

<!-- Transmission 5 -->
  <transmission name="tran5">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="id_5">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor5">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>


<!--  Link 6 -->
  <link name="link6">
    <inertial>
      <mass value="${l6_mass}" />
      <origin xyz="${l6_cmx} ${l6_cmy} ${l6_cmz}"/>
      <inertia
        ixx="${l6_ixx}"
        ixy="${l6_ixy}"
        ixz="${l6_ixz}"
        iyy="${l6_iyy}"
        iyz="${l6_iyz}"
        izz="${l6_izz}" />
    </inertial>
    <visual>
      <geometry>
        <mesh filename="package://open_manipulator_support_description/meshes/link6.STL" />
      </geometry>
      <material name="grey" />
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://open_manipulator_support_description/meshes/link6.STL" />
      </geometry>
    </collision>
  </link>

<!--  Joint 6 -->
  <joint name="id_6" type="revolute">
    <origin xyz="0 0 0.064"/>
    <parent link="link5" />
    <child link="link6" />
    <axis xyz="0 0 1" />
	<limit velocity="2.8" effort="3.1" lower="${-pi}" upper="${pi}" />
  </joint>

<!-- Transmission 6 -->
  <transmission name="tran6">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="id_6">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor6">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>

<!-- Link 7-->
   <link name="link7">
    <inertial>
      <mass value="${l7_mass}" />
      <origin xyz="${l7_cmx} ${l7_cmy} ${l7_cmz}"/>
      <inertia
        ixx="${l7_ixx}"
        ixy="${l7_ixy}"
        ixz="${l7_ixz}"
        iyy="${l7_iyy}"
        iyz="${l7_iyz}"
        izz="${l7_izz}" />
    </inertial>
    <visual>
      <geometry>
        <mesh
          filename="package://open_manipulator_support_description/meshes/link7.STL" />
      </geometry>
      <material name="grey" />
    </visual>
    <collision>
      <geometry>
        <mesh
          filename="package://open_manipulator_support_description/meshes/link7.STL" />
      </geometry>
    </collision>
  </link>

<!-- Joint 7 -->
	<joint
    name="id_7"
    type="revolute">
    <origin xyz="0 0 0.039" />
    <parent link="link6" />
    <child link="link7" />
    <axis xyz="0 0 1" />
    <!--
    <limit velocity="2.8" effort="3.1" lower="${-pi}" upper="${pi}" />
    -->
    <limit velocity="2.8" effort="3.1" lower="${-pi}" upper="-1.5" />
  </joint>

  <!-- Transmission 7 -->
  <transmission name="tran7">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="id_7">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor7">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>

  <!--  Gripper Left-->

<link name="gripper_l">
    <inertial>
      <mass value="${g_mass}" />
      <origin xyz="${g_cmx} ${g_cmy} ${g_cmz}"/>
      <inertia
        ixx="${g_ixx}"
        ixy="${g_ixy}"
        ixz="${g_ixz}"
        iyy="${g_iyy}"
        iyz="${g_iyz}"
        izz="${g_izz}" />
    </inertial>

    <visual>
      <geometry>
        <mesh
          filename="package://open_manipulator_support_description/meshes/gripper.STL" />
      </geometry>
      <material name="grey" />
    </visual>

   <collision>
      <geometry>
        <mesh
          filename="package://open_manipulator_support_description/meshes/gripper.STL" />
      </geometry>
    </collision>
  </link>

<!--Gripper Left Joint -->
  <joint name="joint8" type="prismatic">
    <origin xyz="-0.015 0 0.0595" rpy="0 0 ${pi}"/>
    <parent link="link6" />
    <child link="gripper_l" />
    <axis xyz="1 0 0" />
    <limit velocity="0.1" effort=".1" lower="-.033" upper="0"/>
    <mimic joint="id_7" multiplier="-0.01" offset="-0.0110133334" />
  </joint>


<!--  Gripper Right -->
   <link name="gripper_r">
    <inertial>
      <mass value="${g_mass}" />
      <origin xyz="${-g_cmx} ${g_cmy} ${g_cmz}"/>
      <inertia
        ixx="${g_ixx}"
        ixy="${g_ixy}"
        ixz="${g_ixz}"
        iyy="${g_iyy}"
        iyz="${g_iyz}"
        izz="${g_izz}" />
    </inertial>
        <visual>
      <geometry>
        <mesh filename="package://open_manipulator_support_description/meshes/gripper.STL" />
      </geometry>
      <material name="grey" />
    </visual>

    <collision>
      <geometry>
        <mesh filename="package://open_manipulator_support_description/meshes/gripper.STL" />
      </geometry>
    </collision>
  </link>

<!--Gripper Right Joint -->
   <joint name="joint9" type="prismatic">
    <origin xyz="0.015 0 0.0595" />
    <parent link="link6" />
    <child link="gripper_r" />
    <axis xyz="1 0 0" />
        <limit velocity="0.1" effort=".1" lower="0" upper=".033"/>
    <mimic joint="joint8" multiplier="1" offset="0" />
  </joint>

</robot>

Opening the MoveIt Setup Assistant

Now that we know which urdf/xacro file we are going to use, let’s create a MoveIt package. On the ROSject that we are using that package is already created for you and it’s available on the path /home/user/catkin_ws/src/openmanipulator_ep2_movit_config, but we are going to show how we created it.

In order to open the MoveIt Assistant, the program used to create our first MoveIt Package, we used the following commands:

cd ~/catkin_ws/src
roslaunch moveit_setup_assistant setup_assistant.launch

MoveIt Assistant is a graphical application, so, after running the command aforementioned, on ROSDS you have to open the Graphical Tools by clicking Tools -> Graphical Tools as shown below:

After clicking on that button we should see MoveIt Setup Assistant as in the image below:

If you want the Graphical Tools to be opened in a different tab of the web browser, you can simply click on the Detach button as shown below:

Creating the MoveIt package from scratch

With the MoveIt Assistant open, let’s click on the Create New MoveIt Configuration Package button as shown in the image below:

Now we need to select our URDF file. For that, let’s click on the Browse button under the section Load a URDF or Collada Robot Model and select our URDF file mentioned earlier, which is on the path:

~/simulation_ws/src/open_manipulator_tc/open_manipulator_support_description/urdf/open_manipulator_support.urdf.xacro

then let’s click Load Files as in the image below.

Now you should have the URDF loaded. Make sure you can see it properly as in the right side of the image below:

Generate Collision Matrix

The first thing that is vital for a robot arm when it moves, is to NOT HIT ITSELF. Which seems dumb but, it is really a common way to break a thousand-euros robot arm if it doesn’t have the correct safety features like peak torque detection or some kind of external perception.

We have to generate what is called Self-Collision Matrix. To do it we use the Self-Collision Matrix Generator. We need to generate this matrix because:

  • Doing this we detect which links will collide with each other when moving.
  • We will detect also the links which will never collide with each other, which then we can remove the auto collisions calculations for them, lowering the burden in the processing.
  • Detect which links will be always in collision and therefore we suppose that its normal and therefore we also disable the calculations.
  • Disable also the links adjacent in the kinematic chain which obviously we will disable also their auto collisions.

If you look on the left side image above, we are at the Start section. Let’s click on the Self-Collisions button that appears on the left and then click Generate Collision Matrix.

The buttons we clicked are shown below:

The Sampling Density value of 10,000 collision means how many random robot positions to check for self-collision. The higher the better collision detection matrix it generates, but we will need more time and processing power in parallel.

The Min Collisions of 95% means that for considering that the pair is always colliding has to be 95% of all the random positions tested.

After clicking Generate Collision Matrix we have the matrix generated as in the image below.

The links are listed in a Linear View. Let’s select the Matrix View because that view allows a better understanding.

The green parts of the image above means “Never Collide”.

Note that because the Collision Matrix is random, it could be that every time you generate a new matrix, the matrix appears a bit different:

 

Virtual Joints

A robot that moves around a plane is specified using a planar virtual joint that connects the world frame to the frame of the robot. A robot that doesn’t move, will use a fixed joint to connect the base frame to the world.

In our case we select:

  • Name: virtual_joint ( just to know its a virtual joint )
  • Parent: world
  • Child: the base_link, that we want to connect to world.
  • Joint Type: Fixed, because we won’t move.

You can play with this because, what if we select as a parent a link of another robot, like a turtlebot that moves around? These things we will go deeper when the time comes. DONT FORGET TO HIT SAVE.

Planning Groups

Doing Inverse Kinematics is computationally very intensive. This means that the simpler the kinematics to solve the better. That’s why normally we divide a robot into the maximum parts that allow a correct and easy IK calculation. Ex: a robot with TWO arms, normally will be divided into LEFT_ARM and RIGHT_ARM, because we don’t need to solve inverse kinematics for both, but it all depends on the use case.

We will choose the following:

  • Solver: kdl_kinematics_plugin/KDLKinematicsPlugin as the kinematics solver. This is the plugin in charge of calculating the inverse kinematics. It’s a generic Kinematic solver, that will be ok for now. It is the default one in Moveit!. It only works with Serial Kinematics Chains ( tensegrities and things like that won’t work).
  • Name: openmanipulator_arm seems appropriate.
  • Kin Solv. Attempts: 3 seems reasonable.
  • Planner: for now we leave this none
  • We add the joins!

In our case, we select ALL the Joints except the gripper.

Now we add the gripper through joints also:

  • joint8
  • joint9
  • id_6

NOTE that we are NOT adding anywhere the LINK7 that’s because it serves no purpose in the planning for the moment.

Other Kinematic Solvers: If you want to create a custom Kinematic Solver for your Robotics ARM: http://docs.ros.org/kinetic/api/moveit_tutorials/html/doc/ikfast/ikfast_tutorial.html

Solver that in theory produces more reliable solutions than Jacobian methods in KDL:http://docs.ros.org/kinetic/api/moveit_tutorials/html/doc/trac_ik/trac_ik_tutorial.html

Generate Premade Robot Poses

We now can store premade robot poses to set the robot in safe positions, calibration, very used positions, etc.

 

We added four poses:

  • Two for the OpenManipulator_ARM group
  • Two for the Gripper group.

End Effector:

We can add now the gripper as end-effector. This unblocks some functionality related exclusively with end effectors:

We set:
  • Name: gripper
  • Group ENDEffector: gripper
  • Parent Link: Link5
  • Parent Group: Not necessary here to state it.

 

Things we won’t set:

These are elements that we might enable after, but for now, we leave them unset because we don’t need them:

  • Passive Joints: These are for caster wheels and other arent actuated.
  • 3D perception: We don’t have for the moment any sensors (We mey add it afterwards).
  • Simulation: When executed it tells us there is nothing to change.

ROS controllers:

Here we have two options:
  • Add the controllers manually: This allows us to select which type of control we are adding.
  • Auto-add FollowTrajectoryControll

For the moment we will AutoGenerate the FollowJointTrajectory Control. If we need to change it we just have to re-edit the Moveit package.

Author: Add the Author info

 

FINALLY, generate the package

Now is the time to generate the package that we will use:

  • Select the location. Hit browse and create a new folder with the name of the package you want. In our case, we will call it open_manipulator_morpheuschair_movit_config.

Test what we have created:

cd ~/catkin_ws
source devel/setup.bash;rospack profile
roslaunch open_manipulator_morpheuschair_movit_config demo.launch rviz_tutorial:=true

APPENDIX

So, that is the post for today. We hope you guys enjoyed it. Remember that we also have a video showing everything that is in this post. Please have a look at the video if you didn’t understand some of the things that were explained here. If you liked the content, please subscribe to our channel and leave your comments on the comments section of the video, which is available at:

 

List of materials

 

Edited by Miguel Angel and Ruben Alves.

Topics: ROS Q&A

Discover How to Equip Your Teams with ROS Skills Fast

The Construct For Enterprise

Check Out These Related Posts

0 Comments

Submit a Comment

Your email address will not be published.

This site uses Akismet to reduce spam. Learn how your comment data is processed.

Pin It on Pinterest

Share This