High

DOF Motion Planning

Dinesh Manocha
Department of Computer Science
UNC Chapel Hill
[email protected]
http://gamma.cs.unc.edu

1
Warehouse Automation
• New emerging application for robotics
• Is this the next big thing for robotics after autonomous cars?

• Considerable excitement from academia and industry

• Amazon picking challenge has stimulated a lot of

interest
Warehouse Automation
Need good solutions for:

• perception/vision

• planning

• grasping + manipulation
Warehouse Automation
Need good solutions for:

• perception/vision

• planning

• grasping/manipulation

Integrated solution: combine perception + planning +

grasping in a robust manner
 Hardware Alone is Not the Solution

• In 2011, founder Terry Guo predicted that by 2014

Foxconn would install 1M robots
• However, only tens of thousands robots have been
installed on assembly lines by 2016
• Foxconn found automation ineffective for assembly lines
where people are better than robots?
Robots in Foxconn
 Hardware Alone is Not the Solution

• We need to pay more attention to software and

algorithmic solutions
• Exploit recent growth in computer vision, machine
learning, and parallel computing
• Robots are working with and next to humans
• How can we develop next generation planning
technologies?

High performance and impact can only be achieved by a

combination of hardware and software
Challenge: Human-Robot Interaction
Robot Sensors: Data Collection

Cameras
 Handling Sensor Data
• Human environments
• Clutter, dynamic obstacles
• Data from 3D sensors
• Large number of points (~10k for laser scans, ~20k for
stereo)
• Real-time computation important for fast online
reactive grasping, motion planning
Sensor Data
• Point cloud
• Output from laser/Kinect, etc.
• Cannot encode unknown regions
• Very large
• Octree (octomap)
• Store point cloud in a compact manner
• Support multi-resolution
• Encode occupied/free/unknown regions

11
 FCL: Noisy Point Cloud Data
Rigid Objects Point Clouds Deformable Articulated
Objects Objects
(Discrete) Y Y Y Y
Collision
Detection
Continuous Y Y Y Y
Collision
Detection
Self Collision Y Y Y Y
Detection
Penetration Y N N N
Estimation
Distance Y N Y Y
Computation

Broad-phase Y Y Y Y
Collision
 FCL: Usage
• Independent code, but ROS interface is provided

• Available at http://gamma.cs.unc.edu/FCL

• Part of MoveIt:
http://moveit.ros.org/wiki/MoveIt!
FCL Application: Optimal Inverse
Kinematics for Path Planning (SWRI)
Real-Time Planning with Sensor Data

http://moveit.ros.org/wiki/MoveIt! 15
 Task Executions of Robots
• Advances in technology allow robots to perform
complex tasks

<PR2: fetching a beer from the fridge> <Baxter: $22k robot needs no
programming>
 Task Execution with Multiple
Components
• A task is decomposed into many primitive
subtasks
1.1.Move
Move thethe
body
body to to
thethefridge.
fridge.
2.2.Move
Move thethe
leftleft
armarm to to
thethehandle.
handle.
3.3.Move
Move thethe
body
body to to
open
open thethe
fridge
fridgedoor.
door.
4.4.Move
Move thethe
body
body to to
in in
front
frontofof
thethe
fridge.
fridge.
5.5.Move
Move thethe
leftleft
armarm to to
hold
hold the
thedoor.
door.
6.6.Move
Move thethe
right
right arm
arm to to
thethebeer.
beer.
7.7.Grasp
Grasp thethe
bottle.
bottle.
8.8.Move
Move thethe
right
right arm
arm to to
thethebasket.
basket.
9.9.Release
Release the
the bottle
bottle.
.

<PR2: taking out a beer from the

Most of the subtasks are moving
fridge> the robot to the next desired pose
(From Willow Garage)
 Subtask Execution
• ‘Move the body to the fridge’ subtask
• Use sensors to recognize the objects and
obstacles in the environment
• Compute a collision-free path to the pose close to
the fridge
• Control motors to execute the computed motion
 Robot Planning: Challenges
• How to perform motion planning computation in
realtime?

• How to handle high DOF robots?

• How to handle noisy sensor data?

• How to enable human-robot collaboration?

 Goals

• Realtime Planning algorithm for industrial robots

• End-effector task constraints

• Smooth trajectory
• Avoid singularities
• Dynamic obstacles
• Adapt to human actions
 Motion Planning Algorithms

• Random sampling-based • Optimization-based

algorithms algorithms
 Motion Planning Algorithms

• Random sampling-based • Optimization-based

algorithms algorithms
 Trajectory Computation: Goals

• Compute a trajectory:
• Collision-free
• Singular-free
• Satisfy end-effector trajectory constraint
• Smooth
• Dynamics constraints

23
 Our Approach

• Environment is not frequently changed

• Use precomputation for the environment objects

• Multiple constraints and dynamic obstacles

• Runtime trajectory optimization

24
 Parallel Trajectory Optimization

• Parallel optimization of multiple trajectories

• Use Multiple threads
• Start from different initial trajectories
• Trajectories are generated by quasi-random sampling
• Exploits the multiple CPU cores (multi-cores) or GPU-based cores
(many-cores)

• Used for realtime Motion & Cartesian Planning

http://gamma.cs.unc.edu/ITOMP/ITOMP_ROS/
 Our Approach : Planning Framework
• Precomputation and runtime refinement
Roadmap Precomputation Runtime Trajectory Refinement
Planning
Request
Scheduler
Xgoal
c(t)
Trajectory
PDR Robot Final
P1 Optimization Q*(t)
Roadmap P2 Controller Trajectory
Planner
PM

Robot Static Dynamic

Kinematic Obstacles Obstacles
Constraints

26
 Our Approach : Planning Framework
• Precomputation and runtime refinement
Roadmap Precomputation Runtime Trajectory Refinement
Planning
Request
Scheduler
Xgoal
c(t)
Trajectory
PDR Robot Final
P1 Optimization Q*(t)
Roadmap P2 Controller Trajectory
Planner
PM

Robot Static Dynamic

Kinematic Obstacles Obstacles
Constraints

27
Motion Planning Algorithm: Amazon Challenge

• Random sampling-based • Optimization-based

algorithms algorithms
 29

High DOFs (30-40)

High complexity for optimization
Hierarchical planning

Root : 6 DOF
 30

Floating root (6 DOFs)

Ground contact with feet
Balance constraints:
Dynamic stability

Root : 6 DOF
 31

Compute a trajectory which satisfies

Collision-free
Joint limits (position / velocity / torques, etc.)
Stability (balance)
 32

High-DOF motion planning with stability

Widely used models
Inverted pendulum [Kajita and Tani 1991]
Zero moment point [Huang et al. 2001]
Simplified dynamics model & limited to planar ground

Stability integrated into trajectory optimization

Contact-invariant optimization [Mortdatch et at. 2012]
Direct trajectory optimization [Posa et at. 2013]
 33

The collision-avoiding velocity is used to

set the goal state of the trajectory
Root position of start state :
Root position of goal state :

N internal states of the trajectory is compute

d by an interpolation of the end-point states
 34

Trajectory Optimization

Compute the optimal internal state values

: The cost function for
: Smoothness at
 35

Sum of wrenches (forces and torques)

on robot makes an equilibrium

: Gravity
: Inertia
: Contact reaction wrench
of -th contact point
 36

Contacts are created / destroyed

during the motion
Contact positions / forces affect on
the dynamic stability
 37

For character animation

Simplified dynamics

Human movements have phases

Contacts are invariant in a phase
Reduce the number of contact variables
 38

Continuous search of contact points

Minimize
 39

Trajectory optimization with CIO-based constraint

where
 40
Results

Ducking or stepping over motions

Motions on uneven ground
Side-stepping
41
42
 Cartesian Planning

• Painting Screwing
- x y z translation constraints - x y-axis rotation constraints

43
 Dynamic Obstacle from Kinect

• Depthmap Reconstructed Octomap

44
Real-Time Cartesian Planning

45
Performance on KUKA Manipulators

46
DoraPicker: Amazon Picking Challenge
• Picking and placing general objects in a warehouse
• Evaluated 3 grippers: vacuum, Openhand M2, soft
gripper
• Perception: 3D point cloud -> 6DOF pose of the target
• Planning: Decompose into two subproblems
• Collision-free path from observation to pre-grasp pose
• Move the gripper from from the pre-grasp move to grasp
the object based on a pre-defined grasp plan

• Demonstrated on APC + table scenario

47
Warehouse Automation: Planning
• Planning is an integral component

• Planning needs to take into account perception errors

• Realtime performance

• Constraints on the resulting trajectories

48
DoraPicker: Amazon Challenge

49
 Parallel Trajectory Optimization

Performance improvement with number of cores

 Safe Motion Planning
for Human-Robot Interaction
Motivation:
Uncertain Human Motions
• Co-robots (or cobot)
• Sharing workspace with human
• Human safety
• Uncertain human motions
• Noisy representation of
human motion
• Need future motion
prediction
• Need collision handling
with noisy data
 Goals
• Human motion prediction
• Action classification
• Trajectory estimation
and prediction
• Safe motion planning
• Collision handling with noisy data
• Efficient task planning Current Future
• Reduce task completion time
• Delays when attempting to avoid collisions
Motion Planner Overview

• Human motion prediction

Sense

Plan

Execute

1/5/18 54
Motion Planner Overview

• Trajectory optimization & Replanning

Sense

Plan

Execute

1/5/18 55
Motion Planner Overview

• Probabilistic collision detection

Sense • Computing the collision probability

Plan

Obstacle
Execute
Sensor error and
Robot motion prediction error
distribution
1/5/18 56
Future Motion Probability

Future motion Action Future motion

probability classification regression

: features of current motion

: future motion
: action label

57
Planner Overview

Task Intention & Motion

planner Estimation
Motion Planner

Robot Environment
Motion Prediction

• Human motion database

• Point cloud from Kinect
• Action labels
• Joint motions
• Machine learning
• Action classification (SVM)
• Future motion regression
(Gaussian Processes)
 Action Input Future Action Future Motion
from Kinect Inference Output

15%

Action 1
65%
65% 20% 15%

Action 2
Initial pose Action started 20%

Action 3

60
Future Motion Probability
3. Future motion regression
• Predict the future joint motions for
every action label
• Sparse Pseudo-input Gaussian Process
(SPGP)

4. Combine action classification and

motion regression
Gaussian Process regression

61
Safe Motion Planning with
Predicted Human Motion
• Probabilistic collision detection
• Predicted human motion Center distribution
is used for safe of human joints
human-robot collaboration
• Upper bound of collision
probability between
bounding spheres of
a robot and a human
[Park et al. 16] Robot + Maximum
obstacle probability

1/5/18 62
Safe Motion Planning for Human-Robot

63
Safe Motion Planning for Human-Robot

64
Multi-Agent System
• Independent agents
planning in a shared
environment
• Features of an agent:
• Self actuated
• Independent goals
• Independent sensing
• Other’s actions are
observable
• Environment
• Physical environment
• Obstacles (static or dynamic)

65
 Multi-Robot Navigation
[[Snape et al. 2009, 2011]
• Reciprocal velocity-space
planning on robots
• Challenges for robots
• Sensor Uncertainty
• Motion Uncertainty
• Kineodynamic Constraints
• Implementation on iRobot
Create
• ROS Library implementation
available

http://gamma.cs.unc.edu/HRVO
Multi-Robot Navigation: ROS Integration

[Claes et al., 2011, Willow Garage]