CODEWITHSUNDEEP
drake
none
none

Reputation: 555

How to implement collision constraints for trajectory optimization in pydrake?

I'm trying to ensure that a multi-linked robot doesn't collide with an obstacle while doing trajectory optimization. Would I be able to use ComputeSignedDistancePairClosestPoints within a MathematicalProgram constraint to do this?

For example, if I wanted to add a constraint that my end effector did not collide with a circular geometry, could I add a constraint to all knot points in the trajectory optimization that ComputeSignedDistancePairClosestPoints is greater than some distance? My original thought was to use forward kinematics to calculate the end effector position in world coordinates in terms of the state variables (joint angles), then find the distance from this point to the obstacle and add a constraint that the distance must be greater than the radius of the obstacle.

Upvotes: 1

Views: 516

Answers (1)

Hongkai Dai
Hongkai Dai

Reputation: 2766

You could try to use the constraint DistanceConstraint which imposes that the distance between a specific pair of geometries is above certain threshold. Alternatively you can try MinimumDistanceConstraint which says the distance among any pair of geometries is above certain threshold.

In pydrake, you can do

from pydrake.multibody.inverse_kinematics import DistanceConstraint
prog.AddConstraint(DistanceConstraint(plant, [geometry_id1, geometry_id2], plant_context, distance_lower, distance_upper), q)

Upvotes: 3

Related Questions