feat: add joint velocities support to Robot (#71)

Extract and store joint velocities from JointState messages
in the joint state callback, with a new joint_velocities property.

Author: lvjonok

crisp_py/robot/robot.py

@@ -88,6 +88,7 @@ def __init__(
         self._current_pose = None
         self._target_pose = None
         self._current_joint = None
+        self._current_joint_velocity = None
         self._target_joint = None
         self._target_wrench = None
         self._current_twist = None
@@ -304,6 +305,19 @@ def target_joint(self) -> NDArray:
             )
         return self._target_joint.copy()
 
+    @property
+    def joint_velocities(self) -> NDArray:
+        """Get the current joint velocities of the robot.
+
+        Returns:
+            numpy.ndarray: Copy of current joint velocities.
+        """
+        if self._current_joint_velocity is None:
+            raise RuntimeError(
+                "The robot has not received any joint velocities yet. Run wait_until_ready() before running anything else."
+            )
+        return self._current_joint_velocity.copy()
+
     @property
     def end_effector_twist(self) -> Twist:
         """Get the current twist of the end effector.
@@ -510,6 +524,7 @@ def _callback_current_joint(self, msg: JointState):
             msg (JointState): ROS message containing joint states.
         """
         self._current_joint = self.ros_msg_to_joint(msg).copy()
+        self._current_joint_velocity = self._ros_msg_to_joint_velocity(msg).copy()
 
         if self._target_joint is None:
             self._target_joint = self._current_joint.copy()
@@ -591,6 +606,17 @@ def ros_msg_to_joint(self, msg: JointState) -> NDArray:
             )
         return joint_values.astype(np.float32)
 
+    def _ros_msg_to_joint_velocity(self, msg: JointState) -> NDArray:
+        """Convert a joint state message to a numpy array of joint velocities."""
+        joint_velocities = np.zeros(self.nq)
+        for joint_name, joint_velocity in zip(msg.name, msg.velocity):
+            if joint_name.removeprefix(self._prefix) not in self.config.joint_names:
+                continue
+            joint_velocities[self.config.joint_names.index(joint_name.removeprefix(self._prefix))] = (
+                joint_velocity
+            )
+        return joint_velocities.astype(np.float32)
+
     def _parse_pose_or_position(
         self, position: List | NDArray | None = None, pose: Pose | None = None
     ) -> Pose: