1. Humanoid Motion Planning
Dual-Arm Manipulation and Re-Grasping Tasks
Nikolaus Vahrenkamp
Dmitry Berenson
Tamim Asfour
James Kuffner
Rudiger Dillmann
Vivian Shen
COMS 6731

2. Motion Planning: Dual-Arm
Outline
Single Arm IK Solver
Dual-Arm IK Solver
Motion Planning:
Single Arm Reaching
Motion Planning: Dual-Arm
re-grasping
Reachability with two arms makes everyday tasks (loading dishwasher) much easier.

3. Predefined Grasping Positions
Presets
Need collision-free trajectory with reaching/re-grasping motion.
Requires feasible grasping pose wrt object - objects can have predefined possible grasping poses. In this case, object = wok
Predefined Grasping Positions

4. The Robot
ARMAR III
7 DoF arms, 3 DoF hip

5. Reach and Grasp Fixed Object with One Hand
Single Arm IK Solver
Reach and Grasp Fixed Object with One Hand
Reach and grasp fixed obj with one hand, solve IK problem
Operational space for ARMAR-III = 3 hip joints + 7 DoF of arm

6. Randomized IK Solver
All possible elbow positions pre-computed, best one chosen (if multiple best ones, one with the least joint movement or a random solution chosen).
3-DoF Hip randomly sampled until IK succesfully answered.
If time goes to infinity, solution = prob of 1 if it exists. Avoid endless runtime by stopped after specific # of tries and assumes to valid result

7. Reachability Space & Gradient Descent
Left arm (7 DoF)
2D representation
Gradient Descent Optimization
Hip-yaw joint (1 DoF)
Figures out what is reachable in 6D pose space w/ 5D grid of voxels.
Quickly decide if target pose too far from reachable configs (IK solver unlikely to return solution).
Two ways generating:
Solve large # IK reqs, count # successful queries for each voxel
Random sample joint vals, use fwd kinematix to find pose of EE and therefore 6D voxel
Target pose transformed to shoulder coordinate sys & corresponding voxel returned. IK solver called if voxel > 0
Left pic: 2D rep of reachability of left end vector considering 8 DoF-kinematic chain covering hip-yaw joint & left arm.
Furthur speedup: Gradient descent
If bject pose where corres. reachability space entry lies above threshole, look for local maximum (check neighbor voxles). If voxel w/ higher reach. Space, move toward voxel w/ corresp. dimension of voxel. Repeated local max (no more higher entries)
These extensions only applied w/ some probability, so won’t affect probabilistically complete original algo.

8. 10 DoF IK Solver for Armar-III
Wok set to random pose in front of robot. IK solvers w/ and w/o reachability space called to find valid config for bring EE to one of the 15 pre-computed grasping poses.
Example result in partly blocked scheme.
3 hip + 7 arm joint

9. 10 DoF IK Solver for Armar-III
Without Obstacle
With Obstacle
Avg Runtime
# IK Calls
Without Reach. Space
1,404 ms
101.92
2,880 ms
217.3
With Reach. Space
60 ms
6.1
144 ms
13.6
Wok set to random pose in front of robot. IK solvers w/ and w/o reachability space called to find valid config for bring EE to one of the 15 pre-computed grasping poses.
Example result in partly blocked scheme.

10. Position of Both Arms Without Collision
Dual-Arm IK Solver
Position of Both Arms Without Collision
Re-grasp config = collision free object pose + valid/collision free IK-solution for both arms.
IK problem combining 6D obj pose, 3 hip joints, 7 DoF for each arm ---> 23 dim solution vector.

11. Random Sampling
Can sample 6D pose of robot or 3 hip joints randomly until IK solver successful for one of them.
Cartesian position limited to reachable space and rotation component has no restrictions

12. Reachability Space & Results
Without Obstacle
With Obstacle
Avg Runtime
# IK Calls
Flexible grasp selection
1,404 ms
101.92
2,880 ms
217.3
Obj grasped in left hand
60 ms
6.1
144 ms
13.6
Analytical 7-DoF IK solvers only called if IK-prob of >=1 left and >=1 right grasping pose in reachability space is above threshold.
If IK-prob set high enough, costly IK solver will succeed, has valid pose.
Flex grasp = 23 dims, grasped in 1 hand = 17 dims.

13. Motion Planning for Single Arm Reaching Motions
Collision Free Motion with One Planning Scheme
Proposed planning algos combine search for collision free motion w/ search for soln’s of IK problem in one planning scheme.

14. Planning Scheme for Single Arm Motions
Pgrasp: feasible grasping pose
Pgrasp = Tk(Obj_pose)
Grasping pose defined wrt pose of target obj, derived by applying transformation Tk on object pose.
Set of feasible grasps for each object, with info abt transformations b/w EE and final grasp pos, type of grasp, pre-pos of end-effector, & other grasp quality descriptions.
U can pre-calculate IK solutions for each pose of each grasp in database, use as targets for palnning process. Leads to planning scheme w/ limited search, planner could fail. Also time-consuming to pre-calculate.
Following is two algorithms that determine IK solution while planning. Input = grasp set, output = joint-space trajectory to reach object.

15. Jacobian Pseudoinverse-Based RRT (J+ RRT)
Avoid explicit search for IK by directing RRT towards goal pose -> transposed jacobian used to generate C-space steps out of task space goal direction (no bi-RRT cuz no explicit c-space target configuration)
Useful when no IK solver or only grasping pose.
Multiple task space goals = feasible grasps
Qstart = start config, pobj = pose of object, gc = set of feasible grasps
RRT builds tree of reachable + collision-free configs. New config added to tree w/ corres. Pose of hand
ExtendToGoal called w/ some probability. Biases extension to random grasp

16. J+ RRT Red = generated with ExtendToGoal Blue = solution
Green = optimized solution
Found by standard path pruning
Search focused on grasping objects
Joint limits or collisions b/w hand and object also prevent generation of valid soluiton

17. IK-RRT IK solver used to generate goal pos during planning.
Input = set of feasible grasps + pose of objects = set of target poses.
bi-RRT w/ forward initialized as start config, backward nothing until soln found
Planning loop grows 2 trees (bi-RRT), tries to connect them
w/ some prob, random grasp chosen add po

18. IK RRT Red = generated with ExtendToGoal Blue = solution
Green = optimized solution
Found by standard path pruning
Search tree much smaller because of bi-directional approach of the RTT

19. Motion Planning for Dual-Arm Re-Grasping
Collision Free Handing Off Object and Second Hand Grasping Configuration
Two problems:
Config for handing off object from one hand to other – bring object to pos for other hand to grasp, and choosing grasp of 2nd hand
Collision-free trajectory to move both arms

20. Dual-Arm J+ RRT
Target implicitly defined by set of grasps (instead of fixed obj pose & set of grasps)
Need transformation b/w two hands
Instead of moving 1 arm to fixed goal, move both towards each other
New extendtogoal: selects random grasp & config w/ smallest dist b/w two end-effector poses, tries to move both arms towards a re-grasping pose.
Alternately move arms toward corresp. goal poses in task space.
Jacobian pseudoinverse calc-ed for every step -> sample configs generated
Sample ^ tested for collision/violations of joint limits, added to RRt. if re-grasping pose reached, soln found, otherwise chosen RRT nodes excluded from further steps.

21. Dual-Arm J+ RRT
Target implicitly defined by set of grasps (instead of fixed obj pose & set of grasps)
Need transformation b/w two hands
Instead of moving 1 arm to fixed goal, move both towards each other
New extendtogoal: selects random grasp & config w/ smallest dist b/w two end-effector poses, tries to move both arms towards a re-grasping pose.
Alternately move arms toward corresp. goal poses in task space.
Jacobian pseudoinverse calc-ed for every step -> sample configs generated
Sample ^ tested for collision/violations of joint limits, added to RRt. if re-grasping pose reached, soln found, otherwise chosen RRT nodes excluded from further steps.

22. Dual-Arm J+ RRT

23. Dual-Arm IK RRT
IK solver methods for 1 arm can be used to generate dual arm poses.
Solution = valid pose of obj + corresp joint config of hip and both arms for grasping with both hands.
Instead of predefined obj pose, obj attached to kinematic struct of 1 arm, IK solver operates on set of feasible grasp.

24. Results and Conclusions

25. COMS 6731 Nikolaus Vahrenkamp Dmitry Berenson Tamim Asfour
Humanoid Motion Planning for Dual-Arm Manipulation and Re-Grasping Tasks
Nikolaus Vahrenkamp
Dmitry Berenson
Tamim Asfour
James Kuffner
Rudiger Dillmann
Vivian Shen

26. Thank You!