Are there any differences in retrieving contact information between a double plant and an AutoDiffXd plant?
I created the same scene with both plants in my use case. The case with double reports the contact information with the context by the following code:
query_port_double = plant_double.get_geometry_query_input_port()
query_object_double = query_port_double.Eval(mutable_plant_context_double_list[0])
contact_info_port_double = plant_double.get_contact_results_output_port()
contact_results_double = contact_info_port_double.Eval(mutable_plant_context_double_list[0])
contact_results_double.num_point_pair_contacts()
However, when I do the same for AutoDiffXd case with the same procedures:
query_port_AD = plant_AD.get_geometry_query_input_port()
AD_query_object = query_port_AD.Eval(mutable_plant_contextAD_list[0])
contact_info_port_AD = plant_AD.get_contact_results_output_port()
contact_results_AD = contact_info_port_AD.Eval(mutable_plant_contextAD_list[0])
contact_results_AD.num_point_pair_contacts()
This reports 0 contacts. I checked current positions in the plant_AD with plant_AD.GetPositions(mutable_plant_contextAD_list[0]). It shows the same numerical values as in the plant_double.
Did I miss something when using the AutoDiffXd query_object?
Are the port connections wrong for the plant_AD? I did the following to create the AD version:
builder = DiagramBuilder()
plant = builder.AddSystem(MultibodyPlant(time_step=h))
parser = Parser(plant)
table_model_index = parser.AddModels(tablefile)[0]
box_model_index = parser.AddModels(boxfile)[0]
hand_model_index = parser.AddModels(handfile)[0]
table_top_frame = plant.GetFrameByName("table_top_center")
hand_frame = plant.GetFrameByName("panda_hand")
box_rame = plant.GetFrameByName("base_link_cracker")
plant.WeldFrames(plant.world_frame(), table_top_frame, X_W_table)
plant.Finalize()
# convert into AD
builder_AD = DiagramBuilder_[AutoDiffXd]()
plant_AD = plant.ToScalarType[AutoDiffXd]()
connected_plantAD, connected_sceneGraphAD = AddMultibodyPlantSceneGraph(builder_AD, plant_AD)
Another general question: how do I supply gradients to the solver when enforcing the constraints by functions inside which plant methods and also geometry_query_input_port are called. Does Drake do it internally? Can I provide the sparsity structure of the gradients? It would be nice if there is such a tutorial or example to poke around.
Thanks in advance! Cheers
If you look at the documentation for ComputePairPointPenetration(), you'll note that it has explicitly declared that contact isn't generally supported. You can get contact between spheres and some of the primitives. (I am mildly confused that your code doesn't throw....)
The most acute reason for this is that the underlying computations being used to compute point-pair contact aren't compatible with autodiff; essentially we can't guarantee that you get gradients, so we've chosen not to lie about it.
The support for AutoDiff is nominal -- it means if you have a Diagram with a SceneGraph in it and you want to convert the whole Diagram to AutoDiff for some purpose, the SceneGraph won't stop you. But it does mean you have a more limited set of operations you can do on the AutoDiff Diagram than you could on the double variant.
For constraints/costs you might consider simply using the signed distance in a double-valued Diagram. The signed distance comes with a gradient value. That is frequently used in minimum distance constraints and the like.