From a913338f0033e2a33b435da1977c02ffa9a87e78 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Tue, 20 May 2025 08:50:12 +0200 Subject: [PATCH 1/4] refactor: mujoco attributes in config mujoco xml attribute naming in robot and gripper configuration instead of hard coded --- src/pybind/rcs.cpp | 16 ++++++++++++++-- src/sim/SimGripper.cpp | 31 ++++++++----------------------- src/sim/SimGripper.h | 8 ++++++++ src/sim/SimRobot.cpp | 32 +++++++++----------------------- src/sim/SimRobot.h | 12 ++++++++++++ 5 files changed, 51 insertions(+), 48 deletions(-) diff --git a/src/pybind/rcs.cpp b/src/pybind/rcs.cpp index dc45d13b..76e6e888 100644 --- a/src/pybind/rcs.cpp +++ b/src/pybind/rcs.cpp @@ -413,7 +413,12 @@ PYBIND11_MODULE(_core, m) { &rcs::sim::SimRobotConfig::seconds_between_callbacks) .def_readwrite("realtime", &rcs::sim::SimRobotConfig::realtime) .def_readwrite("trajectory_trace", - &rcs::sim::SimRobotConfig::trajectory_trace); + &rcs::sim::SimRobotConfig::trajectory_trace) + .def_readwrite("robot_type", &rcs::sim::SimRobotConfig::robot_type) + .def_readwrite("arm_collision_geoms", + &rcs::sim::SimRobotConfig::arm_collision_geoms) + .def_readwrite("joints", &rcs::sim::SimRobotConfig::joints) + .def_readwrite("actuators", &rcs::sim::SimRobotConfig::actuators); py::class_(sim, "SimRobotState") .def(py::init<>()) @@ -436,7 +441,14 @@ PYBIND11_MODULE(_core, m) { .def_readwrite("seconds_between_callbacks", &rcs::sim::SimGripperConfig::seconds_between_callbacks) .def_readwrite("ignored_collision_geoms", - &rcs::sim::SimGripperConfig::ignored_collision_geoms); + &rcs::sim::SimGripperConfig::ignored_collision_geoms) + .def_readwrite("collision_geoms", + &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("actuator", &rcs::sim::SimGripperConfig::actuator); py::class_( sim, "SimGripperState") .def(py::init<>()) diff --git a/src/sim/SimGripper.cpp b/src/sim/SimGripper.cpp index 49948c4a..d9b64fac 100644 --- a/src/sim/SimGripper.cpp +++ b/src/sim/SimGripper.cpp @@ -7,19 +7,6 @@ #include #include -struct { - // TODO(juelg): add camera mount collision (first a name needs to be - // added to the model) - std::vector collision_geoms{"hand_c", "d435i_collision", - "finger_0_left", "finger_0_right"}; - - std::vector collision_geoms_fingers{"finger_0_left", - "finger_0_right"}; - std::string joint1 = "finger_joint1"; - std::string joint2 = "finger_joint2"; - std::string actuator = "actuator8"; -} const gripper_names; - namespace rcs { namespace sim { @@ -28,28 +15,26 @@ SimGripper::SimGripper(std::shared_ptr sim, const std::string &id, : sim{sim}, cfg{cfg}, id{id} { this->state = SimGripperState(); this->actuator_id = mj_name2id(this->sim->m, mjOBJ_ACTUATOR, - (gripper_names.actuator + "_" + id).c_str()); + (this->cfg.actuator + "_" + id).c_str()); if (this->actuator_id == -1) { throw std::runtime_error( - std::string("No actuator named " + gripper_names.actuator)); + std::string("No actuator named " + this->cfg.actuator)); } // 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, (gripper_names.joint1 + "_" + id).c_str())]; + this->sim->m, mjOBJ_JOINT, (this->cfg.joint1 + "_" + id).c_str())]; if (this->joint_id_1 == -1) { - throw std::runtime_error( - std::string("No joint named " + gripper_names.joint1)); + 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, (gripper_names.joint2 + "_" + id).c_str())]; + this->sim->m, mjOBJ_JOINT, (this->cfg.joint2 + "_" + id).c_str())]; if (this->joint_id_2 == -1) { - throw std::runtime_error( - std::string("No joint named " + gripper_names.joint2)); + throw std::runtime_error(std::string("No joint named " + this->cfg.joint2)); } // Collision geoms - this->add_collision_geoms(gripper_names.collision_geoms, this->cgeom, false); - this->add_collision_geoms(gripper_names.collision_geoms_fingers, this->cfgeom, + this->add_collision_geoms(this->cfg.collision_geoms, this->cgeom, false); + this->add_collision_geoms(this->cfg.collision_geoms_fingers, this->cfgeom, false); this->sim->register_all_cb(std::bind(&SimGripper::convergence_callback, this), diff --git a/src/sim/SimGripper.h b/src/sim/SimGripper.h index 8a06f4ef..accedc8e 100644 --- a/src/sim/SimGripper.h +++ b/src/sim/SimGripper.h @@ -18,6 +18,14 @@ struct SimGripperConfig : common::GripperConfig { double epsilon_outer = 0.005; double seconds_between_callbacks = 0.05; // 20 Hz std::vector ignored_collision_geoms = {}; + std::vector collision_geoms{"hand_c", "d435i_collision", + "finger_0_left", "finger_0_right"}; + + std::vector collision_geoms_fingers{"finger_0_left", + "finger_0_right"}; + std::string joint1 = "finger_joint1"; + std::string joint2 = "finger_joint2"; + std::string actuator = "actuator8"; }; struct SimGripperState : common::GripperState { diff --git a/src/sim/SimRobot.cpp b/src/sim/SimRobot.cpp index 25720e8d..9488ca79 100644 --- a/src/sim/SimRobot.cpp +++ b/src/sim/SimRobot.cpp @@ -19,24 +19,10 @@ #include "mujoco/mjmodel.h" #include "mujoco/mujoco.h" -struct { - std::array arm_collision_geoms{ - "fr3_link0_collision", "fr3_link1_collision", "fr3_link2_collision", - "fr3_link3_collision", "fr3_link4_collision", "fr3_link5_collision", - "fr3_link6_collision", "fr3_link7_collision"}; - std::array joints = { - "fr3_joint1", "fr3_joint2", "fr3_joint3", "fr3_joint4", - "fr3_joint5", "fr3_joint6", "fr3_joint7", - }; - std::array actuators = { - "fr3_joint1", "fr3_joint2", "fr3_joint3", "fr3_joint4", - "fr3_joint5", "fr3_joint6", "fr3_joint7", - }; -} const model_names; - namespace rcs { namespace sim { +// TODO: check dof contraints // TODO: use C++11 feature to call one constructor from another SimRobot::SimRobot(std::shared_ptr sim, const std::string& id, std::shared_ptr ik, @@ -66,8 +52,8 @@ void SimRobot::move_home() { void SimRobot::init_ids() { std::string name; // Collision geoms - for (size_t i = 0; i < std::size(model_names.arm_collision_geoms); ++i) { - name = model_names.arm_collision_geoms[i]; + for (size_t i = 0; i < std::size(this->cfg.arm_collision_geoms); ++i) { + name = this->cfg.arm_collision_geoms[i]; name.append("_"); name.append(this->id); int id = mj_name2id(this->sim->m, mjOBJ_GEOM, name.c_str()); @@ -85,8 +71,8 @@ void SimRobot::init_ids() { throw std::runtime_error(std::string("No site named " + name)); } // Joints - for (size_t i = 0; i < std::size(model_names.joints); ++i) { - name = model_names.joints[i]; + for (size_t i = 0; i < std::size(this->cfg.joints); ++i) { + name = this->cfg.joints[i]; name.append("_"); name.append(this->id); this->ids.joints[i] = mj_name2id(this->sim->m, mjOBJ_JOINT, name.c_str()); @@ -95,8 +81,8 @@ void SimRobot::init_ids() { } } // Actuators - for (size_t i = 0; i < std::size(model_names.actuators); ++i) { - name = model_names.actuators[i]; + for (size_t i = 0; i < std::size(this->cfg.actuators); ++i) { + name = this->cfg.actuators[i]; name.append("_"); name.append(this->id); this->ids.actuators[i] = @@ -145,8 +131,8 @@ void SimRobot::set_joint_position(const common::VectorXd& q) { } common::VectorXd SimRobot::get_joint_position() { - common::VectorXd q(std::size(model_names.joints)); - for (size_t i = 0; i < std::size(model_names.joints); ++i) { + common::VectorXd q(std::size(this->cfg.joints)); + for (size_t i = 0; i < std::size(this->cfg.joints); ++i) { q[i] = this->sim->d->qpos[this->sim->m->jnt_qposadr[this->ids.joints[i]]]; } return q; diff --git a/src/sim/SimRobot.h b/src/sim/SimRobot.h index e7daf958..1ded5473 100644 --- a/src/sim/SimRobot.h +++ b/src/sim/SimRobot.h @@ -19,6 +19,18 @@ struct SimRobotConfig : common::RobotConfig { bool realtime = false; bool trajectory_trace = false; common::RobotType robot_type = common::RobotType::FR3; + std::vector arm_collision_geoms{ + "fr3_link0_collision", "fr3_link1_collision", "fr3_link2_collision", + "fr3_link3_collision", "fr3_link4_collision", "fr3_link5_collision", + "fr3_link6_collision", "fr3_link7_collision"}; + std::vector joints = { + "fr3_joint1", "fr3_joint2", "fr3_joint3", "fr3_joint4", + "fr3_joint5", "fr3_joint6", "fr3_joint7", + }; + std::vector actuators = { + "fr3_joint1", "fr3_joint2", "fr3_joint3", "fr3_joint4", + "fr3_joint5", "fr3_joint6", "fr3_joint7", + }; }; struct SimRobotState : common::RobotState { From eee1dca288b19ea92ebe7da57015f16c41d4382d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Tue, 20 May 2025 10:59:56 +0200 Subject: [PATCH 2/4] refactor(robot ids): ids must match in config - added attachment site to config - pushed robot id up the stack: names must match in the config when they arrive at the cpp level, thus the python program needs to manage ids etc to differentiate certain types of robots in the sim --- src/pybind/rcs.cpp | 7 +++++-- src/sim/SimRobot.cpp | 15 ++++----------- src/sim/SimRobot.h | 6 +++--- 3 files changed, 12 insertions(+), 16 deletions(-) diff --git a/src/pybind/rcs.cpp b/src/pybind/rcs.cpp index 76e6e888..13b72031 100644 --- a/src/pybind/rcs.cpp +++ b/src/pybind/rcs.cpp @@ -418,6 +418,8 @@ PYBIND11_MODULE(_core, m) { .def_readwrite("arm_collision_geoms", &rcs::sim::SimRobotConfig::arm_collision_geoms) .def_readwrite("joints", &rcs::sim::SimRobotConfig::joints) + .def_readwrite("attachment_site", + &rcs::sim::SimRobotConfig::attachment_site) .def_readwrite("actuators", &rcs::sim::SimRobotConfig::actuators); py::class_(sim, "SimRobotState") @@ -483,8 +485,9 @@ PYBIND11_MODULE(_core, m) { py::arg("cfg")); py::class_>(sim, "SimRobot") - .def(py::init, const std::string &, - std::shared_ptr, bool>(), + .def(py::init, + std::shared_ptr, + std::shared_ptr, bool>(), py::arg("sim"), py::arg("id"), py::arg("ik"), py::arg("register_convergence_callback") = true) .def("get_parameters", &rcs::sim::SimRobot::get_parameters) diff --git a/src/sim/SimRobot.cpp b/src/sim/SimRobot.cpp index 9488ca79..2b7a8464 100644 --- a/src/sim/SimRobot.cpp +++ b/src/sim/SimRobot.cpp @@ -24,10 +24,10 @@ namespace sim { // TODO: check dof contraints // TODO: use C++11 feature to call one constructor from another -SimRobot::SimRobot(std::shared_ptr sim, const std::string& id, - std::shared_ptr ik, +SimRobot::SimRobot(std::shared_ptr sim, std::shared_ptr ik, + std::shared_ptr cfg, bool register_convergence_callback) - : sim{sim}, id{id}, cfg{}, state{}, m_ik(ik) { + : sim{sim}, cfg{cfg}, state{}, m_ik(ik) { this->init_ids(); if (register_convergence_callback) { this->sim->register_cb(std::bind(&SimRobot::is_arrived_callback, this), @@ -54,8 +54,6 @@ void SimRobot::init_ids() { // Collision geoms for (size_t i = 0; i < std::size(this->cfg.arm_collision_geoms); ++i) { name = this->cfg.arm_collision_geoms[i]; - name.append("_"); - name.append(this->id); int id = mj_name2id(this->sim->m, mjOBJ_GEOM, name.c_str()); if (id == -1) { throw std::runtime_error(std::string("No geom named " + name)); @@ -63,8 +61,7 @@ void SimRobot::init_ids() { this->ids.cgeom.insert(id); } // Sites - name = "attachment_site_"; - name.append(this->id); + name = this->cfg.attachment_site; this->ids.attachment_site = mj_name2id(this->sim->m, mjOBJ_SITE, name.c_str()); if (this->ids.attachment_site == -1) { @@ -73,8 +70,6 @@ void SimRobot::init_ids() { // Joints for (size_t i = 0; i < std::size(this->cfg.joints); ++i) { name = this->cfg.joints[i]; - name.append("_"); - name.append(this->id); this->ids.joints[i] = mj_name2id(this->sim->m, mjOBJ_JOINT, name.c_str()); if (this->ids.joints[i] == -1) { throw std::runtime_error(std::string("No joint named " + name)); @@ -83,8 +78,6 @@ void SimRobot::init_ids() { // Actuators for (size_t i = 0; i < std::size(this->cfg.actuators); ++i) { name = this->cfg.actuators[i]; - name.append("_"); - name.append(this->id); this->ids.actuators[i] = mj_name2id(this->sim->m, mjOBJ_ACTUATOR, name.c_str()); if (this->ids.actuators[i] == -1) { diff --git a/src/sim/SimRobot.h b/src/sim/SimRobot.h index 1ded5473..e665c123 100644 --- a/src/sim/SimRobot.h +++ b/src/sim/SimRobot.h @@ -31,6 +31,7 @@ struct SimRobotConfig : common::RobotConfig { "fr3_joint1", "fr3_joint2", "fr3_joint3", "fr3_joint4", "fr3_joint5", "fr3_joint6", "fr3_joint7", }; + std::string attachment_site = "attachment_site"; }; struct SimRobotState : common::RobotState { @@ -45,8 +46,8 @@ struct SimRobotState : common::RobotState { class SimRobot : public common::Robot { public: - SimRobot(std::shared_ptr sim, const std::string &id, - std::shared_ptr ik, + SimRobot(std::shared_ptr sim, std::shared_ptr ik, + std::shared_ptr cfg, bool register_convergence_callback = true); ~SimRobot() override; bool set_parameters(const SimRobotConfig &cfg); @@ -66,7 +67,6 @@ class SimRobot : public common::Robot { SimRobotConfig cfg; SimRobotState state; std::shared_ptr sim; - std::string id; std::shared_ptr m_ik; struct { std::set cgeom; From 7505f96663e63c128dcba6bac26f7e2338a236c0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Tue, 20 May 2025 12:16:59 +0200 Subject: [PATCH 3/4] refactor(robot ids): move ids to config - added base id to config struct and added it to init_ids - fixed order of constructor args in pybind - added id to default config factory - updated bindings - updated sim robot creation code to include ids --- python/examples/fr3.py | 8 +++----- python/rcs/_core/sim.pyi | 13 ++++++++++++- python/rcs/envs/creators.py | 3 +-- python/rcs/envs/sim.py | 9 +++++---- python/rcs/envs/utils.py | 9 ++++++++- src/pybind/rcs.cpp | 9 +++++---- src/sim/SimRobot.cpp | 18 +++++++++++------- src/sim/SimRobot.h | 5 +++-- 8 files changed, 48 insertions(+), 26 deletions(-) diff --git a/python/examples/fr3.py b/python/examples/fr3.py index 40ad0a0e..af4def1d 100644 --- a/python/examples/fr3.py +++ b/python/examples/fr3.py @@ -10,6 +10,7 @@ from rcs.camera.sim import SimCameraConfig, SimCameraSet, SimCameraSetConfig from rcs.control.fr3_desk import FCI, ContextManager, Desk, load_creds_fr3_desk from rcs.envs.creators import get_urdf_path +from rcs.envs.utils import default_fr3_sim_robot_cfg ROBOT_IP = "192.168.101.1" ROBOT_INSTANCE = RobotPlatform.SIMULATION @@ -69,11 +70,8 @@ def main(): urdf_path = get_urdf_path(URDF_PATH, allow_none_if_not_found=False) assert urdf_path is not None ik = rcs.common.IK(urdf_path) - robot = rcs.sim.SimRobot(simulation, "0", ik) - cfg = sim.SimRobotConfig() - cfg.tcp_offset = rcs.common.Pose(rcs.common.FrankaHandTCPOffset()) - cfg.realtime = False - robot.set_parameters(cfg) + cfg = default_fr3_sim_robot_cfg(urdf_path) + robot = rcs.sim.SimRobot(simulation, ik, cfg) gripper_cfg_sim = sim.SimGripperConfig() gripper = sim.SimGripper(simulation, "0", gripper_cfg_sim) diff --git a/python/rcs/_core/sim.pyi b/python/rcs/_core/sim.pyi index 0da44b92..9c355520 100644 --- a/python/rcs/_core/sim.pyi +++ b/python/rcs/_core/sim.pyi @@ -119,9 +119,14 @@ class SimGripper(rcs._core.common.Gripper): def set_parameters(self, cfg: SimGripperConfig) -> bool: ... class SimGripperConfig(rcs._core.common.GripperConfig): + actuator: str + collision_geoms: list[str] + collision_geoms_fingers: list[str] epsilon_inner: float epsilon_outer: float ignored_collision_geoms: list[str] + joint1: str + joint2: str seconds_between_callbacks: float def __init__(self) -> None: ... @@ -140,7 +145,7 @@ class SimGripperState(rcs._core.common.GripperState): class SimRobot(rcs._core.common.Robot): def __init__( - self, sim: Sim, id: str, ik: rcs._core.common.IK, register_convergence_callback: bool = True + self, sim: Sim, ik: rcs._core.common.IK, cfg: SimRobotConfig, register_convergence_callback: bool = True ) -> None: ... def get_parameters(self) -> SimRobotConfig: ... def get_state(self) -> SimRobotState: ... @@ -148,8 +153,14 @@ class SimRobot(rcs._core.common.Robot): def set_parameters(self, cfg: SimRobotConfig) -> bool: ... class SimRobotConfig(rcs._core.common.RobotConfig): + actuators: list[str] + arm_collision_geoms: list[str] + attachment_site: str + base: str joint_rotational_tolerance: float + joints: list[str] realtime: bool + robot_type: rcs._core.common.RobotType seconds_between_callbacks: float tcp_offset: rcs._core.common.Pose trajectory_trace: bool diff --git a/python/rcs/envs/creators.py b/python/rcs/envs/creators.py index 84339ccb..c765a77e 100644 --- a/python/rcs/envs/creators.py +++ b/python/rcs/envs/creators.py @@ -190,8 +190,7 @@ def __call__( # type: ignore simulation = sim.Sim(mjb_file) ik = rcs.common.IK(urdf_path) - robot = rcs.sim.SimRobot(simulation, "0", ik) - robot.set_parameters(robot_cfg) + robot = rcs.sim.SimRobot(simulation, ik, robot_cfg) env: gym.Env = RobotEnv(robot, control_mode) env = FR3Sim(env, simulation, sim_wrapper) diff --git a/python/rcs/envs/sim.py b/python/rcs/envs/sim.py index eae055b6..c168c2ab 100644 --- a/python/rcs/envs/sim.py +++ b/python/rcs/envs/sim.py @@ -158,15 +158,15 @@ def env_from_xml_paths( sim_gui: bool = True, truncate_on_collision: bool = True, ) -> "CollisionGuard": + # TODO: this needs to support non FR3 robots assert isinstance(env.unwrapped, RobotEnv) simulation = sim.Sim(mjmld) ik = rcs.common.IK(urdf, max_duration_ms=300) - robot = rcs.sim.SimRobot(simulation, id, ik) - cfg = sim.SimRobotConfig() + cfg = default_fr3_sim_robot_cfg(mjmld, id) cfg.realtime = False if tcp_offset is not None: cfg.tcp_offset = tcp_offset - robot.set_parameters(cfg) + robot = rcs.sim.SimRobot(simulation, ik, cfg) to_joint_control = False if control_mode is not None: if control_mode != env.unwrapped.get_control_mode(): @@ -250,8 +250,9 @@ def __init__(self, env, cam_robot_joints: VecType, scene: str = "lab_simple_pick self.unwrapped: RobotEnv assert isinstance(self.unwrapped.robot, sim.SimRobot), "Robot must be a sim.SimRobot instance." self.sim = env.get_wrapper_attr("sim") + cfg = default_fr3_sim_robot_cfg(scene, "1") self.cam_robot = rcs.sim.SimRobot( - self.sim, "1", env.unwrapped.robot.get_ik(), register_convergence_callback=False + self.sim, env.unwrapped.robot.get_ik(), cfg, register_convergence_callback=False ) self.cam_robot.set_parameters(default_fr3_sim_robot_cfg(scene)) self.cam_robot_joints = cam_robot_joints diff --git a/python/rcs/envs/utils.py b/python/rcs/envs/utils.py index 47782146..3fc6efd2 100644 --- a/python/rcs/envs/utils.py +++ b/python/rcs/envs/utils.py @@ -1,3 +1,4 @@ +import copy import logging from os import PathLike from pathlib import Path @@ -17,10 +18,16 @@ logger.setLevel(logging.INFO) -def default_fr3_sim_robot_cfg(mjcf: str | Path = "fr3_empty_world") -> sim.SimRobotConfig: +def default_fr3_sim_robot_cfg(mjcf: str | Path = "fr3_empty_world", idx: str = "0") -> sim.SimRobotConfig: cfg = sim.SimRobotConfig() cfg.tcp_offset = get_tcp_offset(mjcf) cfg.realtime = False + cfg.robot_type = rcs.common.RobotType.FR3 + cfg.arm_collision_geoms = [f"{id}_{idx}" for id in copy.deepcopy(cfg.arm_collision_geoms)] + cfg.joints = [f"{id}_{idx}" for id in copy.deepcopy(cfg.joints)] + cfg.actuators = [f"{id}_{idx}" for id in copy.deepcopy(cfg.actuators)] + cfg.attachment_site = f"attachment_site_{idx}" + cfg.base = f"base_{idx}" return cfg diff --git a/src/pybind/rcs.cpp b/src/pybind/rcs.cpp index 13b72031..b14e4e88 100644 --- a/src/pybind/rcs.cpp +++ b/src/pybind/rcs.cpp @@ -420,7 +420,8 @@ PYBIND11_MODULE(_core, m) { .def_readwrite("joints", &rcs::sim::SimRobotConfig::joints) .def_readwrite("attachment_site", &rcs::sim::SimRobotConfig::attachment_site) - .def_readwrite("actuators", &rcs::sim::SimRobotConfig::actuators); + .def_readwrite("actuators", &rcs::sim::SimRobotConfig::actuators) + .def_readwrite("base", &rcs::sim::SimRobotConfig::base); py::class_(sim, "SimRobotState") .def(py::init<>()) @@ -486,9 +487,9 @@ PYBIND11_MODULE(_core, m) { py::class_>(sim, "SimRobot") .def(py::init, - std::shared_ptr, - std::shared_ptr, bool>(), - py::arg("sim"), py::arg("id"), py::arg("ik"), + std::shared_ptr, rcs::sim::SimRobotConfig, + bool>(), + py::arg("sim"), py::arg("ik"), py::arg("cfg"), py::arg("register_convergence_callback") = true) .def("get_parameters", &rcs::sim::SimRobot::get_parameters) .def("set_parameters", &rcs::sim::SimRobot::set_parameters, diff --git a/src/sim/SimRobot.cpp b/src/sim/SimRobot.cpp index 2b7a8464..50ac7817 100644 --- a/src/sim/SimRobot.cpp +++ b/src/sim/SimRobot.cpp @@ -25,8 +25,7 @@ namespace sim { // TODO: check dof contraints // TODO: use C++11 feature to call one constructor from another SimRobot::SimRobot(std::shared_ptr sim, std::shared_ptr ik, - std::shared_ptr cfg, - bool register_convergence_callback) + SimRobotConfig cfg, bool register_convergence_callback) : sim{sim}, cfg{cfg}, state{}, m_ik(ik) { this->init_ids(); if (register_convergence_callback) { @@ -60,13 +59,19 @@ void SimRobot::init_ids() { } this->ids.cgeom.insert(id); } - // Sites + // Attachment Site name = this->cfg.attachment_site; this->ids.attachment_site = mj_name2id(this->sim->m, mjOBJ_SITE, name.c_str()); if (this->ids.attachment_site == -1) { throw std::runtime_error(std::string("No site named " + name)); } + // Base + name = this->cfg.base; + this->ids.base = mj_name2id(this->sim->m, mjOBJ_BODY, name.c_str()); + if (this->ids.base == -1) { + throw std::runtime_error(std::string("No body named " + name)); + } // Joints for (size_t i = 0; i < std::size(this->cfg.joints); ++i) { name = this->cfg.joints[i]; @@ -198,10 +203,9 @@ void SimRobot::set_joints_hard(const common::VectorXd& q) { } common::Pose SimRobot::get_base_pose_in_world_coordinates() { - auto id = mj_name2id(this->sim->m, mjOBJ_BODY, - (std::string("base_") + this->id).c_str()); - Eigen::Map translation(this->sim->d->xpos + 3 * id); - auto quat = this->sim->d->xquat + 4 * id; + Eigen::Map translation(this->sim->d->xpos + + 3 * this->ids.base); + auto quat = this->sim->d->xquat + 4 * this->ids.base; Eigen::Quaterniond rotation(quat[0], quat[1], quat[2], quat[3]); return common::Pose(rotation, translation); } diff --git a/src/sim/SimRobot.h b/src/sim/SimRobot.h index e665c123..dbb632af 100644 --- a/src/sim/SimRobot.h +++ b/src/sim/SimRobot.h @@ -32,6 +32,7 @@ struct SimRobotConfig : common::RobotConfig { "fr3_joint5", "fr3_joint6", "fr3_joint7", }; std::string attachment_site = "attachment_site"; + std::string base = "base"; }; struct SimRobotState : common::RobotState { @@ -47,8 +48,7 @@ struct SimRobotState : common::RobotState { class SimRobot : public common::Robot { public: SimRobot(std::shared_ptr sim, std::shared_ptr ik, - std::shared_ptr cfg, - bool register_convergence_callback = true); + SimRobotConfig cfg, bool register_convergence_callback = true); ~SimRobot() override; bool set_parameters(const SimRobotConfig &cfg); SimRobotConfig *get_parameters() override; @@ -74,6 +74,7 @@ class SimRobot : public common::Robot { std::array joints; std::array ctrl; std::array actuators; + int base; } ids; void is_moving_callback(); void is_arrived_callback(); From ac9bf2bd530604de2e7d3bddbd08509a3282a89a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Tue, 20 May 2025 13:38:44 +0200 Subject: [PATCH 4/4] refactor(gripper): id fill method and gripper with id - added ids to gripper - added add id methods in cpp and created bindings - usage of the above mentioned method --- python/examples/fr3.py | 8 +++++--- python/rcs/_core/sim.pyi | 4 +++- python/rcs/envs/creators.py | 2 +- python/rcs/envs/sim.py | 3 ++- python/rcs/envs/utils.py | 21 +++++++++------------ src/pybind/rcs.cpp | 10 ++++++---- src/sim/SimGripper.cpp | 15 +++++++-------- src/sim/SimGripper.h | 19 ++++++++++++++++--- src/sim/SimRobot.h | 14 ++++++++++++++ src/sim/test.cpp | 6 +++--- 10 files changed, 66 insertions(+), 36 deletions(-) diff --git a/python/examples/fr3.py b/python/examples/fr3.py index af4def1d..2d9b8eee 100644 --- a/python/examples/fr3.py +++ b/python/examples/fr3.py @@ -10,7 +10,6 @@ from rcs.camera.sim import SimCameraConfig, SimCameraSet, SimCameraSetConfig from rcs.control.fr3_desk import FCI, ContextManager, Desk, load_creds_fr3_desk from rcs.envs.creators import get_urdf_path -from rcs.envs.utils import default_fr3_sim_robot_cfg ROBOT_IP = "192.168.101.1" ROBOT_INSTANCE = RobotPlatform.SIMULATION @@ -70,11 +69,14 @@ def main(): urdf_path = get_urdf_path(URDF_PATH, allow_none_if_not_found=False) assert urdf_path is not None ik = rcs.common.IK(urdf_path) - cfg = default_fr3_sim_robot_cfg(urdf_path) + cfg = sim.SimRobotConfig() + cfg.add_id("0") + cfg.tcp_offset = rcs.common.Pose(rcs.common.FrankaHandTCPOffset()) robot = rcs.sim.SimRobot(simulation, ik, cfg) gripper_cfg_sim = sim.SimGripperConfig() - gripper = sim.SimGripper(simulation, "0", gripper_cfg_sim) + gripper_cfg_sim.add_id("0") + gripper = sim.SimGripper(simulation, gripper_cfg_sim) # add camera to have a rendering gui cameras = { diff --git a/python/rcs/_core/sim.pyi b/python/rcs/_core/sim.pyi index 9c355520..ff64f481 100644 --- a/python/rcs/_core/sim.pyi +++ b/python/rcs/_core/sim.pyi @@ -113,7 +113,7 @@ class SimCameraSetConfig: def frame_rate(self, arg0: int) -> None: ... class SimGripper(rcs._core.common.Gripper): - def __init__(self, sim: Sim, id: str, cfg: SimGripperConfig) -> None: ... + def __init__(self, sim: Sim, cfg: SimGripperConfig) -> None: ... def get_parameters(self) -> SimGripperConfig: ... def get_state(self) -> SimGripperState: ... def set_parameters(self, cfg: SimGripperConfig) -> bool: ... @@ -129,6 +129,7 @@ class SimGripperConfig(rcs._core.common.GripperConfig): joint2: str seconds_between_callbacks: float def __init__(self) -> None: ... + def add_id(self, id: str) -> None: ... class SimGripperState(rcs._core.common.GripperState): def __init__(self) -> None: ... @@ -165,6 +166,7 @@ class SimRobotConfig(rcs._core.common.RobotConfig): tcp_offset: rcs._core.common.Pose trajectory_trace: bool def __init__(self) -> None: ... + def add_id(self, id: str) -> None: ... class SimRobotState(rcs._core.common.RobotState): def __init__(self) -> None: ... diff --git a/python/rcs/envs/creators.py b/python/rcs/envs/creators.py index c765a77e..38aa2784 100644 --- a/python/rcs/envs/creators.py +++ b/python/rcs/envs/creators.py @@ -199,7 +199,7 @@ def __call__( # type: ignore env = CameraSetWrapper(env, camera_set, include_depth=True) if gripper_cfg is not None and isinstance(gripper_cfg, rcs.sim.SimGripperConfig): - gripper = sim.SimGripper(simulation, "0", gripper_cfg) + gripper = sim.SimGripper(simulation, gripper_cfg) env = GripperWrapper(env, gripper, binary=True) env = GripperWrapperSim(env, gripper) diff --git a/python/rcs/envs/sim.py b/python/rcs/envs/sim.py index c168c2ab..02e64b7d 100644 --- a/python/rcs/envs/sim.py +++ b/python/rcs/envs/sim.py @@ -181,7 +181,8 @@ def env_from_xml_paths( c_env = FR3Sim(c_env, simulation) if gripper: gripper_cfg = sim.SimGripperConfig() - fh = sim.SimGripper(simulation, id, gripper_cfg) + gripper_cfg.add_id(id) + fh = sim.SimGripper(simulation, gripper_cfg) c_env = GripperWrapper(c_env, fh) c_env = GripperWrapperSim(c_env, fh) return cls( diff --git a/python/rcs/envs/utils.py b/python/rcs/envs/utils.py index 3fc6efd2..fcd61bfa 100644 --- a/python/rcs/envs/utils.py +++ b/python/rcs/envs/utils.py @@ -1,4 +1,3 @@ -import copy import logging from os import PathLike from pathlib import Path @@ -23,15 +22,11 @@ def default_fr3_sim_robot_cfg(mjcf: str | Path = "fr3_empty_world", idx: str = " cfg.tcp_offset = get_tcp_offset(mjcf) cfg.realtime = False cfg.robot_type = rcs.common.RobotType.FR3 - cfg.arm_collision_geoms = [f"{id}_{idx}" for id in copy.deepcopy(cfg.arm_collision_geoms)] - cfg.joints = [f"{id}_{idx}" for id in copy.deepcopy(cfg.joints)] - cfg.actuators = [f"{id}_{idx}" for id in copy.deepcopy(cfg.actuators)] - cfg.attachment_site = f"attachment_site_{idx}" - cfg.base = f"base_{idx}" + cfg.add_id(idx) return cfg -def default_fr3_hw_robot_cfg(async_control: bool = False): +def default_fr3_hw_robot_cfg(async_control: bool = False) -> FR3Config: robot_cfg = FR3Config() robot_cfg.tcp_offset = rcs.common.Pose(rcs.common.FrankaHandTCPOffset()) robot_cfg.speed_factor = 0.1 @@ -40,7 +35,7 @@ def default_fr3_hw_robot_cfg(async_control: bool = False): return robot_cfg -def default_fr3_hw_gripper_cfg(async_control: bool = False): +def default_fr3_hw_gripper_cfg(async_control: bool = False) -> rcs.hw.FHConfig: gripper_cfg = rcs.hw.FHConfig() gripper_cfg.epsilon_inner = gripper_cfg.epsilon_outer = 0.1 gripper_cfg.speed = 0.1 @@ -72,11 +67,13 @@ def default_realsense(name2id: dict[str, str] | None) -> RealSenseCameraSet | No return RealSenseCameraSet(cam_cfg) -def default_fr3_sim_gripper_cfg(): - return sim.SimGripperConfig() +def default_fr3_sim_gripper_cfg(idx: str = "0") -> sim.SimGripperConfig: + cfg = sim.SimGripperConfig() + cfg.add_id(idx) + return cfg -def default_mujoco_cameraset_cfg(): +def default_mujoco_cameraset_cfg() -> SimCameraSetConfig: cameras = { "wrist": SimCameraConfig(identifier="wrist_0", type=int(CameraType.fixed)), "default_free": SimCameraConfig(identifier="", type=int(CameraType.default_free)), @@ -88,7 +85,7 @@ def default_mujoco_cameraset_cfg(): ) -def get_tcp_offset(mjcf: str | Path): +def get_tcp_offset(mjcf: str | Path) -> rcs.common.Pose: """Reads out tcp offset set in mjcf file. Convention: The tcp offset is stored in the model as a numeric attribute named "tcp_offset". diff --git a/src/pybind/rcs.cpp b/src/pybind/rcs.cpp index b14e4e88..0514393c 100644 --- a/src/pybind/rcs.cpp +++ b/src/pybind/rcs.cpp @@ -421,7 +421,8 @@ PYBIND11_MODULE(_core, m) { .def_readwrite("attachment_site", &rcs::sim::SimRobotConfig::attachment_site) .def_readwrite("actuators", &rcs::sim::SimRobotConfig::actuators) - .def_readwrite("base", &rcs::sim::SimRobotConfig::base); + .def_readwrite("base", &rcs::sim::SimRobotConfig::base) + .def("add_id", &rcs::sim::SimRobotConfig::add_id, py::arg("id")); py::class_(sim, "SimRobotState") .def(py::init<>()) @@ -451,7 +452,8 @@ PYBIND11_MODULE(_core, m) { &rcs::sim::SimGripperConfig::collision_geoms_fingers) .def_readwrite("joint1", &rcs::sim::SimGripperConfig::joint1) .def_readwrite("joint2", &rcs::sim::SimGripperConfig::joint2) - .def_readwrite("actuator", &rcs::sim::SimGripperConfig::actuator); + .def_readwrite("actuator", &rcs::sim::SimGripperConfig::actuator) + .def("add_id", &rcs::sim::SimGripperConfig::add_id, py::arg("id")); py::class_( sim, "SimGripperState") .def(py::init<>()) @@ -477,9 +479,9 @@ PYBIND11_MODULE(_core, m) { .def("_stop_gui_server", &rcs::sim::Sim::stop_gui_server); py::class_>(sim, "SimGripper") - .def(py::init, const std::string &, + .def(py::init, const rcs::sim::SimGripperConfig &>(), - py::arg("sim"), py::arg("id"), py::arg("cfg")) + py::arg("sim"), py::arg("cfg")) .def("get_parameters", &rcs::sim::SimGripper::get_parameters) .def("get_state", &rcs::sim::SimGripper::get_state) .def("set_parameters", &rcs::sim::SimGripper::set_parameters, diff --git a/src/sim/SimGripper.cpp b/src/sim/SimGripper.cpp index d9b64fac..0985cac4 100644 --- a/src/sim/SimGripper.cpp +++ b/src/sim/SimGripper.cpp @@ -10,12 +10,11 @@ namespace rcs { namespace sim { -SimGripper::SimGripper(std::shared_ptr sim, const std::string &id, - const SimGripperConfig &cfg) - : sim{sim}, cfg{cfg}, id{id} { +SimGripper::SimGripper(std::shared_ptr sim, const SimGripperConfig &cfg) + : sim{sim}, cfg{cfg} { this->state = SimGripperState(); - this->actuator_id = mj_name2id(this->sim->m, mjOBJ_ACTUATOR, - (this->cfg.actuator + "_" + id).c_str()); + this->actuator_id = + mj_name2id(this->sim->m, mjOBJ_ACTUATOR, this->cfg.actuator.c_str()); if (this->actuator_id == -1) { throw std::runtime_error( std::string("No actuator named " + this->cfg.actuator)); @@ -23,12 +22,12 @@ SimGripper::SimGripper(std::shared_ptr sim, const std::string &id, // 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 + "_" + id).c_str())]; + 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 + "_" + id).c_str())]; + 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)); } @@ -53,7 +52,7 @@ void SimGripper::add_collision_geoms(const std::vector &cgeoms_str, cgeoms_set.clear(); } for (size_t i = 0; i < std::size(cgeoms_str); ++i) { - std::string name = cgeoms_str[i] + "_" + id; + std::string name = cgeoms_str[i]; int coll_id = mj_name2id(this->sim->m, mjOBJ_GEOM, name.c_str()); if (coll_id == -1) { diff --git a/src/sim/SimGripper.h b/src/sim/SimGripper.h index accedc8e..c01ba86c 100644 --- a/src/sim/SimGripper.h +++ b/src/sim/SimGripper.h @@ -26,6 +26,21 @@ struct SimGripperConfig : common::GripperConfig { std::string joint1 = "finger_joint1"; std::string joint2 = "finger_joint2"; std::string actuator = "actuator8"; + + void add_id(const std::string &id) { + for (auto &s : this->collision_geoms) { + s = s + "_" + id; + } + for (auto &s : this->collision_geoms_fingers) { + s = s + "_" + id; + } + for (auto &s : this->ignored_collision_geoms) { + s = s + "_" + id; + } + this->joint1 = this->joint1 + "_" + id; + this->joint2 = this->joint2 + "_" + id; + this->actuator = this->actuator + "_" + id; + } }; struct SimGripperState : common::GripperState { @@ -46,7 +61,6 @@ class SimGripper : public common::Gripper { int joint_id_1; int joint_id_2; SimGripperState state; - std::string id; bool convergence_callback(); bool collision_callback(); std::set cgeom; @@ -57,8 +71,7 @@ class SimGripper : public common::Gripper { void m_reset(); public: - SimGripper(std::shared_ptr sim, const std::string &id, - const SimGripperConfig &cfg); + SimGripper(std::shared_ptr sim, const SimGripperConfig &cfg); ~SimGripper() override; bool set_parameters(const SimGripperConfig &cfg); diff --git a/src/sim/SimRobot.h b/src/sim/SimRobot.h index dbb632af..1c286a09 100644 --- a/src/sim/SimRobot.h +++ b/src/sim/SimRobot.h @@ -33,6 +33,20 @@ struct SimRobotConfig : common::RobotConfig { }; std::string attachment_site = "attachment_site"; std::string base = "base"; + + void add_id(const std::string &id) { + for (auto &s : this->arm_collision_geoms) { + s = s + "_" + id; + } + for (auto &s : this->joints) { + s = s + "_" + id; + } + for (auto &s : this->actuators) { + s = s + "_" + id; + } + this->attachment_site = this->attachment_site + "_" + id; + this->base = this->base + "_" + id; + } }; struct SimRobotState : common::RobotState { diff --git a/src/sim/test.cpp b/src/sim/test.cpp index c09e7eb2..5e7995af 100644 --- a/src/sim/test.cpp +++ b/src/sim/test.cpp @@ -127,12 +127,12 @@ int test_sim() { std::string id = "0"; auto ik = std::make_shared(urdf); - auto fr3 = rcs::sim::SimRobot(sim, id, ik); auto tcp_offset = rcs::common::Pose(rcs::common::FrankaHandTCPOffset()); - rcs::sim::SimRobotConfig fr3_config = *fr3.get_parameters(); + rcs::sim::SimRobotConfig fr3_config; fr3_config.tcp_offset = tcp_offset; fr3_config.seconds_between_callbacks = 0.05; // 20hz - fr3.set_parameters(fr3_config); + fr3_config.add_id(id); + auto fr3 = rcs::sim::SimRobot(sim, ik, fr3_config); std::jthread t(rendering_loop, m, d); sim->step(1); for (size_t i = 0; i < 100; ++i) {