Integrated Grasp and Motion PlanningFor grasping an object in a cluttered environment several tasks need to take place:

1. Finding a collision free path2. Calculating the Inverse Kinematics (IK)3. Finding a feasible grasping pose for the hand4. If 2 arms, can we compute a grasp for each hand?

Each of these task is difficult with a high DOF humanoid robot

Probabilistic Path Planning: RRT’s

RRT – Rapidly-Exploring Random Trees• Uses probabilistic sampling approach to finding “safe”, collision-free

configurations in Configuration Space (CSPACE).• From an existing safe configuration in freespace, it expands the

configuration towards a goal configuration, checking for collisions between a known safe state and the new state.• Once the tree expands and reaches the goal configuration, a path has

been found.• Path may be very jerky with high DOF’s – smoothing needed

Integrated Grasp and Motion Planning

• Combines collision free path planning RRT with feasible grasp calculation• Assume object and it’s pose are known (from sensing, e.g. vision)• From starting configuration Q_start (joint space), and knowing Cartesian

location of the object to be picked up, P_object, we do RRT expansion of the robot. For single arm, 10 DOF (7 arm, 3 torso – can lean over to reach).• For dual arm, 14-DOF (fixed torso).• For every new node of the tree Q_i, we calculate the forward kinematics to

end point, P_i• A node N(Q_i and P_i) can then be used to test a feasible grasp.

Integrated Grasp and Motion Planning

• At random intervals (roll the dice!) a node N_approach of the RRT is chosen to see if we can find a feasible grasp from here

• We want to move the arm from P_approach to P_object (Cartesian motion). Solving inverse kinematics from P_object can be costly.

• Instead, we use the inverse Jacobian to find a new configuration of the joints, Q_new, that moves us toward P_object:

∆X = J ∆∙ Θ ; J_Inv ∆X = ∆∙ Θ • P_grasp is closest point on object to our current position. • We move in ∆Θ increments towards P_grasp. • We do collision checking at each [Q_new = Q_new +∆Θ] –moving towards P_grasp. • If collision, we test a grasp from the last non-collision Cartesian point, P_new.

Integrated Grasp and Motion Planning

From P_new, we test a grasp for contact. What is the grasp approach direction?• We can randomly pick an approach direction for the hand from a sphere

surrounding P_grasp. This allows for sampling the approach vector space.How do we determine if it is stable grasp?• We can use any number of Grasp Quality measures. We first test for at least 2

fingers being in contact, and then compute a Grasp Quality measure. • If Grasp Quality > threshold (good grasp) then we can recreate the planned

path for this grasp and implement it on the robot.BiManual Grasping: Can run planners in parallel on left and right arms.

Discussion• This method uses the Jacobian to implicitly solve the IK problem. You

could also construct a hybrid IK solver using a 6-DOF arm solution and a 3-DOF torso RRT to solve the IK problem

• This method computes grasps “on the fly”, testing them as they are generated by the planner. You could also have a pre-computed set of canonical grasps, possibly for this known object, that could be used.

• Data-driven grasp planner can be used if object (and its shape) are unknown