Grasp Matrix definition and calculation with hydro-elastic contact wrench results - extension of commonly used soft finger contact model?

Question

I have a theoretical question:

I want to define and calculate a grasp quality measure on grasps simulated in drake with hydroelastic contact. For that purpose I want to use the properties of grasp matrix G which is one of the commonly used grasp metrics. G is defined in Murray's A Mathematical Introduction to Robotic Manipulation as G = Ad^T * B, where Ad is the wrench transformation matrix and B is the wrench component selection matrix dependent on the contact model used. One of the most commonly mentioned contact model is the soft finger contact model and B would be in that case a matrix which only lets through the 3 linear forces and the moment around the z-axis of the 6-dimensional wrench.

However, from the results I have simulated here, I get nontrivial values for the moments around x- and y-axes as well:

This code simulates the grasp of a parallel jaw gripper robotiq 140 on an irregular shaped object:

#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();
                        const Vector3d& tau_Ac_W = info.F_Ac_W().rotational();

                        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;
                        // Moment
                        std::cout << "  tau_Ac_W: [" << tau_Ac_W.x() << ", " << tau_Ac_W.y() << ", " << tau_Ac_W.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 two simulated contact wrenches of the parallel jaw gripper:

Contact 1:
  F_Ac_W: [-0.0261167, 10.1739, -0.962392]
  p_WC: [0.203198, -0.0284439, -0.014547]
  tau_Ac_W: [0.00708233, -0.00437683, 0.0108222]
Contact 2:
  F_Ac_W: [0.139772, -10.2277, -1.66222]
  p_WC: [0.203141, 0.0284596, -0.0147996]
  tau_Ac_W: [0.00690174, -0.000686731, -0.00920002]

As you can see that the Moment around the x-axis of the first contact has 70% of the magnitude of the Moment around the z-axis. I fear that if I used the soft finger contact model, these nontrivial would be neglected, leading to a false picture presented by the grasp metric based on this. Also, if we completely ignore the moments about the x- and y-axes in the soft finger contact model, the grasp analysis could potentially indicate that we would need more contact points in order to achieve force closure, which would not correspond to the simulated results.

A natural remedy coming to my mind would be to extend the soft finger contact model to a so-called "complete-constrained" model (mentioned here on p. 7 for example ). We would used the identity matrix for B and in extension define new rotational friction constants (dependent or indepent of each other) for the moments around x- and y-axis just like we did for the z-axis in the soft finger contact model. (table taken from p.219 of Murray's A Mathematical Introduction to Robotic Manipulation)

Since the old theory of soft finger contact models (and also point contact with and without friction), assume a point contact and the new hydroelastic contact modelling is area based, I wanted to check with you if this makes any sense or if a grasp matrix is ill-defined with hydroelastic contact.

(A sidenote: It was suggested to me that I use the center of pressure, the point on the contact surface where the acted wrench has zero moment in x- and y-axis, but I think the problem still remains that the calculated grasp matrix would not reflect any moments in x- and y- direction and thus not portray the (potentially force closed) simulated results with hydroelastic contact)

Thank you.

Answer 1

It's an interesting question, and I would think it should have a satisfying answer (but it might require some work). Let me state in back to you in my own words...

A Cartesian force applied at a point contact lies within a friction cone, and induces (through the grasp matrix) a contact wrench cone. Hydroelastic contacts take a surface integral over the pressure field, accumulating the Cartesian force at every dx to a total contact wrench. Is there any efficient description of the contact wrench cone for this contact model? (this is, of course, related to the literature on limit surfaces)

Of course the soft contact model is only a very coarse approximation in the first place, and I expect you're only looking for a coarse approximation here, too. One could probably work out what happens for box on box, and try to understand that first. Maybe you 6dof friction cone would be a reasonable approximation? I'm not actually sure without digging in a bit more.