fix: add moveit controller and gripper sensor

eagerx_interbotix/xseries/real/enginenodes.py

@@ -61,14 +61,16 @@ def make(
             spec.outputs.obs.space.update(low=-1, high=1, shape=[4])
         elif mode == "ee_pose":
             spec.outputs.obs.space.update(shape=[7])
+        elif mode == "gripper_position":
+            spec.outputs.obs.space.update(shape=[1])
         else:
             spec.outputs.obs.space.update(shape=[len(joints)])
 
         return spec
 
     def initialize(self, spec: NodeSpec, simulator: Any):
         self.mode = spec.config.mode
-        if self.mode not in ["position", "velocity", "effort", "ee_position", "ee_orientation", "ee_pose"]:
+        if self.mode not in ["position", "velocity", "effort", "ee_position", "ee_orientation", "ee_pose", "gripper_position"]:
             raise NotImplementedError(f"This mode is not implemented: {spec.config.mode}")
 
         # Get arm client
@@ -106,6 +108,10 @@ def callback(self, t_n: float, tick: Msg):
             rot_matrix, position, _ = self.arm.get_ee_pose()
             raise NotImplementedError("how to convert rotation_matrix into quaternion vector?")
             # obs = np.array(joint_state.effort, dtype="float32")
+        elif self.mode == "gripper_position":
+            gripper_pos = self.arm.get_gripper_state().position
+            # pos, vel, effort = state.position, state.velocity, state.effort
+            obs = np.array(gripper_pos, dtype="float32")
         else:
             raise NotImplementedError(f"This mode is not implemented: {self.mode}")
         return dict(obs=obs)
@@ -254,25 +260,43 @@ def make(
         arm_name: str,
         robot_type: str,
         color: str = "green",
+        pressure: float = 0.5,
+        pressure_lower: float = 150,
+        pressure_upper: float = 350,
     ) -> NodeSpec:
-        """XseriesGripper spec"""
+        """XseriesGripper spec
+
+        :param name: Name of the node.
+        :param rate: Rate of the node.
+        :param arm_name: Name of the arm.
+        :param robot_type: Type of the robot.
+        :param color: Color of the node.
+        :param pressure: fraction from 0 - 1 where '0' means the gripper operates at 'gripper_pressure_lower_limit' and '1'
+                         means the gripper operates at 'gripper_pressure_upper_limit'.
+        :param pressure_lower: lowest 'effort' that should be applied to the gripper if gripper_pressure is set to 0;
+                               it should be high enough to open/close the gripper (~150 PWM or ~400 mA current).
+        :param pressure_upper: largest 'effort' that should be applied to the gripper if gripper_pressure is set to 1;
+                               it should be low enough that the motor doesn't 'overload' when gripping an object for a few
+                               seconds (~350 PWM or ~900 mA).
+        """
         spec = cls.get_specification()
 
         # Modify default node params
         spec.config.update(name=name, rate=rate, process=eagerx.ENGINE, color=color)
         spec.config.update(arm_name=arm_name, robot_type=robot_type)
         spec.config.inputs = ["tick", "action"]
         spec.config.outputs = ["action_applied"]
+        spec.config.update(pressure=pressure, pressure_lower=pressure_lower, pressure_upper=pressure_upper)
         return spec
 
     def initialize(self, spec: NodeSpec, simulator: Any):
-        self.dxl = core.InterbotixRobotXSCore(spec.config.robot_type, spec.config.name, False)
+        self.dxl = core.InterbotixRobotXSCore(spec.config.robot_type, spec.config.arm_name, False)
         self.gripper = gripper.InterbotixGripperXSInterface(
             self.dxl,
             "gripper",
-            gripper_pressure=0.5,
-            gripper_pressure_lower_limit=150,
-            gripper_pressure_upper_limit=350,
+            gripper_pressure=spec.config.pressure,
+            gripper_pressure_lower_limit=spec.config.pressure_lower,
+            gripper_pressure_upper_limit=spec.config.pressure_upper,
         )
 
     @register.states()
@@ -324,3 +348,169 @@ def callback(self, t_n: float, tick: Msg):
 
     def shutdown(self):
         pass
+
+
+class XseriesMoveIt(eagerx.EngineNode):
+    @classmethod
+    def make(
+        cls,
+        name: str,
+        rate: float,
+        arm_name: str,
+        robot_type: str,
+        joints: List[str],
+        vel_limit: List[float],
+        color: str = "green",
+        # heartbeat = 1.0,
+        kp_pos: Union[int, Dict[str, int]] = 800,
+        ki_pos: Union[int, Dict[str, int]] = 0,
+        kd_pos: Union[int, Dict[str, int]] = 0,
+        kp_vel: Union[int, Dict[str, int]] = 100,
+        ki_vel: Union[int, Dict[str, int]] = 1920,
+        ff_acc: Union[int, Dict[str, int]] = 0,
+        ff_vel: Union[int, Dict[str, int]] = 0,
+    ) -> NodeSpec:
+        """Make the parameter specification for an Interbotix Move It controller.
+
+        :param name: Name of the node
+        :param rate: Rate of the node.
+        :param arm_name: Name of the arm.
+        :param robot_type: Manipulator type.
+        :param joints: Names of all the joint motors in the group.
+        :param color: Color of logged messages.
+        :param mode: Either "position" or "velocity". Applies to the whole group of motors.
+        :param profile_type: "time" or "velocity" (see InterbotixArm.set_operating_mode for info).
+                             Applies to the whole group of motors.
+        :param profile_velocity: Sets velocity of the Profile (see InterbotixArm.set_operating_mode for info).
+                                 ‘0’ represents an infinite velocity. Applies to the whole group of motors.
+        :param profile_acceleration: Sets acceleration time of the Profile (see InterbotixArm.set_operating_mode for info).
+                                     ‘0’ represents an infinite acceleration. Applies to the whole group of motors.
+        :param kp_pos: Position P gain. Either for a single motors (dict) or the whole group (int value).
+        :param ki_pos: Position I gain. Either for a single motors (dict) or the whole group (int value).
+        :param kd_pos: Position D gain. Either for a single motors (dict) or the whole group (int value).
+        :param kp_vel: Velocity P gain. Either for a single motors (dict) or the whole group (int value).
+        :param ki_vel: Velocity I gain. Either for a single motors (dict) or the whole group (int value).
+        :param ff_acc: Feedforward 2nd gain. Either for a single motors (dict) or the whole group (int value).
+        :param ff_vel: Feedforward 1st gain. Either for a single motors (dict) or the whole group (int value).
+        :return: Parameter specification.
+        """
+        spec = cls.get_specification()
+
+        # Modify default node params
+        spec.config.update(name=name, rate=rate, process=eagerx.ENGINE, color=color)
+        spec.config.inputs = ["tick", "action"]
+        spec.config.outputs = ["status", "action_applied"]
+
+        # Set parameters, defined by the signature of cls.initialize(...)
+        spec.config.joints = joints
+        spec.config.vel_limit = vel_limit
+
+        # Set motor configs
+        spec.config.update(arm_name=arm_name, robot_type=robot_type)
+        spec.config.update(
+            mode="position", profile_type="velocity", profile_acceleration=13, profile_velocity=131
+        )
+        spec.config.update(kp_pos=kp_pos, ki_pos=ki_pos, kd_pos=kd_pos)
+        spec.config.update(kp_vel=kp_vel, ki_vel=ki_vel)
+        spec.config.update(ff_acc=ff_acc, ff_vel=ff_vel)
+
+        # Set shape of spaces
+        spec.inputs.action.space.shape = [len(joints)]
+        spec.outputs.action_applied.space.shape = [len(joints)]
+        return spec
+
+    def initialize(self, spec: NodeSpec, simulator: Any):
+        # Get arm client
+        if "client" not in simulator:
+            simulator["client"] = Client(spec.config.robot_type, spec.config.arm_name, group_name="arm")
+        self.arm = simulator["client"]
+
+        # Remap joint measurements & commands according to ordering in spec.config.joints.
+        self.arm.set_joint_remapping(spec.config.joints)
+        self.vel_limit = np.array(spec.config.vel_limit, dtype="float32")
+        self.dt = 1 / self.rate
+        # self.hearbeat = spec.config.hearbeat
+
+        # Set operating mode
+        if spec.config.mode == "position":
+            mode = Client.POSITION
+        elif spec.config.mode == "velocity":
+            mode = Client.VELOCITY
+        else:
+            raise ValueError(f"Mode `{spec.config.mode}` not recognized.")
+
+        # Check control mode exists.
+        if mode not in self.arm.SUPPORTED_MODES:
+            raise NotImplementedError(f"The selected control mode is not implemented: {spec.config.mode}.")
+
+        # Set operating mode
+        self.arm.set_operating_mode(
+            mode=mode,
+            profile_type=spec.config.profile_type,
+            profile_acceleration=spec.config.profile_acceleration,
+            profile_velocity=spec.config.profile_velocity,
+        )
+
+        # Set gains
+        gains = dict()
+        gains["kp_pos"], gains["ki_pos"], gains["kd_pos"] = spec.config.kp_pos, spec.config.ki_pos, spec.config.kd_pos
+        gains["kp_vel"], gains["ki_vel"] = spec.config.kp_vel, spec.config.ki_vel
+        gains["ff_acc"], gains["ff_vel"] = spec.config.ff_acc, spec.config.ff_vel
+        for key, gain in gains.items():
+            if isinstance(gain, list):
+                for name in spec.config.joints:
+                    self.arm.set_pid_gains(name=name, **{key: gain})
+            elif isinstance(gain, int):
+                self.arm.set_pid_gains(**{key: gain})
+            else:
+                raise ValueError(f"Gain `{key}` has an invalid value `{gain}`.")
+
+    @register.states()
+    def reset(self):
+        self._last_t_n = None
+        self._last_targj = None
+        self._last_currj = None
+
+    @register.inputs(tick=Space(shape=(), dtype="int64"), action=Space(dtype="float32"))
+    @register.outputs(status=Space(low=0, high=1, shape=(), dtype="int64"), action_applied=Space(dtype="float32"))
+    def callback(self, t_n: float, tick: Msg, action: Msg):
+        # Get command
+        cmd = action.msgs[-1]
+        # todo: Determine status: 0: ongoing, 1: success
+        currj = np.array(self.arm.get_joint_states().position, dtype="float32")
+        targj = np.array(action.msgs[-1], dtype="float32")
+        diffj = targj - currj
+
+        # Determine time since last command
+        self._last_currj = currj if self._last_currj is None else self._last_currj
+        self._last_targj = targj if self._last_targj is None else self._last_targj
+        self._last_t_n = t_n if self._last_t_n is None else self._last_t_n
+        targj_changed = np.allclose(targj, self._last_targj)
+        dt = t_n - self._last_t_n
+        if dt > 4.0:
+            idle_update = np.allclose(currj, self._last_currj, atol=5e-2) and not np.allclose(targj, currj, atol=5e-2)
+            if idle_update:
+                self.backend.logwarn(f"[{self.name}]: Resending command. Occurs if idle or after emergency stop.")
+            self._last_currj = currj
+            self._last_t_n = t_n
+        else:
+            idle_update = False
+
+        # Periodically resend command to arm, in case it was stopped & joint positions are not moving.
+        if not targj_changed or idle_update:
+            # Write command to arm
+            self._last_targj = targj
+            timestampj = diffj / self.vel_limit
+            timestamp = np.max(timestampj)
+            self.arm.go_to(points=targj.tolist(), timestamps=timestamp, remap=True)
+            status = 0
+        elif all(np.abs(diffj) < 1e-2):
+            status = 1
+        else:
+            status = 0
+
+        # Send action that has been applied.
+        return dict(status=np.array(status, dtype="int64"), action_applied=cmd)
+
+    def shutdown(self):
+        pass

eagerx_interbotix/xseries/xseries.py

@@ -25,6 +25,7 @@ class Xseries(eagerx.Object):
         position=Space(dtype="float32"),
         velocity=Space(dtype="float32"),
         force_torque=Space(low=-20, high=20, shape=(6,), dtype="float32"),
+        gripper_position=Space(dtype="float32"),
         ee_pos=Space(low=[-2, -2, 0], high=[2, 2, 2], dtype="float32"),
         ee_orn=Space(low=-1, high=1, shape=(4,), dtype="float32"),
         moveit_status=Space(low=0, high=1, shape=(), dtype="int64"),
@@ -56,6 +57,7 @@ def make(
         fixed_base=True,
         motor_config=None,
         mode_config=None,
+        pressure: float = 0.5,
     ) -> ObjectSpec:
         """Object spec of Xseries"""
         if URDF is None:
@@ -90,11 +92,12 @@ def make(
             vel_limit.append(joint_obj.limit.velocity)
 
         # Determine gripper limits
-        gripper_lower, gripper_upper = [], []
+        gripper_lower, gripper_upper, gripper_vel_limit = [], [], []
         for n in gripper_names:
             joint_obj = next((joint for joint in urdf.joints if joint.name == n), None)
             gripper_lower.append(joint_obj.limit.lower)
             gripper_upper.append(joint_obj.limit.upper)
+            gripper_vel_limit.append(joint_obj.limit.velocity)
 
         # Modify default config
         spec.config.name = name
@@ -119,11 +122,13 @@ def make(
         spec.config.vel_limit = vel_limit
         spec.config.sleep_positions = sleep_positions
         spec.config.urdf = urdf.to_xml_string()
+        spec.config.pressure = pressure
 
         # Set rates
         spec.sensors.position.rate = rate
         spec.sensors.velocity.rate = rate
         spec.sensors.force_torque.rate = rate
+        spec.sensors.gripper_position.rate = rate
         spec.sensors.ee_pos.rate = rate
         spec.sensors.ee_orn.rate = rate
         spec.sensors.moveit_status.rate = rate
@@ -135,6 +140,8 @@ def make(
         # Set variable spaces
         spec.sensors.position.space.update(low=joint_lower, high=joint_upper)
         spec.sensors.velocity.space.update(low=[-v for v in vel_limit], high=vel_limit)
+        # spec.sensors.gripper_position.space.update(low=[gripper_lower[0], -gripper_vel_limit[0]], high=[gripper_upper[1], gripper_vel_limit[0]])
+        spec.sensors.gripper_position.space.update(low=[gripper_lower[0]*0.9], high=[gripper_upper[0]*1.1])
         spec.actuators.pos_control.space.update(low=joint_lower, high=joint_upper)
         spec.actuators.moveit_to.space.update(low=joint_lower, high=joint_upper)
         spec.actuators.vel_control.space.update(low=[-v for v in vel_limit], high=vel_limit)
@@ -205,6 +212,12 @@ def pybullet_engine(spec: ObjectSpec, graph: EngineGraph):
             links=[spec.config.gripper_link],
             mode="orientation",
         )
+        gripper_sensor = JointSensor.make(
+            "gripper_sensor",
+            rate=spec.sensors.gripper_position.rate,
+            joints=spec.config.gripper_names[:1],
+            mode="position",
+        )
 
         # Create actuator engine nodes
         # Rate=None, but we will connect it to an actuator (thus will use the rate set in the agnostic specification)
@@ -254,13 +267,14 @@ def pybullet_engine(spec: ObjectSpec, graph: EngineGraph):
 
         # Connect all engine nodes
         graph.add(
-            [pos_sensor, vel_sensor, ft_sensor, ee_pos_sensor, ee_orn_sensor, pos_control, vel_control, gripper, moveit_to]
+            [pos_sensor, vel_sensor, ft_sensor, ee_pos_sensor, ee_orn_sensor, gripper_sensor, pos_control, vel_control, gripper, moveit_to]
         )
         graph.connect(source=pos_sensor.outputs.obs, sensor="position")
         graph.connect(source=vel_sensor.outputs.obs, sensor="velocity")
         graph.connect(source=ft_sensor.outputs.obs, sensor="force_torque")
         graph.connect(source=ee_pos_sensor.outputs.obs, sensor="ee_pos")
         graph.connect(source=ee_orn_sensor.outputs.obs, sensor="ee_orn")
+        graph.connect(source=gripper_sensor.outputs.obs, sensor="gripper_position")
         graph.connect(source=moveit_to.outputs.status, sensor="moveit_status")
         graph.connect(actuator="pos_control", target=pos_control.inputs.action)
         graph.connect(actuator="vel_control", target=vel_control.inputs.action)
@@ -283,7 +297,7 @@ def reality_engine(spec: ObjectSpec, graph: EngineGraph):
         spec.engine.states.color = DummyState.make()
 
         # Create sensor engine nodes
-        from eagerx_interbotix.xseries.real.enginenodes import XseriesGripper, XseriesSensor, XseriesArm, DummySensor
+        from eagerx_interbotix.xseries.real.enginenodes import XseriesGripper, XseriesSensor, XseriesArm, DummySensor, XseriesMoveIt
 
         joints = spec.config.joint_names
         robot_type = spec.config.robot_type
@@ -323,6 +337,14 @@ def reality_engine(spec: ObjectSpec, graph: EngineGraph):
             arm_name=arm_name,
             robot_type=robot_type,
         )
+        gripper_sensor = XseriesSensor.make(
+            "gripper_sensor",
+            rate=spec.sensors.gripper_position.rate,
+            joints=joints,
+            mode="gripper_position",
+            arm_name=arm_name,
+            robot_type=robot_type,
+        )
 
         # Create actuator engine nodes
         # todo: set space to limits (pos=joint_limits, vel=vel_limits, effort=[-1, 1]?)
@@ -357,18 +379,32 @@ def reality_engine(spec: ObjectSpec, graph: EngineGraph):
             arm_name=arm_name,
             robot_type=robot_type,
         )
+        moveit_to = XseriesMoveIt.make(
+            "moveit_to",
+            rate=spec.actuators.moveit_to.rate,
+            arm_name=arm_name,
+            robot_type=robot_type,
+            joints=joints,
+            vel_limit=[0.5*c for c in spec.config.vel_limit],
+            kp_pos=800,
+            kd_pos=1000,
+        )
         gripper = XseriesGripper.make(
-            "gripper_control", rate=spec.actuators.gripper_control.rate, arm_name=arm_name, robot_type=robot_type
+            "gripper_control", rate=spec.actuators.gripper_control.rate, arm_name=arm_name, robot_type=robot_type, pressure=spec.config.pressure,
+            pressure_lower=150, pressure_upper=350,
         )
         ft_sensor = DummySensor.make("ft_sensor", rate=spec.sensors.force_torque.rate)
 
         # Connect all engine nodes
-        graph.add([pos_sensor, vel_sensor, ee_pos_sensor, ee_orn_sensor, pos_control, vel_control, gripper, ft_sensor])
+        graph.add([pos_sensor, vel_sensor, ee_pos_sensor, ee_orn_sensor, gripper_sensor, pos_control, vel_control, moveit_to, gripper, ft_sensor])
         graph.connect(source=ft_sensor.outputs.obs, sensor="force_torque")
         graph.connect(source=pos_sensor.outputs.obs, sensor="position")
         graph.connect(source=vel_sensor.outputs.obs, sensor="velocity")
         graph.connect(source=ee_pos_sensor.outputs.obs, sensor="ee_pos")
         graph.connect(source=ee_orn_sensor.outputs.obs, sensor="ee_orn")
+        graph.connect(source=gripper_sensor.outputs.obs, sensor="gripper_position")
+        graph.connect(source=moveit_to.outputs.status, sensor="moveit_status")
         graph.connect(actuator="pos_control", target=pos_control.inputs.action)
         graph.connect(actuator="vel_control", target=vel_control.inputs.action)
         graph.connect(actuator="gripper_control", target=gripper.inputs.action)
+        graph.connect(actuator="moveit_to", target=moveit_to.inputs.action)