ROS-Industrial Basic Developer’s

Training Class
July 2023

Southwest Research Institute

1
 Session 4:
Motion Planning
Moveit! Planning using C++
Intro to Planners
Intro to Perception
Southwest Research Institute

2
 Motion Planning in C++

MoveIt! provides a high-level C++ API:

moveit_cpp

#include <moveit/moveit_cpp/moveit_cpp.h>
...
moveit_cpp::MoveItCpp::Ptr moveItCpp = make_shared(node);
moveit_cpp::PlanningComponent::Ptr planner = make_shared(“arm", moveItCpp);

planner->setGoal(“home”);
planner->plan();
planner->execute();

5 lines = collision-aware path planning & execution

Amazing!

3
Reminder: MoveIt! Complexity

Sensor Input

Main difference
between MoveGroup
and MoveItCpp is the
layer of abstraction

http://moveit.ros.org/wiki/High-level_Overview_Diagram
http://moveit.ros.org/wiki/Pipeline_Overview_Diagram 4
 Motion Planning in C++

Pre-defined position:
planner.setGoal(“home”);

Joint position:
robot_state::RobotState joints.setStateValues(names, positions);
planner.setGoal(joints);

Cartesian position:
Affine3d pose = {x, y, z, r, p, y};
planner.setGoal(pose);

5
 Exercise 4.0
Exercise 4.0:
Motion Planning using C++ Param: base_frame

fake_ar_pub

vision_node myworkcell_node

descartes_node
myworkcell_moveit_cfg
ur5_driver
myworkcell_support

7
 Intro to Planners
• Types of Motion Plans
• Basic Toolpath Plan
• Planning Workflows
• Common Motion Planners
– OMPL
– Descartes
– TrajOpt
• Motion Planning Frameworks
• Simple Planning Pipelines
• Advanced Planning Pipelines
8
 Types of Motion Plans
Freespace Process Combined

Motion plans between far- Motion plans optimize Motion plans that can be
spaced start and end robot pose between segmented into portions
points under-constrained that are freespace motions
waypoints and others that are process
motions

Example: Moving from a Example: A continuous line Example: Moving from a

generic, off-the-surface mapped around the edge generic, off-the-surface
"start pose" to the upper of a piece to be welded "start pose" to the edge of
righthand corner of a a jig-held part and then
surface for painting welding the edge at a
known EE angle

9
 Toolpath Plan Example

Definitions
Raster - A series of
PART specified Cartesian
waypoints to be
executed without
breaking*
Transition - A
freespace move
between rasters
Entry/Exit - A freespace
move from/to a position
away from the part

*depends on
application

10
 Common Motion Planners

Motion Planner Application Space Notes

OMPL Free-space Planning Stochastic sampling; Easy and convenient
interface
TrajOpt Trajectory Optimization Optimize existing trajectory on constraints
(distance from collision, joint limits, etc.)

Descartes Cartesian path planning Globally optimum; sampling-based search;

Captures “tolerances”
Simple Planner Free-space Planning Naive simple linear interpolation between
waypoints
STOMP Free-space Planning Optimization-based; Emphasizes smooth
paths
CHOMP Trajectory Optimization Gradient-based trajectory optimization for
collision avoidance and cost-reduction

11
 OMPL
Open Motion Planning Library: Final Joint
Configuration
Randomly Sample Valid Joint States then
Solve for Sequence
Planners we often use:
• RRT
– Build a tree along different potential joint
configurations to arrive at the final pose
• RRT-Connect
– Build a tree from each side and try to connect
them
– Parameters Starting Joint
Configuration
Unideal Sample
Joint Configuration
• Range (same as above)
• See more at
https://ompl.kavrakilab.org/planners.htm
l

12
 Descartes
Sample ‘all’ Possible Solutions then Graph Search for
Best Trajectory

13
 TrajOpt
Optimize Seed Trajectory based on Weighted
Cost Functions (distance from collision, joint
limits, etc.)
• All parameters have a coefficient that can
be increased/decreased to change its
influence
• Example costs:
– Proximity to a singularity
– Velocity/Acceleration/Jerk smoothing
– Avoid collisions
• Weighed sums of all collision terms
• Safety margin-based cost
– Encourage/discourage DOF usage
• Cartesian: rotation about z encouraged &
unconstrained
• Joint: usage of the wrist discouraged with a high
cost
• Constraints are simply infinite costs
– The absolute limit of the safety margin would
be set and anything in collision with it would
cause the planner to fail

14
 Motion Planning Environments
Interfaces used to generate motion plans can be:
• Open Source or License-based
• UI or script based
• Leverage a variety of planners
• Contain additional hooks to simulation packages

These differ from raw planners with:

• ROS API
• Collision environment management
• Visualization packages
• Planning pipeline/Task Constructor capabilities
MoveIt!/MoveIt!2 Tesseract

Easy to use interface, wizard features, broad toolset Enables very complex planning, different toolset

15
INTRODUCTION TO PERCEPTION

18
 Outline

• Camera Calibration
• 3D Data Introduction
• Explanation of the Perception Tools
Available in ROS
• Intro to PCL tools
– Exercise 4.1

19
 Objectives

• Understanding of the calibration capabilities

• Experience with 3D data and RVIZ
• Experience with Point Cloud Library tools*

20
 Industrial Calibration

• Perform intrinsic and extrinsic calibration

• Continuously improving library
• Resources, library
– Github link
– Wiki link
• Resources, tutorials
– Github industrial calibration tutorials link

21
Industrial (Intrinsic) Calibration

• The INTRINSIC Calibration

procedure requires
movement of the camera to
known positions along an
axis that is approximately
normal to the calibration
target.
• Using the resulting intrinsic
calibration parameters for a
given camera yields
significantly better extrinsic
calibration or pose
estimation accuracy.
22
Industrial (Extrinsic) Calibration

https://www.youtube.com/watch?v=MJFtEr_Y4ak
23
 3D Cameras

• RGBD cameras, TOF

cameras, stereo vision, 3D
laser scanner
• Driver for Asus Xtion camera
and the Kinect (1.0) is in the
package openni2_launch
• Driver for Kinect 2.0 is in
package iai_kinect2 (github
link)
• https://rosindustrial.org/3d-
camera-survey

24
 3D Cameras

• Produce (colored) point cloud data

• Huge data volume
– Over 300,000 points per cloud
– 30 Hz frame rate

25
 Perception Processing Pipeline
• Goal: Gain knowledge from sensor
data
Camera
• Process data in order to
– Improve data quality  filter noise
– Enhance succeeding processing steps
 reduce amount of data
– Create a consistent environment Processing
model  Combine data from different
view points
– Simplify detection problem 
segment interesting regions Robot
– Gain knowledge about environment  Capabilities
classify surfaces

26
 Perception Tools

• Overview of OpenCV • Focus on PCL tools for

• Overview of PCL exercise
• PCL and OpenCV in ROS
• Other libraries

27
 Perception Libraries (OpenCV)

• Open Computer Vision Library (OpenCv) -

http://opencv.org/
– Focused on 2D images
– 2D Image processing
– Video
– Sensor calibration
– 2D features
– GUI
– GPU acceleration
http://opencv.org

28
 OpenCV tutorial

• Perform image processing to determine pump

orientation (roll angle)
• Github tutorial link
• Training Wiki link

29
 Perception Libraries (OpenCV)

• Open CV 3.2
– Has more 3D tools
• LineMod
– https://www.youtube.com/watch?v=vsThfxzIUjs
• PPF
– Has opencv_contrib
• Community contributed code
• Some tutorials

30
 Perception Libraries (PCL)
• Point Cloud Library (PCL) -
http://pointclouds.org/
– Focused on 3D Range(Colorized) data

http://pointclouds.org

31
 Perception Libraries (PCL)
• PCL Command Line Tools
– sudo apt install pcl-tools
– Tools (140+)
• pcl_viewer
• pcl_point_cloud_editor
• pcl_voxel_grid
• pcl_sac_segmentation_plane
• pcl_cluster_extraction
• pcl_passthrough_filter
• pcl_marching_cubes_reconstruction
• pcl_normal_estimation
• pcl_outlier_removal

32
 ROS Bridges

• OpenCV & PCL are external libraries

• “Bridges” are created to adapt the
libraries to the ROS architecture
– OpenCV: http://ros.org/wiki/vision_opencv
– PCL: http://ros.org/wiki/pcl_ros
• Standard Nodes (PCL Filters):
http://ros.org/wiki/pcl_ros#ROS_nodelets

33
 Many More Libraries

• Many more libraries in the ROS Ecosystem

– AR Tracker
http://www.ros.org/wiki/ar_track_alvar
– Robot Self Filter
http://www.ros.org/wiki/robot_self_filter

34
 Exercise 4.1

• Play with PointCloud data

– Play a point cloud file to simulate data coming from a
Asus 3D sensor.
– Matches scene for demo_manipulation
– 3D Data in ROS 2
– Use PCL Command Line Tools
• https://industrial-training-
master.readthedocs.io/en/humble/_source/sessi
on4/ros2/2-Introduction-to-Perception.html

35
 Review/Q&A

Session 3 Session 4
ROS-Industrial Moveit! Planning
• Architecture Intro to Planners
• Capabilities Perception
Motion Planning • Calibration
• PointCloud File
• Examine MoveIt Planning • OpenCV
Environment
• PCL
• Setup New Robot • PCL Command Line Tools
• Motion Planning (Rviz)
• Motion Planning (C++)

36