feat(sim): adds configureable kp and kv gains for sim robots

Author: juelg

python/rcs/_core/sim.pyi

@@ -263,11 +263,29 @@ class SimRobotConfig(rcs._core.common.RobotConfig[M]):
             "fr3_joint6",
             "fr3_joint7",
         ],
+        kp: list[float] | None = None,
+        kv: list[float] | None = None,
         base: str = "base",
         dof: int = 7,
         joint_limits: numpy.ndarray[tuple[typing.Literal[2], M], numpy.dtype[numpy.float64]] = ...,
     ) -> None: ...
     def add_prefix(self, id: str) -> None: ...
+    @property
+    def kp(self) -> list[float] | None:
+        """
+        Per-joint position gains; None uses the MuJoCo XML values
+        """
+
+    @kp.setter
+    def kp(self, arg0: list[float] | None) -> None: ...
+    @property
+    def kv(self) -> list[float] | None:
+        """
+        Per-joint velocity gains; None uses the MuJoCo XML values
+        """
+
+    @kv.setter
+    def kv(self, arg0: list[float] | None) -> None: ...
 
 class SimRobotState(rcs._core.common.RobotState):
     def __init__(self) -> None: ...

src/pybind/rcs.cpp

@@ -543,7 +543,9 @@ PYBIND11_MODULE(_core, m) {
                       std::vector<std::string> arm_collision_geoms,
                       std::vector<std::string> joints,
                       std::optional<rcs::common::VectorXd> q_home,
-                      std::vector<std::string> actuators, std::string base,
+                      std::vector<std::string> actuators,
+                      std::optional<std::vector<double>> kp,
+                      std::optional<std::vector<double>> kv, std::string base,
                       size_t dof,
                       const Eigen::Matrix<double, 2, Eigen::Dynamic,
                                           Eigen::ColMajor>& joint_limits) {
@@ -559,6 +561,8 @@ PYBIND11_MODULE(_core, m) {
             config.arm_collision_geoms = arm_collision_geoms;
             config.joints = joints;
             config.actuators = actuators;
+            config.kp = kp;
+            config.kv = kv;
             config.base = base;
             config.dof = dof;
             config.joint_limits = joint_limits;
@@ -580,6 +584,7 @@ PYBIND11_MODULE(_core, m) {
           py::arg("joints") = default_simrobot_cfg.joints,
           py::arg("q_home") = default_simrobot_cfg.q_home,
           py::arg("actuators") = default_simrobot_cfg.actuators,
+          py::arg("kp") = std::nullopt, py::arg("kv") = std::nullopt,
           py::arg("base") = default_simrobot_cfg.base,
           py::arg("dof") = default_simrobot_cfg.dof,
           py::arg("joint_limits") = default_simrobot_cfg.joint_limits)
@@ -594,6 +599,12 @@ PYBIND11_MODULE(_core, m) {
                      &rcs::sim::SimRobotConfig::arm_collision_geoms)
       .def_readwrite("joints", &rcs::sim::SimRobotConfig::joints)
       .def_readwrite("actuators", &rcs::sim::SimRobotConfig::actuators)
+      .def_readwrite(
+          "kp", &rcs::sim::SimRobotConfig::kp,
+          "Per-joint position gains; None uses the MuJoCo XML values")
+      .def_readwrite(
+          "kv", &rcs::sim::SimRobotConfig::kv,
+          "Per-joint velocity gains; None uses the MuJoCo XML values")
       .def_readwrite("base", &rcs::sim::SimRobotConfig::base)
       .def_readwrite("dof", &rcs::sim::SimRobotConfig::dof)
       .def_readwrite("joint_limits", &rcs::sim::SimRobotConfig::joint_limits)

src/sim/SimRobot.cpp

@@ -29,6 +29,7 @@ SimRobot::SimRobot(std::shared_ptr<Sim> sim,
                    bool register_convergence_callback)
     : sim{sim}, cfg{cfg}, state{}, m_ik(ik) {
   this->init_ids();
+  this->set_config(cfg);
   if (register_convergence_callback) {
     this->sim->register_cb(std::bind(&SimRobot::is_arrived_callback, this),
                            this->cfg.seconds_between_callbacks);
@@ -101,6 +102,22 @@ void SimRobot::init_ids() {
 bool SimRobot::set_config(const SimRobotConfig& cfg) {
   this->cfg = cfg;
   this->state.inverse_tcp_offset = cfg.tcp_offset.inverse();
+  if (cfg.kp.has_value() != cfg.kv.has_value())
+    throw std::runtime_error("kp and kv must both be set or unset");
+  if (cfg.kp.has_value()) {
+    size_t n = std::size(this->ids.actuators);
+    if (cfg.kp->size() != n || cfg.kv->size() != n)
+      throw std::runtime_error("kp/kv size must match number of joints");
+    for (size_t i = 0; i < n; ++i) {
+      int act = this->ids.actuators[i];
+      if (this->sim->m->actuator_gaintype[act] != mjGAIN_FIXED ||
+          this->sim->m->actuator_biastype[act] != mjBIAS_AFFINE)
+        throw std::runtime_error("kp/kv require a position actuator");
+      this->sim->m->actuator_gainprm[act * mjNGAIN + 0] = (*cfg.kp)[i];
+      this->sim->m->actuator_biasprm[act * mjNBIAS + 1] = -(*cfg.kp)[i];
+      this->sim->m->actuator_biasprm[act * mjNBIAS + 2] = -(*cfg.kv)[i];
+    }
+  }
   return true;
 }
 

src/sim/SimRobot.h

@@ -29,6 +29,8 @@ struct SimRobotConfig : common::RobotConfig {
       "fr3_joint1", "fr3_joint2", "fr3_joint3", "fr3_joint4",
       "fr3_joint5", "fr3_joint6", "fr3_joint7",
   };
+  std::optional<std::vector<double>> kp;
+  std::optional<std::vector<double>> kv;
   std::string base = "base";
 
   void add_prefix(const std::string& id) {