crisp-python 1.4.0__tar.gz → 1.5.0__tar.gz

{crisp_python-1.4.0/crisp_python.egg-info → crisp_python-1.5.0}/PKG-INFO

@@ -1,6 +1,6 @@
 Metadata-Version: 2.4
 Name: crisp-python
-Version: 1.4.0
+Version: 1.5.0
 Summary: Simple python interface to control robots that use crisp_controllers and other ROS2 components.
 Project-URL: Homepage, https://utiasDSL.github.io/crisp_controllers/
 Requires-Python: >=3.11

{crisp_python-1.4.0 → crisp_python-1.5.0}/crisp_py/robot.py

@@ -1,13 +1,12 @@
 """Provides a client to control the franka robot. It is the easiest way to control the robot using ROS2."""
 
 import threading
-from dataclasses import dataclass
 from typing import List
 
 import numpy as np
 import rclpy
 import rclpy.executors
-from geometry_msgs.msg import PoseStamped, WrenchStamped
+from geometry_msgs.msg import PoseStamped, TwistStamped, WrenchStamped
 from numpy.typing import NDArray
 from rclpy.callback_groups import ReentrantCallbackGroup
 from rclpy.node import Node
@@ -20,36 +19,7 @@ from crisp_py.control.joint_trajectory_controller_client import JointTrajectoryC
 from crisp_py.control.parameters_client import ParametersClient
 from crisp_py.robot_config import FrankaConfig, RobotConfig
 from crisp_py.utils.callback_monitor import CallbackMonitor
-
-
-@dataclass
-class Pose:
-    """Compact representation of an SE3 object."""
-
-    position: np.ndarray
-    orientation: Rotation
-
-    def copy(self) -> "Pose":
-        """Create a copy of this pose."""
-        return Pose(self.position.copy(), Rotation.from_quat(self.orientation.as_quat()))
-
-    def __str__(self) -> str:
-        """Return a string representation of a Pose."""
-        return f"Pos: {np.array2string(self.position, suppress_small=True, precision=2, floatmode='fixed')},\n Orientation: {np.array2string(self.orientation.as_matrix(), suppress_small=True, precision=2, floatmode='fixed')}"
-
-    def __sub__(self, other: "Pose") -> "Pose":
-        """Subtract another pose from this pose, i.e. compute the relative pose."""
-        return Pose(
-            self.position - other.position,
-            self.orientation * other.orientation.inv(),
-        )
-
-    def __add__(self, other: "Pose") -> "Pose":
-        """Add another pose to this pose, i.e. add a relative pose."""
-        return Pose(
-            self.position + other.position,
-            other.orientation * self.orientation,
-        )
+from crisp_py.utils.geometry import Pose, Twist
 
 
 class Robot:
@@ -111,6 +81,7 @@ class Robot:
         self._current_joint = None
         self._target_joint = None
         self._target_wrench = None
+        self._current_twist = None
 
         self._callback_monitor = CallbackMonitor(
             node=self.node,
@@ -145,6 +116,16 @@ class Robot:
             callback_group=ReentrantCallbackGroup(),
         )
 
+        self.node.create_subscription(
+            TwistStamped,
+            self.config.current_twist_topic,
+            self._callback_monitor.monitor(
+                f"{namespace.capitalize()} Current Twist", self._callback_current_twist
+            ),
+            qos_profile_sensor_data,
+            callback_group=ReentrantCallbackGroup(),
+        )
+
         self.node.create_timer(
             1.0 / self.config.publish_frequency,
             self._callback_monitor.monitor(
@@ -237,6 +218,22 @@ class Robot:
             )
         return self._target_joint.copy()
 
+    @property
+    def end_effector_twist(self) -> Twist:
+        """Get the current twist of the end effector.
+
+        Returns:
+            Twist: Current end-effector twist message.
+
+        Raises:
+            RuntimeError: If no twist messages have been received yet.
+        """
+        if self._current_twist is None:
+            raise RuntimeError(
+                f"The robot has not received any twists yet. Is the current_twist_topic {self.config.current_twist_topic} correct?"
+            )
+        return self._current_twist.copy()
+
     def is_ready(self) -> bool:
         """Check if the robot is ready for operation.
 
@@ -311,7 +308,11 @@ class Robot:
         """
         if self._target_pose is None or not rclpy.ok():
             return
-        self._target_pose_publisher.publish(self._pose_to_pose_msg(self._target_pose))
+        self._target_pose_publisher.publish(
+            self._target_pose.to_ros_msg(
+                self.config.base_frame, self.node.get_clock().now().to_msg()
+            )
+        )
 
     def _callback_publish_target_joint(self):
         """Publish the current target joint configuration if one exists.
@@ -384,10 +385,21 @@ class Robot:
         Args:
             msg (PoseStamped): ROS message containing the current pose.
         """
-        self._current_pose = self._pose_msg_to_pose(msg)
+        self._current_pose = Pose.from_ros_msg(msg)
         if self._target_pose is None:
             self._target_pose = self._current_pose.copy()
 
+    def _callback_current_twist(self, msg: TwistStamped):
+        """Update the current twist from a ROS message.
+
+        This callback is triggered when a new twist message is received. It updates
+        the current twist.
+
+        Args:
+            msg (TwistStamped): ROS message containing the current twist.
+        """
+        self._current_twist = Twist.from_ros_msg(msg)
+
     def _callback_current_joint(self, msg: JointState):
         """Update the current joint state from a ROS message.
 
@@ -467,34 +479,6 @@ class Robot:
         # if switch_to_default_controller:
         #     self.controller_switcher_client.switch_controller(self.config.default_controller)
 
-    def _pose_msg_to_pose(self, pose: PoseStamped) -> Pose:
-        """Convert a ROS2 pose msg to a pose."""
-        position = np.array([pose.pose.position.x, pose.pose.position.y, pose.pose.position.z])
-        orientation = Rotation.from_quat(
-            [
-                pose.pose.orientation.x,
-                pose.pose.orientation.y,
-                pose.pose.orientation.z,
-                pose.pose.orientation.w,
-            ]
-        )
-        return Pose(position, orientation)
-
-    def _pose_to_pose_msg(self, pose: Pose) -> PoseStamped:
-        """Convert a pose to a pose message."""
-        msg = PoseStamped()
-        msg.header.frame_id = self.config.base_frame
-        msg.header.stamp = self.node.get_clock().now().to_msg()
-        msg.pose.position.x, msg.pose.position.y, msg.pose.position.z = pose.position
-        q = pose.orientation.as_quat()
-        (
-            msg.pose.orientation.x,
-            msg.pose.orientation.y,
-            msg.pose.orientation.z,
-            msg.pose.orientation.w,
-        ) = q
-        return msg
-
     def _joint_to_joint_msg(
         self, q: NDArray, dq: NDArray | None = None, tau: NDArray | None = None
     ) -> JointState:

{crisp_python-1.4.0 → crisp_python-1.5.0}/crisp_py/robot_config.py

@@ -41,6 +41,7 @@ class RobotConfig:
     target_joint_topic: str = "target_joint"
     current_pose_topic: str = "current_pose"
     current_joint_topic: str = "joint_states"
+    current_twist_topic: str = "current_twist"
 
     publish_frequency: float = 50.0
     time_to_home: float = 5.0

crisp_python-1.5.0/crisp_py/utils/geometry.py

@@ -0,0 +1,107 @@
+"""Utility functions for geometry operations."""
+
+from dataclasses import dataclass
+
+import numpy as np
+from builtin_interfaces.msg import Time as TimeMsg
+from geometry_msgs.msg import PoseStamped, TwistStamped
+from scipy.spatial.transform import Rotation
+
+
+@dataclass
+class Pose:
+    """Compact representation of an SE3 object."""
+
+    position: np.ndarray
+    orientation: Rotation
+
+    def copy(self) -> "Pose":
+        """Create a copy of this pose."""
+        return Pose(self.position.copy(), Rotation.from_quat(self.orientation.as_quat()))
+
+    def __str__(self) -> str:
+        """Return a string representation of a Pose."""
+        return f"Pos: {np.array2string(self.position, suppress_small=True, precision=2, floatmode='fixed')},\n Orientation: {np.array2string(self.orientation.as_matrix(), suppress_small=True, precision=2, floatmode='fixed')}"
+
+    def __sub__(self, other: "Pose") -> "Pose":
+        """Subtract another pose from this pose, i.e. compute the relative pose."""
+        return Pose(
+            self.position - other.position,
+            self.orientation * other.orientation.inv(),
+        )
+
+    def __add__(self, other: "Pose") -> "Pose":
+        """Add another pose to this pose, i.e. add a relative pose."""
+        return Pose(
+            self.position + other.position,
+            other.orientation * self.orientation,
+        )
+
+    @classmethod
+    def from_ros_msg(cls, msg: PoseStamped) -> "Pose":
+        """Create Pose from ROS PoseStamped message."""
+        position = np.array([msg.pose.position.x, msg.pose.position.y, msg.pose.position.z])
+        orientation = Rotation.from_quat(
+            [
+                msg.pose.orientation.x,
+                msg.pose.orientation.y,
+                msg.pose.orientation.z,
+                msg.pose.orientation.w,
+            ]
+        )
+        return cls(position, orientation)
+
+    def to_ros_msg(self, frame_id: str, stamp: TimeMsg) -> PoseStamped:
+        """Convert Pose to ROS PoseStamped message."""
+        msg = PoseStamped()
+        msg.header.frame_id = frame_id
+        msg.header.stamp = stamp
+        msg.pose.position.x, msg.pose.position.y, msg.pose.position.z = self.position
+        q = self.orientation.as_quat()
+        (
+            msg.pose.orientation.x,
+            msg.pose.orientation.y,
+            msg.pose.orientation.z,
+            msg.pose.orientation.w,
+        ) = q
+        return msg
+
+
+@dataclass
+class Twist:
+    """Compact representation of a twist (linear and angular velocity)."""
+
+    linear: np.ndarray
+    angular: np.ndarray
+
+    def copy(self) -> "Twist":
+        """Create a copy of this twist."""
+        return Twist(self.linear.copy(), self.angular.copy())
+
+    def __str__(self) -> str:
+        """Return a string representation of a Twist."""
+        return f"Linear: {np.array2string(self.linear, suppress_small=True, precision=2, floatmode='fixed')},\n Angular: {np.array2string(self.angular, suppress_small=True, precision=2, floatmode='fixed')}"
+
+    def magnitude(self) -> float:
+        """Return the magnitude of the linear velocity."""
+        return np.linalg.norm(self.linear).astype(float)
+
+    def angular_magnitude(self) -> float:
+        """Return the magnitude of the angular velocity."""
+        return np.linalg.norm(self.angular).astype(float)
+
+    @classmethod
+    def from_ros_msg(cls, msg: TwistStamped) -> "Twist":
+        """Create Twist from ROS TwistStamped message."""
+        linear = np.array([msg.twist.linear.x, msg.twist.linear.y, msg.twist.linear.z])
+        angular = np.array([msg.twist.angular.x, msg.twist.angular.y, msg.twist.angular.z])
+        return cls(linear, angular)
+
+    def to_ros_msg(self, frame_id: str, stamp: TimeMsg) -> TwistStamped:
+        """Convert Twist to ROS TwistStamped message."""
+        msg = TwistStamped()
+        msg.header.frame_id = frame_id
+        msg.header.stamp = stamp
+        msg.twist.linear.x, msg.twist.linear.y, msg.twist.linear.z = self.linear
+        msg.twist.angular.x, msg.twist.angular.y, msg.twist.angular.z = self.angular
+        return msg

{crisp_python-1.4.0 → crisp_python-1.5.0/crisp_python.egg-info}/PKG-INFO

@@ -1,6 +1,6 @@
 Metadata-Version: 2.4
 Name: crisp-python
-Version: 1.4.0
+Version: 1.5.0
 Summary: Simple python interface to control robots that use crisp_controllers and other ROS2 components.
 Project-URL: Homepage, https://utiasDSL.github.io/crisp_controllers/
 Requires-Python: >=3.11

{crisp_python-1.4.0 → crisp_python-1.5.0}/crisp_python.egg-info/SOURCES.txt

@@ -35,6 +35,7 @@ crisp_py/sensors/sensor_config.py
 crisp_py/utils/__init__.py
 crisp_py/utils/callback_monitor.py
 crisp_py/utils/diagnostics_viewer.py
+crisp_py/utils/geometry.py
 crisp_python.egg-info/PKG-INFO
 crisp_python.egg-info/SOURCES.txt
 crisp_python.egg-info/dependency_links.txt

{crisp_python-1.4.0 → crisp_python-1.5.0}/pyproject.toml

@@ -1,6 +1,6 @@
 [project]
 name = "crisp-python"
-version = "1.4.0"
+version = "1.5.0"
 description = "Simple python interface to control robots that use crisp_controllers and other ROS2 components."
 readme = "README.md"
 license-files = ["LICENSE.md"]