Fix: use control timestep parameter in controller

Uses the control timestep of the environment to configure the
Cartesian velocity controller. The latter was previously fixed to 0.1
seconds.

Author: JeanElsner

src/dm_robotics/panda/arm.py

@@ -328,18 +328,21 @@ class Cartesian6dVelocityEffector(
     cartesian_6d_velocity_effector.Cartesian6dVelocityEffector):
   """Panda Version of the MoMa Cartesian6dVelocityEffector."""
 
-  def __init__(self, robot_params: params.RobotParams, arm: robot_arm.RobotArm,
+  def __init__(self,
+               robot_params: params.RobotParams,
+               arm: robot_arm.RobotArm,
                gripper: robot_hand.RobotHand,
                joint_velocity_effector: effector.Effector,
-               tcp_sensor: robot_tcp_sensor.RobotTCPSensor):
+               tcp_sensor: robot_tcp_sensor.RobotTCPSensor,
+               control_timestep: float = 0.1):
     self._frame = robot_params.control_frame
     self._arm = arm
     self._get_world_pos = tcp_sensor.observables[tcp_sensor.get_obs_key(
         robot_tcp_sensor.Observations.POS)]
     model_params = cartesian_6d_velocity_effector.ModelParams(
         gripper.tool_center_point, arm.joints)
     control_params = cartesian_6d_velocity_effector.ControlParams(
-        0.1,
+        control_timestep,
         joint_position_limit_velocity_scale=.95,
         minimum_distance_from_joint_position_limit=.01,
         joint_velocity_limits=np.array(consts.VELOCITY_LIMITS['max']))
@@ -483,7 +486,8 @@ def _joint_torques(self, physics: mjcf.Physics) -> np.ndarray:
             self._arm.joints).qfrc_passive  # pytype: disable=attribute-error
 
 
-def build_robot(robot_params: params.RobotParams) -> robot.Robot:
+def build_robot(robot_params: params.RobotParams,
+                control_timestep: float = 0.1) -> robot.Robot:
   """Builds a MoMa robot model of the Panda."""
   robot_sensors = []
   arm = Panda(actuation=robot_params.actuation, name=robot_params.name)
@@ -520,7 +524,7 @@ def build_robot(robot_params: params.RobotParams) -> robot.Robot:
     joint_velocity_effector = ArmEffector(robot_params, arm)
     _arm_effector = Cartesian6dVelocityEffector(robot_params, arm, gripper,
                                                 joint_velocity_effector,
-                                                tcp_sensor)
+                                                tcp_sensor, control_timestep)
 
   robot.standard_compose(arm, gripper)
   moma_robot = robot.StandardRobot(arm=arm,

src/dm_robotics/panda/environment.py

@@ -37,9 +37,11 @@ def __init__(self,
     self._robots = collections.OrderedDict()
     for robot_params in self._robot_params:
       if robot_params.robot_ip is not None:
-        robot = hardware.build_robot(robot_params)
+        robot = hardware.build_robot(robot_params,
+                                     control_timestep=self._control_timestep)
       else:
-        robot = arm.build_robot(robot_params)
+        robot = arm.build_robot(robot_params,
+                                control_timestep=self._control_timestep)
       self._robots[robot_params.name] = robot
     for robot, robot_params in zip(self._robots.values(), self._robot_params):
       self._arena.attach(robot.arm, robot_params.attach_site)

src/dm_robotics/panda/hardware.py

@@ -210,7 +210,8 @@ def set_control(self, physics: mjcf.Physics, command: np.ndarray) -> None:
     self._command = 0 if command[0] < 0.5 else 0.08
 
 
-def build_robot(robot_params: params.RobotParams) -> robot.Robot:
+def build_robot(robot_params: params.RobotParams,
+                control_timestep: float = 0.1) -> robot.Robot:
   """Builds a MoMa robot model of the Panda with hardware in the loop."""
   hardware_panda = panda_py.Panda(robot_params.robot_ip)
   hardware_panda.set_default_behavior()
@@ -256,7 +257,8 @@ def build_robot(robot_params: params.RobotParams) -> robot.Robot:
   elif robot_params.actuation == arm_constants.Actuation.CARTESIAN_VELOCITY:
     joint_velocity_effector = ArmEffector(robot_params, panda, hardware_panda)
     _arm_effector = arm_module.Cartesian6dVelocityEffector(
-        robot_params, panda, gripper, joint_velocity_effector, tcp_sensor)
+        robot_params, panda, gripper, joint_velocity_effector, tcp_sensor,
+        control_timestep)
 
   robot.standard_compose(panda, gripper)
   moma_robot = robot.StandardRobot(arm=panda,