refactor(sim): generalize gripper to use a single joint

Author: juelg

python/rcs/_core/sim.pyi

@@ -116,8 +116,7 @@ class SimGripperConfig(rcs._core.common.GripperConfig):
     epsilon_inner: float
     epsilon_outer: float
     ignored_collision_geoms: list[str]
-    joint1: str
-    joint2: str
+    joint: str
     seconds_between_callbacks: float
     def __init__(self) -> None: ...
     def add_id(self, id: str) -> None: ...

src/pybind/rcs.cpp

@@ -467,8 +467,7 @@ PYBIND11_MODULE(_core, m) {
                      &rcs::sim::SimGripperConfig::collision_geoms)
       .def_readwrite("collision_geoms_fingers",
                      &rcs::sim::SimGripperConfig::collision_geoms_fingers)
-      .def_readwrite("joint1", &rcs::sim::SimGripperConfig::joint1)
-      .def_readwrite("joint2", &rcs::sim::SimGripperConfig::joint2)
+      .def_readwrite("joint", &rcs::sim::SimGripperConfig::joint)
       .def_readwrite("actuator", &rcs::sim::SimGripperConfig::actuator)
       .def("add_id", &rcs::sim::SimGripperConfig::add_id, py::arg("id"));
   py::class_<rcs::sim::SimGripperState, rcs::common::GripperState>(

src/sim/SimGripper.cpp

@@ -21,15 +21,10 @@ SimGripper::SimGripper(std::shared_ptr<Sim> sim, const SimGripperConfig &cfg)
   }
   // this->tendon_id =
   //     mj_name2id(this->sim->m, mjOBJ_TENDON, ("split_" + id).c_str());
-  this->joint_id_1 = this->sim->m->jnt_qposadr[mj_name2id(
-      this->sim->m, mjOBJ_JOINT, this->cfg.joint1.c_str())];
-  if (this->joint_id_1 == -1) {
-    throw std::runtime_error(std::string("No joint named " + this->cfg.joint1));
-  }
-  this->joint_id_2 = this->sim->m->jnt_qposadr[mj_name2id(
-      this->sim->m, mjOBJ_JOINT, this->cfg.joint2.c_str())];
-  if (this->joint_id_2 == -1) {
-    throw std::runtime_error(std::string("No joint named " + this->cfg.joint2));
+  this->joint_id = this->sim->m->jnt_qposadr[mj_name2id(
+      this->sim->m, mjOBJ_JOINT, this->cfg.joint.c_str())];
+  if (this->joint_id == -1) {
+    throw std::runtime_error(std::string("No joint named " + this->cfg.joint));
   }
   // Collision geoms
   this->add_collision_geoms(this->cfg.collision_geoms, this->cgeom, false);
@@ -95,7 +90,7 @@ void SimGripper::set_normalized_width(double width, double force) {
 double SimGripper::get_normalized_width() {
   // TODO: maybe we should use the mujoco sensors? Not sure what the difference
   // is between reading out from qpos and reading from the sensors.
-  double width = this->sim->d->qpos[this->joint_id_1] / this->MAX_JOINT_WIDTH;
+  double width = this->sim->d->qpos[this->joint_id] / this->MAX_JOINT_WIDTH;
   // sometimes the joint is slightly outside of the bounds
   if (width < 0) {
     width = 0;
@@ -157,8 +152,7 @@ void SimGripper::m_reset() {
   this->state = SimGripperState();
   this->state.max_unnormalized_width = this->MAX_WIDTH;
   // reset state hard
-  this->sim->d->qpos[this->joint_id_1] = this->MAX_JOINT_WIDTH;
-  this->sim->d->qpos[this->joint_id_2] = this->MAX_JOINT_WIDTH;
+  this->sim->d->qpos[this->joint_id] = this->MAX_JOINT_WIDTH;
   this->sim->d->ctrl[this->actuator_id] = this->MAX_WIDTH;
 }
 

src/sim/SimGripper.h

@@ -23,8 +23,7 @@ struct SimGripperConfig : common::GripperConfig {
 
   std::vector<std::string> collision_geoms_fingers{"finger_0_left",
                                                    "finger_0_right"};
-  std::string joint1 = "finger_joint1";
-  std::string joint2 = "finger_joint2";
+  std::string joint = "finger_joint1";
   std::string actuator = "actuator8";
 
   void add_id(const std::string &id) {
@@ -37,8 +36,7 @@ struct SimGripperConfig : common::GripperConfig {
     for (auto &s : this->ignored_collision_geoms) {
       s = s + "_" + id;
     }
-    this->joint1 = this->joint1 + "_" + id;
-    this->joint2 = this->joint2 + "_" + id;
+    this->joint = this->joint + "_" + id;
     this->actuator = this->actuator + "_" + id;
   }
 };
@@ -58,8 +56,7 @@ class SimGripper : public common::Gripper {
   SimGripperConfig cfg;
   std::shared_ptr<Sim> sim;
   int actuator_id;
-  int joint_id_1;
-  int joint_id_2;
+  int joint_id;
   SimGripperState state;
   bool convergence_callback();
   bool collision_callback();