diff --git a/bindings/pyc3/BUILD.bazel b/bindings/pyc3/BUILD.bazel index 789b30d..ced0886 100644 --- a/bindings/pyc3/BUILD.bazel +++ b/bindings/pyc3/BUILD.bazel @@ -47,10 +47,12 @@ pybind_py_library( "@drake//bindings/pydrake/common:default_scalars_pybind", "@drake//bindings/pydrake/common:value_pybind", "@drake//bindings/pydrake/common:sorted_pair_pybind", + "@drake//bindings/pydrake/common:type_pack", ], cc_so_name = "systems", cc_srcs = ["c3_systems_py.cc"], py_deps = [ + "@drake//bindings/pydrake", # TODO: This may break pip install when building pyc3_wheel ":module_py", ], py_imports = ["."], diff --git a/bindings/pyc3/c3_multibody_py.cc b/bindings/pyc3/c3_multibody_py.cc index ca7abea..5fdd861 100644 --- a/bindings/pyc3/c3_multibody_py.cc +++ b/bindings/pyc3/c3_multibody_py.cc @@ -71,7 +71,9 @@ PYBIND11_MODULE(multibody, m) { &ContactPairConfig::body_B_collision_geom_indices) .def_readwrite("mu", &ContactPairConfig::mu) .def_readwrite("num_friction_directions", - &ContactPairConfig::num_friction_directions); + &ContactPairConfig::num_friction_directions) + .def_readwrite("planar_normal_direction", + &ContactPairConfig::planar_normal_direction); py::class_(m, "LCSFactoryOptions") .def(py::init<>()) @@ -85,6 +87,10 @@ PYBIND11_MODULE(multibody, m) { .def_readwrite("num_contacts", &LCSFactoryOptions::num_contacts) .def_readwrite("spring_stiffness", &LCSFactoryOptions::spring_stiffness) .def_readwrite("mu", &LCSFactoryOptions::mu) + .def_readwrite("planar_normal_direction", + &LCSFactoryOptions::planar_normal_direction) + .def_readwrite("planar_normal_direction_per_contact", + &LCSFactoryOptions::planar_normal_direction_per_contact) .def_readwrite("contact_pair_configs", &LCSFactoryOptions::contact_pair_configs); diff --git a/examples/resources/cartpole_softwalls/c3Plus_controller_cartpole_options.yaml b/examples/resources/cartpole_softwalls/c3Plus_controller_cartpole_options.yaml index e6b4972..e25e758 100644 --- a/examples/resources/cartpole_softwalls/c3Plus_controller_cartpole_options.yaml +++ b/examples/resources/cartpole_softwalls/c3Plus_controller_cartpole_options.yaml @@ -17,6 +17,7 @@ lcs_factory_options: mu: [0, 0] N: 5 dt: 0.01 + planar_normal_direction: [0, 1, 0] c3_options: warm_start: false diff --git a/examples/resources/cartpole_softwalls/c3_controller_cartpole_options.yaml b/examples/resources/cartpole_softwalls/c3_controller_cartpole_options.yaml index 1f886fa..ced5cf6 100644 --- a/examples/resources/cartpole_softwalls/c3_controller_cartpole_options.yaml +++ b/examples/resources/cartpole_softwalls/c3_controller_cartpole_options.yaml @@ -17,6 +17,7 @@ lcs_factory_options: mu: [ 0, 0 ] N: 5 dt: 0.01 + planar_normal_direction: [0, 1, 0] c3_options: warm_start: false diff --git a/examples/resources/cube_pivoting/c3_controller_pivoting_options.yaml b/examples/resources/cube_pivoting/c3_controller_pivoting_options.yaml index 8e83e9c..7c8b2b9 100644 --- a/examples/resources/cube_pivoting/c3_controller_pivoting_options.yaml +++ b/examples/resources/cube_pivoting/c3_controller_pivoting_options.yaml @@ -20,18 +20,21 @@ lcs_factory_options: body_B_collision_geom_indices: [0] mu: 0.1 num_friction_directions: 1 + planar_normal_direction: [0, 1, 0] - body_A: "cube" body_A_collision_geom_indices: [0] body_B: "right_finger" body_B_collision_geom_indices: [0] mu: 0.1 num_friction_directions: 1 + planar_normal_direction: [0, 1, 0] - body_A: "cube" body_A_collision_geom_indices: [0] body_B: "platform" body_B_collision_geom_indices: [0] mu: 0.1 num_friction_directions: 1 + planar_normal_direction: [0, 1, 0] c3_options: warm_start: false diff --git a/multibody/lcs_factory.cc b/multibody/lcs_factory.cc index 6d40095..0ae0c3d 100644 --- a/multibody/lcs_factory.cc +++ b/multibody/lcs_factory.cc @@ -53,6 +53,7 @@ LCSFactory::LCSFactory( mu_.clear(); n_friction_directions_per_contact_.clear(); contact_pairs_.clear(); + planar_normal_direction_per_contact_.clear(); for (auto& pair : options.contact_pair_configs.value()) { std::vector body_A_collision_geoms = plant.GetCollisionGeometriesForBody(plant.GetBodyByName(pair.body_A)); @@ -67,6 +68,14 @@ LCSFactory::LCSFactory( pair.num_friction_directions); } } + if (pair.num_friction_directions == 1) { + DRAKE_DEMAND(pair.planar_normal_direction.has_value()); + DRAKE_DEMAND(pair.planar_normal_direction.value().size() == 3); + planar_normal_direction_per_contact_.push_back( + pair.planar_normal_direction.value()); + } else { + planar_normal_direction_per_contact_.push_back({}); + } } n_lambda_ = multibody::LCSFactory::GetNumContactVariables( contact_model_, n_contacts_, n_friction_directions_per_contact_); @@ -100,6 +109,30 @@ LCSFactory::LCSFactory( mu_(options.mu.value()), frictionless_(contact_model_ == ContactModel::kFrictionlessSpring), dt_(options.dt) { + // Handle planar normal directions for contacts with 1 friction direction + // Initialize all with empty default values + planar_normal_direction_per_contact_.resize(n_contacts_, {}); + + for (int i = 0; i < n_contacts_; ++i) { + if (n_friction_directions_per_contact_[i] == 1) { + // Planar contact - assign the appropriate normal + if (options_.planar_normal_direction.has_value()) { + // Single planar normal provided - use for all planar contacts + DRAKE_DEMAND(options_.planar_normal_direction.value().size() == 3); + planar_normal_direction_per_contact_[i] = + options_.planar_normal_direction.value(); + } else { + // Per-contact planar normals provided + DRAKE_DEMAND(options_.planar_normal_direction_per_contact.has_value()); + DRAKE_DEMAND( + options_.planar_normal_direction_per_contact.value().size() == + (size_t)n_contacts_); + planar_normal_direction_per_contact_[i] = + options_.planar_normal_direction_per_contact.value()[i]; + } + } + // Non-planar contacts keep the default empty value {} + } Jt_row_sizes_ = 2 * Eigen::Map( n_friction_directions_per_contact_.data(), n_friction_directions_per_contact_.size()); @@ -112,14 +145,16 @@ void LCSFactory::ComputeContactJacobian(VectorXd& phi, MatrixXd& Jn, Jt.resize(Jt_row_sizes_.sum(), n_v_); // Tangential contact Jacobian - Eigen::Vector3d planar_normal = {0, 1, 0}; double phi_i; MatrixX J_i; for (int i = 0; i < n_contacts_; i++) { multibody::GeomGeomCollider collider(plant_, contact_pairs_[i]); - if (frictionless_ || n_friction_directions_per_contact_[i] == 1) + if (frictionless_ || n_friction_directions_per_contact_[i] == 1) { + Eigen::Vector3d planar_normal = + Eigen::Map( + planar_normal_direction_per_contact_[i].data()); std::tie(phi_i, J_i) = collider.EvalPlanar(context_, planar_normal); - else + } else std::tie(phi_i, J_i) = collider.EvalPolytope( context_, n_friction_directions_per_contact_[i]); diff --git a/multibody/lcs_factory.h b/multibody/lcs_factory.h index 40916e7..57274db 100644 --- a/multibody/lcs_factory.h +++ b/multibody/lcs_factory.h @@ -316,7 +316,11 @@ class LCSFactory { int n_contacts_; ///< Number of contact points. std::vector n_friction_directions_per_contact_; ///< Number of friction directions. - ContactModel contact_model_; ///< The contact model being used. + std::vector> + planar_normal_direction_per_contact_; ///< Optional normal vector for + ///< planar contact for each + ///< contact. + ContactModel contact_model_; ///< The contact model being used. int n_q_; ///< Number of configuration variables. int n_v_; ///< Number of velocity variables. int n_x_; ///< Number of state variables. diff --git a/multibody/lcs_factory_options.h b/multibody/lcs_factory_options.h index e27d664..b85597c 100644 --- a/multibody/lcs_factory_options.h +++ b/multibody/lcs_factory_options.h @@ -1,5 +1,7 @@ #pragma once +#include + #include "drake/common/yaml/yaml_io.h" #include "drake/common/yaml/yaml_read_archive.h" @@ -11,7 +13,9 @@ struct ContactPairConfig { std::vector body_A_collision_geom_indices; std::vector body_B_collision_geom_indices; int num_friction_directions; - double mu; // friction coefficient + std::optional> + planar_normal_direction; // optional normal vector for planar contact + double mu; // friction coefficient template void Serialize(Archive* a) { @@ -20,6 +24,7 @@ struct ContactPairConfig { a->Visit(DRAKE_NVP(body_A_collision_geom_indices)); a->Visit(DRAKE_NVP(body_B_collision_geom_indices)); a->Visit(DRAKE_NVP(num_friction_directions)); + a->Visit(DRAKE_NVP(planar_normal_direction)); a->Visit(DRAKE_NVP(mu)); } }; @@ -35,6 +40,11 @@ struct LCSFactoryOptions { num_friction_directions_per_contact; // Number of friction directions per std::optional> mu; // Friction coefficient for each contact + std::optional> + planar_normal_direction; // Optional normal vector for planar contact + std::optional>> + planar_normal_direction_per_contact; // Optional normal vector for planar + // contact for each contact std::optional> contact_pair_configs; // Optional detailed contact pair configurations @@ -50,6 +60,8 @@ struct LCSFactoryOptions { a->Visit(DRAKE_NVP(num_friction_directions)); a->Visit(DRAKE_NVP(num_friction_directions_per_contact)); a->Visit(DRAKE_NVP(mu)); + a->Visit(DRAKE_NVP(planar_normal_direction)); + a->Visit(DRAKE_NVP(planar_normal_direction_per_contact)); a->Visit(DRAKE_NVP(contact_pair_configs)); a->Visit(DRAKE_NVP(N)); diff --git a/multibody/test/resources/lcs_factory_pivoting_options.yaml b/multibody/test/resources/lcs_factory_pivoting_options.yaml index 5b8f128..1b7b7f6 100644 --- a/multibody/test/resources/lcs_factory_pivoting_options.yaml +++ b/multibody/test/resources/lcs_factory_pivoting_options.yaml @@ -1,5 +1,6 @@ contact_model: "stewart_and_trinkle" num_friction_directions: 1 +planar_normal_direction: [0, 1, 0] num_contacts: 3 spring_stiffness: 0 mu: [0.1, 0.1, 0.1] @@ -12,15 +13,18 @@ contact_pair_configs: # Not used but kept to test parsing body_B_collision_geom_indices: [0] mu: 0.1 num_friction_directions: 1 + planar_normal_direction: [0, 1, 0] - body_A: "cube" body_A_collision_geom_indices: [0] body_B: "right_finger" body_B_collision_geom_indices: [0] mu: 0.1 num_friction_directions: 1 + planar_normal_direction: [0, 1, 0] - body_A: "cube" body_A_collision_geom_indices: [0] body_B: "platform" body_B_collision_geom_indices: [0] mu: 0.1 num_friction_directions: 1 + planar_normal_direction: [0, 1, 0]