diff --git a/python/rcs/_core/sim.pyi b/python/rcs/_core/sim.pyi index 8ea8ae40..042e2608 100644 --- a/python/rcs/_core/sim.pyi +++ b/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: ... diff --git a/src/pybind/rcs.cpp b/src/pybind/rcs.cpp index 59833a97..ac6e21ae 100644 --- a/src/pybind/rcs.cpp +++ b/src/pybind/rcs.cpp @@ -543,7 +543,9 @@ PYBIND11_MODULE(_core, m) { std::vector arm_collision_geoms, std::vector joints, std::optional q_home, - std::vector actuators, std::string base, + std::vector actuators, + std::optional> kp, + std::optional> kv, std::string base, size_t dof, const Eigen::Matrix& 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) diff --git a/src/sim/SimRobot.cpp b/src/sim/SimRobot.cpp index 646a2387..6e785cee 100644 --- a/src/sim/SimRobot.cpp +++ b/src/sim/SimRobot.cpp @@ -29,6 +29,7 @@ SimRobot::SimRobot(std::shared_ptr 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; } diff --git a/src/sim/SimRobot.h b/src/sim/SimRobot.h index 5270bd67..97109c35 100644 --- a/src/sim/SimRobot.h +++ b/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> kp; + std::optional> kv; std::string base = "base"; void add_prefix(const std::string& id) {