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 9d2275e3cc5f5772e5e7f41760318790b5c49643
bazel build //examples/simple_gripper:robotiq_140_shape_completion_get_contact_forces
bazel run //examples/simple_gripper:robotiq_140_shape_completion_get_contact_forces
- 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
- How can I apply a scale transformation to a RigidBody?
- How do I get specific results from the GraphOfConvexSets class in Drake?
- How to visualize Jacobian of Mathematical Program
- How do I get hydroelastic contact forces in Drake C++?
- How can I add a model and directly weld it?
- Dynamic Geometry (Mesh) Changes post-plant finalize
- Drake geometry added using MultiBodyPlant is too shiny
- Weird behavior from model when simulating it on drake
- No module named 'manipulation'
- How to edit alpha transparency for new GLTF models programatically
- Pick and place simulation using the Robotiq 2F-140 manipulator in drake
- Why was I reminded that my plant is not fully actuated？
- Stitching together trajectories
- Adding items to simulation at runtime
- Integration of drake to OpenSUSE - Algorithm/LinearSolvers/IpMumpsSolverInterface.cpp:28:10: fatal error: mpi.h: No such file or directory
- Integration of drake to OpenSUSE - error: cannot convert std::string_view
- Up-to-date workflow for adding external forces to object post-finalized for trajectory optimization
- Integration of drake to OpenSUSE - build error with spdlog and fmt Part 2
- Integration of drake to OpenSUSE - error in drake/tools/workspace/pybind11/mkdoc.py
- Integration of drake to OpenSUSE - build error with spdlog and fmt
- Deformable torus not showing in pydrake port of C++ deformable torus example
- Integration of drake to OpenSUSE - error in external/mumps_internal/BUILD.bazel
- Getting joint location(3D pose) w.r.t world of Kuka Iiwa arm in Drake
- Using InverseDynamicsController on coupled joints
- Can I visualize (but not simulate) using drake?
- Update PyDrake to the most recent version
- InverseDynamicsController and InverseDynamics producing unexpected output