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

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.

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

The Robot ARMAR III 7 DoF arms, 3 DoF hip

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

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

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 until @ local max (no more higher entries) These extensions only applied w/ some probability, so won’t affect probabilistically complete original algo.

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

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.

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.

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

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.

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.

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.

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

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

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

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

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

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.

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.

Dual-Arm J+ RRT

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.

Results and Conclusions

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

Thank You!