I'm trying to include a signed distance constraint/cost in my MathematicalProgram for obstacle avoidance of some predetermined obstacles.
I understand from these questions that the correct way to do it is probably to wrap everything in a MultibodyPlant. Unfortunately, I lacked the foresight to do this, hence what I have right now is just a point in the form of an Expression that I wish to evaluate the signed distance to a set of AnchoredGeometry in the SceneGraph.
My current attempt is as follows:
scene_graph = SceneGraph()
source_id = scene_graph.RegisterSource()
scene_graph.RegisterAnchoredGeometry(source_id, GeometryInstance(...))
query_object = scene_graph.get_query_output_port().Eval(
scene_graph.CreateDefaultContext()
)
...
point = ... # array([<Expression "x(0,0)">, <Expression "x(0,1)">, <Expression "x(0,2)">], dtype=object)
prog.AddConstraint(
query_object.ComputeSignedDistanceToPoint(point)
>= desired_distance
)
Unfortunately, this gives me the error
TypeError: ComputeSignedDistanceToPoint(): incompatible function arguments. The following argument types are supported:
1. (self: pydrake.geometry.QueryObject, p_WQ: numpy.ndarray[numpy.float64[3, 1]], threshold: float = inf) -> List[drake::geometry::SignedDistanceToPoint<double>]
Invoked with: <pydrake.geometry.QueryObject object at 0x71897253d030>, array([<Expression "x(0,0)">, <Expression "x(0,1)">,
<Expression "x(0,2)">], dtype=object)
Is there any way to get this to work?
Unfortunately SceneGraph doesn't support symbolic computation in the distance query (typically the distance is computed numerically through FCL, which doesn't support Drake's symbolic expression).
On the other hand, we have DistanceConstraint class which put lower/upper bounds on the distance between a pair of geometries. Could you use this constraint? One pair of the geometry could be a sphere with a tiny radius (hence a point).
You mentioned issue with MultibodyPlant. Do you not have MultibodyPlant in this problem, but only have a SceneGraph?