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]
