ArmConfig: add Cartesian control limits, eliminate magic numbers (#153) (#155)

The arm declares its own dynamic capabilities — TeleopController and
servo primitives read from arm.config instead of hardcoded constants.

- ArmConfig: add max_cartesian_speed, max_cartesian_angular,
  reactive_gain (used by _step_pose_as_twist)
- UR5e: 0.3 m/s, 1.0 rad/s (industrial)
- Unify reactive lookahead: use config.lookahead_time instead of
  2*control_dt (matches trajectory execution feedforward)

JACO2 values set in ada_mj separately (0.06 m/s, 0.3 rad/s —
conservative for human-proximate operation).

Closes #153

Author: siddhss5

src/mj_manipulator/arms/ur5e.py

@@ -134,6 +134,8 @@ def create_ur5e_arm(
         ),
         ee_site=ee_site,
         tcp_offset=tcp_offset,
+        max_cartesian_speed=0.3,  # industrial arm, faster
+        max_cartesian_angular=1.0,
     )
 
     arm = Arm(env, config)

src/mj_manipulator/config.py

@@ -64,10 +64,15 @@ class ArmConfig(EntityConfig):
     """Configuration for a single arm.
 
     Gripper-specific fields (body names, actuator, hand type) belong on the
-    Gripper protocol, not here. ArmConfig defines the kinematic chain only.
+    Gripper protocol, not here. ArmConfig defines the kinematic chain and
+    the arm's dynamic capabilities.
 
     kinematic_limits is required and robot-specific. Use your robot's
     datasheet values (e.g., KinematicLimits for UR5e, Franka, Xarm).
+
+    Cartesian control parameters (max_cartesian_speed, max_cartesian_angular,
+    reactive_gain) are used by TeleopController and servo primitives. Set
+    these based on the arm's workspace, PD dynamics, and safety requirements.
     """
 
     kinematic_limits: KinematicLimits  # required: robot-specific velocity/acceleration limits
@@ -78,6 +83,20 @@ class ArmConfig(EntityConfig):
     extra_arm_body_names: list[str] | None = None  # Additional bodies to treat as part of arm for collision
     planning_defaults: PlanningDefaults = field(default_factory=PlanningDefaults)
 
+    # Cartesian control limits — the arm declares what it can do.
+    # TeleopController and servo primitives read these.
+    max_cartesian_speed: float = 0.2
+    """Maximum EE linear speed (m/s) for reactive control (teleop, servo).
+    Set conservatively for arms near humans (e.g., 0.06 for feeding).
+    Enforced via Jacobian-based speed checking in _check_and_commit."""
+
+    max_cartesian_angular: float = 0.5
+    """Maximum EE angular speed (rad/s) for reactive control."""
+
+    reactive_gain: float = 1.0
+    """Proportional gain for pose error → twist conversion in teleop.
+    Higher = faster response to gizmo movement, but may overshoot."""
+
     def __post_init__(self):
         """Set entity_type to arm."""
         object.__setattr__(self, "entity_type", "arm")

src/mj_manipulator/physics_controller.py

@@ -180,7 +180,7 @@ def step_reactive(
         )
 
         # Reactive arm: small lookahead
-        reactive_lookahead = 2.0 * self.control_dt
+        reactive_lookahead = self.config.lookahead_time
         q_cmd = state.target_position + reactive_lookahead * state.target_velocity
         self.data.ctrl[state.actuator_ids] = q_cmd
 

src/mj_manipulator/sim_context.py

@@ -657,7 +657,7 @@ def _set_reactive_target(self, arm_name: str, position: np.ndarray, velocity: np
         state.target_velocity = (
             np.asarray(velocity).copy() if velocity is not None else np.zeros(len(state.actuator_ids))
         )
-        state.lookahead = 2.0 * self._controller.control_dt
+        state.lookahead = self._controller.config.lookahead_time
 
     def _step_cartesian_impl(self, arm_name: str, position: np.ndarray, velocity: np.ndarray | None) -> None:
         if self._controller is not None:

src/mj_manipulator/teleop.py

@@ -610,9 +610,9 @@ def _step_pose_as_twist(self, target_pose: np.ndarray) -> TeleopState:
             ) / (2 * np.sin(angle))
             rot_err = axis * angle
 
-        gain = 1.0
-        max_linear = 0.2
-        max_angular = 0.5
+        gain = self._arm.config.reactive_gain
+        max_linear = self._arm.config.max_cartesian_speed
+        max_angular = self._arm.config.max_cartesian_angular
 
         linear_vel = np.clip(gain * pos_err, -max_linear, max_linear)
         angular_vel = np.clip(gain * rot_err, -max_angular, max_angular)

tests/test_physics_controller.py

@@ -103,9 +103,9 @@ def test_step_reactive(self, controller, model_and_data):
             np.array([0.5, -0.5]),
             np.array([0.1, 0.1]),
         )
-        # Actuators should have been commanded with reactive lookahead
-        # cmd = 0.5 + 2*0.002*0.1 = 0.5004
-        expected = 0.5 + 2.0 * controller.control_dt * 0.1
+        # Actuators should have been commanded with lookahead_time feedforward
+        # cmd = 0.5 + lookahead_time * 0.1
+        expected = 0.5 + controller.config.lookahead_time * 0.1
         assert abs(data.ctrl[controller._arms["test_arm"].actuator_ids[0]] - expected) < 1e-6
 
     def test_step_reactive_unknown_raises(self, controller):