Advanced Descartes Path Planning¶
In this exercise, we will use advanced features with Descartes to solve a complex path for part being held by the robot which gets processed by a stationary tool.
Motivation¶
MoveIt! is a framework meant primarily for performing “free-space” motion where the objective is to move a robot from point A to point B and you don’t particularly care about how that gets done. These types of problems are only a subset of frequently performed tasks. Imagine any manufacturing ‘’process’’ like welding or painting. You very much care about where that tool is pointing the entire time the robot is at work.
This tutorial introduces you to Descartes, a ‘’Cartesian’’ motion planner meant for moving a robot along some process path. It’s only one of a number of ways to solve this kind of problem, but it’s got some neat properties:
- It’s deterministic and globally optimum (to a certain search resolution).
- It can search redundant degrees of freedom in your problem (say you have 7 robot joints or you have a process where the tool’s Z-axis rotation doesn’t matter).
Reference Example¶
Further Information and Resources¶
APIs:
Scan-N-Plan Application: Problem Statement¶
In this exercise, you will add two new nodes, two xacro, and config file to your Scan-N-Plan application, that:
- Takes the config file
puzzle_bent.csv
and creates a descartes trajectory where the part is held by the robot and manipulated around a stationary tool. - Produces a joint trajectory that commands the robot to trace the perimeter of the marker (as if it is dispensing adhesive).
Scan-N-Plan Application: Guidance¶
In the interest of time, we’ve included several files:
- The first is a template node
adv_descartes_node.cpp
where most of the exercise is spent creating the complicated trajectory for deburring a complicated part. - The second node,
adv_myworkcell_node.cpp
, is a duplicate of themyworkcell_node.cpp
that has been updated to call theadv_plan_path
service provided byadv_descartes_node.cpp
. - The config file
puzzle_bent.csv
which contains the path relative to the part coordinate system. - The two xacro files
puzzle_mount.xacro
andgrinder.xacro
which are used to update the urdf/xacroworkcell.xacro
file.
Left to you are the details of:
- Updating the workcell.xacro file to include the two new xacro files.
- Updating the moveit_config package to define a new Planning Group for this exercise, including the new end-effector links.
- Defining a series of Cartesian poses that comprise a robot “path”.
- Translating those paths into something Descartes can understand.
Setup workspace¶
This exercise uses the same workspace from the Basic Training course. If you don’t have this workspace (completed through Exercise 4.1), copy the completed reference code and pull in other required dependencies as shown below. Otherwise move to the next step.
mkdir ~/catkin_ws cd ~/catkin_ws cp -r ~/industrial_training/exercises/4.1/src . cd src git clone https://github.com/jmeyer1292/fake_ar_publisher.git git clone -b kinetic-devel https://github.com/ros-industrial-consortium/descartes.git sudo apt install ros-kinetic-ur-kinematics sudo apt install ros-kinetic-ur-description
Copy over the
adv_descartes_node_unfinished.cpp
into your core package’s src/ folder and rename itadv_descartes_node.cpp
.cp ~/industrial_training/exercises/5.0/src/adv_descartes_node_unfinished.cpp myworkcell_core/src/adv_descartes_node.cpp
Create rules in the
myworkcell_core
package’sCMakeLists.txt
to build a new node calledadv_descartes_node
. As in previous exercises, add these lines near similar lines in the template file (not as a block as shown below).add_executable(adv_descartes_node src/adv_descartes_node.cpp) add_dependencies(adv_descartes_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) target_link_libraries(adv_descartes_node ${catkin_LIBRARIES})
Copy over the
adv_myworkcell_node.cpp
into your core package’s src/ folder.cp ~/industrial_training/exercises/5.0/src/myworkcell_core/src/adv_myworkcell_node.cpp myworkcell_core/src/
Create rules in the
myworkcell_core
package’sCMakeLists.txt
to build a new node calledadv_myworkcell_node
. As in previous exercises, add these lines near similar lines in the template file (not as a block as shown below).add_executable(adv_myworkcell_node src/adv_myworkcell_node.cpp) add_dependencies(adv_myworkcell_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) target_link_libraries(adv_myworkcell_node ${catkin_LIBRARIES})
Copy over the necessesary config file:
mkdir ~/catkin_ws/src/myworkcell_core/config cp ~/industrial_training/exercises/5.0/src/myworkcell_core/config/puzzle_bent.csv myworkcell_core/config/ cp ~/industrial_training/exercises/5.0/src/myworkcell_support/urdf/grinder.xacro myworkcell_support/urdf/ cp ~/industrial_training/exercises/5.0/src/myworkcell_support/urdf/puzzle_mount.xacro myworkcell_support/urdf/ mkdir ~/catkin_ws/src/myworkcell_support/meshes cp ~/industrial_training/exercises/5.0/src/myworkcell_support/meshes/* myworkcell_support/meshes/
Add new package dependencies:
- Add
tf_conversions
toCMakeLists.txt
(2 places) andpackage.xml
(1 place)
- Add
Update your workcell.xacro file.¶
Add two
<include>
tags for the newgrinder.xacro
andpuzzle_mount.xacro
files.Attach the grinder to the world link with the following offset:
<origin xyz="0.0 -0.4 0.6" rpy="0 3.14159 0"/>
- Look in the
grinder.xacro
file to locate the appropriate<child_link>
name. - Copy one of the other
<joint>
tag definitions and modify as appropriate.
- Look in the
Attach the puzzle mount to the robot’s tool0 frame with the following offset:
<origin xyz="0 0 0" rpy="1.5708 0 0"/>
- Look in the
puzzle_mount.xacro
file to locate the appropriate<child_link>
name. You may need to study its various<link>
and<joint>
definitions to find the root link of this part. - The
tool0
frame is standardized across most ROS-I URDFs to be the robot’s end-effector mounting flange.
- Look in the
Launch the demo.launch file within your moveit_config package to verify the workcell. There should be a grinder sticking out of the table and a puzzle-shaped part attached to the robot.
roslaunch myworkcell_moveit_config demo.launch
Add new planning group to your moveit_config package.¶
Re-run the MoveIt! Setup Assistant and create a new Planning Group named puzzle. Define the kinematic chain to extend from the base_link to the new part link.
roslaunch myworkcell_moveit_config setup_assistant.launch
- Note: Since you added geometry, you should also regenerate the allowed collision matrix.
Complete Advanced Descartes Node¶
- First, the function
makePuzzleToolPoses()
needs to be completed. The file path for puzzle_bent.csv is needed. For portability, don’t hardcode the full path. Please use the ROS toolros::package::getPath()
to retrieve the root path of the relevant package.- reference getPath() API
- Next, the function
makeDescartesTrajectory()
needs to be completed. The transform between world and grinder_frame needs to be found. Also Each point needs to have the orientation tolerance set for the z-axis to +/- PI;- reference lookupTransform() API
- reference tf::conversions namespace
- reference TolerancedFrame definition
- reference OrientationTolerance definition
Update the setup.launch file.¶
- Update the file to take a boolean argument named adv so that either the basic or advanced descartes node can be launched. Use
<if>
and<unless>
modifiers to control which node is launched.- reference roslaunch XML wiki
Test Full Application¶
Run the new setup file, then your main advanced workcell node:
roslaunch myworkcell_support setup.launch adv:=true rosrun myworkcell_core adv_myworkcell_node
- Descartes can take several minutes to plan this complex path, so be patient.
- It’s difficult to see what’s happening with the rviz planning-loop animation always running. Disable this loop animation in rviz (Displays -> Planned Path -> Loop Animation) before running
adv_myworkcell_node
.