fix(gripper): joint value calculation

- so101 min/max joint values in radians
- fixed width calculation in normalized gripper
- added header files in include to linting and formatting

Author: juelg

Makefile

@@ -5,14 +5,17 @@ COMPILE_MODE = Release
 # CPP
 cppcheckformat:
 	clang-format --dry-run -Werror -i $(shell find ${CPPSRC} -name '*.cpp' -o -name '*.cc' -o -name '*.h')
+	clang-format --dry-run -Werror -i $(shell find include -name '*.cpp' -o -name '*.cc' -o -name '*.h')
 	clang-format --dry-run -Werror -i $(shell find extensions/rcs_fr3/src -name '*.cpp' -o -name '*.cc' -o -name '*.h')
 
 cppformat:
 	clang-format -Werror -i $(shell find ${CPPSRC} -name '*.cpp' -o -name '*.cc' -o -name '*.h')
+	clang-format -Werror -i $(shell find include -name '*.cpp' -o -name '*.cc' -o -name '*.h')
 	clang-format -Werror -i $(shell find extensions/rcs_fr3/src -name '*.cpp' -o -name '*.cc' -o -name '*.h')
 
 cpplint: 
 	clang-tidy -p=build --warnings-as-errors='*' $(shell find ${CPPSRC} -name '*.cpp' -o -name '*.cc' -name '*.h')
+	clang-tidy -p=build --warnings-as-errors='*' $(shell find include -name '*.cpp' -o -name '*.cc' -name '*.h')
 
 # import errors
 # clang-tidy -p=build --warnings-as-errors='*' $(shell find extensions/rcs_fr3/src -name '*.cpp' -o -name '*.cc' -name '*.h')

examples/so101_env_joint_control.py

@@ -0,0 +1,59 @@
+import logging
+
+import numpy as np
+from rcs.envs.base import ControlMode, RelativeTo
+from rcs.envs.creators import SimEnvCreator
+
+import rcs
+from rcs import sim
+
+logger = logging.getLogger(__name__)
+logger.setLevel(logging.INFO)
+
+
+def main():
+
+    robot_cfg = sim.SimRobotConfig()
+    robot_cfg.actuators = ["1", "2", "3", "4", "5"]
+    robot_cfg.joints = [
+        "1",
+        "2",
+        "3",
+        "4",
+        "5",
+    ]
+    robot_cfg.base = "base"
+    robot_cfg.robot_type = rcs.common.RobotType.UR5e
+    robot_cfg.attachment_site = "gripper"
+    robot_cfg.arm_collision_geoms = []
+    gripper_cfg = sim.SimGripperConfig
+    gripper_cfg.min_actuator_width = -0.17453292519943295
+    gripper_cfg.max_actuator_width = 1.7453292519943295
+    gripper_cfg.min_joint_width = -0.17453292519943295
+    gripper_cfg.max_joint_width = 1.7453292519943295
+    env_rel = SimEnvCreator()(
+        control_mode=ControlMode.JOINTS,
+        collision_guard=False,
+        robot_cfg=robot_cfg,
+        gripper_cfg=gripper_cfg,
+        # cameras=default_mujoco_cameraset_cfg(),
+        max_relative_movement=np.deg2rad(5),
+        relative_to=RelativeTo.LAST_STEP,
+        mjcf=rcs.scenes["so101_empty_world"]["mjb"],
+        urdf_path=rcs.scenes["so101_empty_world"]["urdf"],
+    )
+    env_rel.get_wrapper_attr("sim").open_gui()
+
+    for _ in range(10):
+        obs, info = env_rel.reset()
+        for _ in range(10):
+            # sample random relative action and execute it
+            act = env_rel.action_space.sample()
+            obs, reward, terminated, truncated, info = env_rel.step(act)
+            if truncated or terminated:
+                logger.info("Truncated or terminated!")
+                return
+
+
+if __name__ == "__main__":
+    main()

include/rcs/Robot.h

@@ -62,18 +62,21 @@ static const std::unordered_map<RobotType, RobotMetaConfig> robots_meta_config =
         // TODO: update this to angle values
        RobotMetaConfig{
            // q_home (5‐vector):
-           (VectorXd(5) << -9.40612320177057, -99.66130397967824,
-            99.9124726477024, 69.96996996996998, -9.095744680851055)
+           (VectorXd(5) << -0.1805846, -1.91335968, 1.56934507, 1.16014604,
+            -0.25400112)
                .finished(),
            // dof:
            5,
            // joint_limits (2×5):
            (Eigen::Matrix<double, 2, Eigen::Dynamic, Eigen::ColMajor>(2, 5) <<
                 // low 5‐tuple
-                -100.0,
-            -100.0, -100.0, -100.0, -100.0,
+                -1.9198621771937616,
+            -1.9198621771937634, -1.7453292519943295, -1.6580627969561903,
+            -2.7925268969992407,
+
             // high 5‐tuple
-            100.0, 100.0, 100.0, 100.0, 100.0)
+            1.9198621771937616, 1.9198621771937634, 1.5707963267948966,
+            1.6580627969561903, 2.7925268969992407)
                .finished()}}}};
 
 struct RobotConfig {

src/sim/SimGripper.cpp

@@ -94,8 +94,8 @@ 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] - this->cfg.min_actuator_width) /
-      (this->cfg.max_joint_width - this->cfg.min_actuator_width);
+      (this->sim->d->qpos[this->joint_id] + this->cfg.min_joint_width) /
+      (this->cfg.max_joint_width + this->cfg.min_joint_width);
   // sometimes the joint is slightly outside of the bounds
   if (width < 0) {
     width = 0;
@@ -159,7 +159,7 @@ void SimGripper::m_reset() {
   this->state = SimGripperState();
   // reset state hard
   this->sim->d->qpos[this->joint_id] = this->cfg.max_joint_width;
-  this->sim->d->ctrl[this->actuator_id] = this->cfg.max_joint_width;
+  this->sim->d->ctrl[this->actuator_id] = this->cfg.max_actuator_width;
 }
 
 void SimGripper::reset() { this->m_reset(); }