drake
How do I get hydroelastic contact forces in Drake C++?
I have the following grasp simulation in Drake C++ with a Robotiq-140 gripper on a irregular mesh (which is derived from the robotiq.cc code mentioned in Issue 19842 ):
#include <iostream>
#include <memory>
#include <string>
#include <fmt/format.h>
#include "drake/multibody/parsing/parser.h"
#include "drake/multibody/parsing/package_map.h"
#include "drake/multibody/plant/multibody_plant.h"
#include "drake/multibody/parsing/collision_filter_groups.h"
#include "drake/systems/analysis/simulator.h"
#include "drake/systems/framework/diagram_builder.h"
#include "drake/systems/primitives/constant_vector_source.h"
#include "drake/visualization/visualization_config_functions.h"
#include "drake/multibody/plant/discrete_contact_pair.h"
namespace drake {
namespace examples {
namespace simple_gripper {
namespace {
int do_main() {
auto meshcat = std::make_shared<geometry::Meshcat>();
systems::DiagramBuilder<double> builder;
auto [plant, scene_graph] =
multibody::AddMultibodyPlantSceneGraph(&builder, 0.002);
plant.set_discrete_contact_approximation( drake::multibody::DiscreteContactApproximation::kLagged);
multibody::Parser parser(&plant);
multibody::PackageMap::RemoteParams params;
params.urls = {"https://github.com/RussTedrake/kinova-movo/archive/"
"d94d1d7da7ff8fc71f2439bb0a8989f1e6fd79b4.tar.gz"};
params.sha256 =
"a9201477a23f410f10d00e86847de778c175d3d3c8971be52a9ac881194e4887";
params.strip_prefix = "kinova-movo-d94d1d7da7ff8fc71f2439bb0a8989f1e6fd79b4";
parser.package_map().AddRemote("kinova-movo", params);
parser.package_map().AddPackageXml(
parser.package_map().GetPath("kinova-movo") +
"/movo_common/movo_description/package.xml");
std::string with_mimic = R"""(
directives:
- add_model:
name: spam
file: package://drake/examples/simple_gripper/mesh.sdf
default_free_body_pose: { base_link: {
translation: [0.2, 0.05, 0.00],
rotation: !Rpy { deg: [90.0, 0.0, 0.0 ]}
} }
- add_model:
name: table
file: package://drake/examples/kuka_iiwa_arm/models/table/extra_heavy_duty_table_surface_only_collision.sdf
- add_weld:
parent: world
child: table::table_link
X_PC:
translation: [0.0, 0.0, -0.81]
)""";
parser.AddModelsFromString(with_mimic, "dmd.yaml");
parser.AddModelsFromUrl(
"package://drake/examples/simple_gripper/robotiq_140_gripper.urdf");
plant.WeldFrames(
plant.world_frame(),
plant.GetBodyByName("robotiq_arg2f_base_link").body_frame(),
math::RigidTransformd(math::RollPitchYawd(M_PI , 0, M_PI),
Eigen::Vector3d(0.2, 0, 0.21)));
plant.Finalize();
auto torque = builder.AddSystem<systems::ConstantVectorSource>(Vector1d(2));
builder.Connect(torque->get_output_port(), plant.get_actuation_input_port());
visualization::AddDefaultVisualization(&builder, meshcat);
auto diagram = builder.Build();
// Set up simulator.
systems::Simulator simulator(*diagram);
meshcat->StartRecording(32.0, false);
simulator.AdvanceTo(20.0);
meshcat->PublishRecording();
// Pause so that you can see the meshcat output.
std::cout << "[Press Ctrl-C to finish]." << std::endl;
std::cin.ignore(std::numeric_limits<std::streamsize>::max(), '\n');
return 0;
}
} // namespace
} // namespace simple_gripper
} // namespace examples
} // namespace drake
int main(int, char*[]) {
return drake::examples::simple_gripper::do_main();
}
It produces the following contact forces:
How do I get the two long red (translational) contact forces and their starting points in the picture?
Solution
One way of doing this is to add the following to your code.
const auto& final_context = simulator.get_context();
const auto& plant_context = diagram->GetSubsystemContext(plant, final_context);
const ContactResults<double>& contact_results =
plant.get_contact_results_output_port().Eval<ContactResults<double>>(plant_context);
std::cout << "Contact forces and centroids at the end of the simulation:" << std::endl;
for (int i = 0; i < contact_results.num_hydroelastic_contacts(); ++i) {
const HydroelasticContactInfo<double>& info =
contact_results.hydroelastic_contact_info(i);
const Vector3d& F_Ac_W = info.F_Ac_W().translational();
const Vector3d& p_WC = info.contact_surface().centroid();
std::cout << "Contact " << i << ":" << std::endl;
//Force applied on body A, at the centroid point C, expressed in the world frame W
std::cout << " F_Ac_W: [" << F_Ac_W.x() << ", " << F_Ac_W.y() << ", " << F_Ac_W.z() << "]" << std::endl;
//position p_WC of the centroid point C in the world frame W
std::cout << " p_WC: [" << p_WC.x() << ", " << p_WC.y() << ", " << p_WC.z() << "]" << std::endl;
}
(more info about this can be found here: https://drake.mit.edu/doxygen_cxx/classdrake_1_1multibody_1_1_multibody_plant.html https://drake.mit.edu/doxygen_cxx/classdrake_1_1multibody_1_1_contact_results.html https://drake.mit.edu/doxygen_cxx/classdrake_1_1multibody_1_1_hydroelastic_contact_info.html )
You can print out the values of the contact forces at the end of the simulation. The whole code is then:
#include <iostream>
#include <memory>
#include <string>
#include <fmt/format.h>
#include "drake/common/eigen_types.h"
#include "drake/multibody/parsing/parser.h"
#include "drake/multibody/parsing/package_map.h"
#include "drake/multibody/plant/multibody_plant.h"
#include "drake/multibody/parsing/collision_filter_groups.h"
#include "drake/systems/analysis/simulator.h"
#include "drake/systems/framework/diagram_builder.h"
#include "drake/systems/primitives/constant_vector_source.h"
#include "drake/visualization/visualization_config_functions.h"
#include "drake/multibody/plant/discrete_contact_pair.h"
#include "drake/multibody/plant/contact_results.h"
namespace drake {
namespace examples {
namespace simple_gripper {
using Eigen::Vector3d;
using multibody::ContactResults;
using multibody::HydroelasticContactInfo;
namespace {
int do_main() {
auto meshcat = std::make_shared<geometry::Meshcat>();
systems::DiagramBuilder<double> builder;
auto [plant, scene_graph] =
multibody::AddMultibodyPlantSceneGraph(&builder, 0.002);
plant.set_discrete_contact_approximation( drake::multibody::DiscreteContactApproximation::kLagged);
multibody::Parser parser(&plant);
multibody::PackageMap::RemoteParams params;
params.urls = {"https://github.com/RussTedrake/kinova-movo/archive/"
"d94d1d7da7ff8fc71f2439bb0a8989f1e6fd79b4.tar.gz"};
params.sha256 =
"a9201477a23f410f10d00e86847de778c175d3d3c8971be52a9ac881194e4887";
params.strip_prefix = "kinova-movo-d94d1d7da7ff8fc71f2439bb0a8989f1e6fd79b4";
parser.package_map().AddRemote("kinova-movo", params);
parser.package_map().AddPackageXml(
parser.package_map().GetPath("kinova-movo") +
"/movo_common/movo_description/package.xml");
std::string with_mimic = R"""(
directives:
- add_model:
name: spam
file: package://drake/examples/simple_gripper/mesh.sdf
default_free_body_pose: { base_link: {
translation: [0.2, 0.05, 0.00],
rotation: !Rpy { deg: [90.0, 0.0, 0.0 ]}
} }
- add_model:
name: table
file: package://drake/examples/kuka_iiwa_arm/models/table/extra_heavy_duty_table_surface_only_collision.sdf
- add_weld:
parent: world
child: table::table_link
X_PC:
translation: [0.0, 0.0, -0.81]
)""";
parser.AddModelsFromString(with_mimic, "dmd.yaml");
parser.AddModelsFromUrl(
"package://drake/examples/simple_gripper/robotiq_140_gripper.urdf");
plant.WeldFrames(
plant.world_frame(),
plant.GetBodyByName("robotiq_arg2f_base_link").body_frame(),
math::RigidTransformd(math::RollPitchYawd(M_PI , 0, M_PI),
Eigen::Vector3d(0.2, 0, 0.21)));
plant.Finalize();
auto torque = builder.AddSystem<systems::ConstantVectorSource>(Vector1d(2));
builder.Connect(torque->get_output_port(), plant.get_actuation_input_port());
visualization::AddDefaultVisualization(&builder, meshcat);
auto diagram = builder.Build();
// Set up simulator.
systems::Simulator simulator(*diagram);
meshcat->StartRecording(32.0, false);
simulator.AdvanceTo(20.0);
meshcat->PublishRecording();
const auto& final_context = simulator.get_context();
const auto& plant_context = diagram->GetSubsystemContext(plant, final_context);
const ContactResults<double>& contact_results =
plant.get_contact_results_output_port().Eval<ContactResults<double>>(plant_context);
std::cout << "Contact forces and centroids at the end of the simulation:" << std::endl;
for (int i = 0; i < contact_results.num_hydroelastic_contacts(); ++i) {
const HydroelasticContactInfo<double>& info =
contact_results.hydroelastic_contact_info(i);
const Vector3d& F_Ac_W = info.F_Ac_W().translational();
const Vector3d& p_WC = info.contact_surface().centroid();
std::cout << "Contact " << i << ":" << std::endl;
//Force applied on body A, at the centroid point C, expressed in the world frame W
std::cout << " F_Ac_W: [" << F_Ac_W.x() << ", " << F_Ac_W.y() << ", " << F_Ac_W.z() << "]" << std::endl;
//position p_WC of the centroid point C in the world frame W
std::cout << " p_WC: [" << p_WC.x() << ", " << p_WC.y() << ", " << p_WC.z() << "]" << std::endl;
}
// Pause so that you can see the meshcat output.
std::cout << "[Press Ctrl-C to finish]." << std::endl;
std::cin.ignore(std::numeric_limits<std::streamsize>::max(), '\n');
return 0;
}
} // namespace
} // namespace simple_gripper
} // namespace examples
} // namespace drake
int main(int, char*[]) {
return drake::examples::simple_gripper::do_main();
}
The output would look like this:
Contact forces and centroids at the end of the simulation:
Contact 0:
F_Ac_W: [0.000115463, -0.0332584, 2.36681]
p_WC: [0.22126, 0.0196855, -0.0459446]
Contact 1:
F_Ac_W: [-0.0887268, 10.3418, -0.539927]
p_WC: [0.201652, -0.0288424, -0.0140817]
Contact 2:
F_Ac_W: [0.0886113, -10.3086, -1.33639]
p_WC: [0.200856, 0.0288451, -0.0139309]
Steps to reproduce and run this code and example
git clone -b VanillaDrakeWithIssue19842Mods https://github.com/hedaniel7/drake.git
cd drake
git checkout aabad5f8c1599d8507331e668c634e5666466cda
bazel build //examples/simple_gripper:robotiq_140_shape_completion_get_contact_forces
bazel run //examples/simple_gripper:robotiq_140_shape_completion_get_contact_forces
- Specify background color in Scenario/ Camera Config yaml
- How to add end effector constraints to vertices and edges of a GCS?
- Can KinematicTrajectoryOptimization support gurobi solver
- Linking Error with ROS PCL and Drake: Different VTK versions
- Reproducing the ManipulationStation for a custom robot
- Visualising output of RGBD sensor in drake
- Trouble using bazel built Drake with ROS2
- How to make a gripper with two fingers (2DOF) have 1 DOF making one finger symmetric to the other
- Setting non-zero time step in Multibody Plant still results in a Continuous system
- Why create a standalone plant just for the controller
- Parts of .obj missing in visualizer
- Simulate deformable objects with drake
- Report bodies that are involved in a collision in Drake
- Reading contact_results creates algebraic loop
- What is the spatial inertia frame and expressed about point for the spatial inertias in the inverse dynamics?
- How to serialize trajectories to file?
- Is specifying the same decision variable multiple times in AddConstraint problematic?
- QueryObject.ComputeSignedDistanceToPoint with Expression type
- writing diagram to SVG uses ascii by default, not utf-8
- Collision between environment bodies should already be filtered
- `KinematicTrajectoryOptimization` fails with `DurationCost` and any `AccelerationBounds` (even infinite)
- SetFreeBodyPose is not actually relative to the world frame
- Extracting subsystem from system
- How to lock specific states of the base joint 'panda_link0'?
- How to correctly update the mass of a body in the context
- How do I get hydroelastic contact forces in Drake C++?
- How do I use RotaryEncoder and other templated sensor types in pydrake?
- Solving a drake::symbolic::expression for a graph of convex sets
- Render with RgbdSensor on server
- Clarification on the distinction between AddPathVelocityConstraint and AddVelocityConstraintAtNormalizedTime for KinematicTrajectoryOptimization