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

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

@@ -1,6 +1,6 @@
 Metadata-Version: 2.4
 Name: crisp-python
-Version: 1.3.1
+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.5.0/crisp_py/camera/__init__.py

@@ -0,0 +1,6 @@
+"""Initialize the camera module."""
+
+from crisp_py.camera.camera import Camera
+from crisp_py.camera.camera_config import CameraConfig
+
+__import__ = [Camera, CameraConfig]

{crisp_python-1.3.1 → crisp_python-1.5.0}/crisp_py/camera/camera_config.py

@@ -16,16 +16,3 @@ class CameraConfig:
     camera_color_info_topic: str | None = None
 
     max_image_delay: float = 1.0
-
-
-@dataclass
-class FrankaCameraConfig(CameraConfig):
-    """Example camera configuration used with Franka."""
-
-    camera_name: str = "franka"
-    camera_frame: str = "franka_link"
-
-    resolution: tuple[int, int] | None = (256, 256)
-
-    camera_color_image_topic: str | None = "franka/color/image_raw"
-    camera_color_info_topic: str | None = "franka/color/camera_info"

{crisp_python-1.3.1 → crisp_python-1.5.0}/crisp_py/config/trigger_left.yaml

@@ -1,4 +1,4 @@
-joint_state_topic: gripper_state_broadcaster/joint_states
+joint_state_topic: trigger_state_broadcaster/joint_states
 max_value: 1.10193625925745
 min_value: 1.8907875646311676
 index: 1

{crisp_python-1.3.1 → crisp_python-1.5.0}/crisp_py/control/controller_switcher.py

@@ -15,18 +15,16 @@ class ControllerSwitcherClient:
     """ControllerSwitcher class allows user to communicate with the controller_manager and manage controllers in an easy way."""
 
     def __init__(
-        self, node: Node, always_active: list[str] = ["joint_state_broadcaster", "pose_broadcaster"]
+        self,
+        node: Node,
     ):
         """Initialize the ControllerSwitcher.
 
         Args:
             node (Node): Node used for the communication with the controller_manager.
-            always_active (list[str], optional): List of controllers that are always active. Defaults to ["joint_state_broadcaster", "franka_robot_state_broadcaster"].
         """
         self.node = node
 
-        self.always_active = always_active
-
         self.load_client = node.create_client(
             LoadController,
             "controller_manager/load_controller",
@@ -160,7 +158,7 @@ class ControllerSwitcherClient:
 
         to_deactivate = []
         for active_controller in active_controllers:
-            if active_controller not in self.always_active:
+            if not active_controller.endswith("broadcaster"):  # Do not deactivate broadcasters
                 to_deactivate.append(active_controller)
 
         to_activate = [controller_name]

{crisp_python-1.3.1 → 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.3.1 → crisp_python-1.5.0}/crisp_py/robot_config.py

@@ -5,7 +5,7 @@ from dataclasses import dataclass, field
 import numpy as np
 
 
-@dataclass
+@dataclass(kw_only=True)
 class RobotConfig:
     """Configuration class for robot parameters.
 
@@ -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
@@ -184,3 +185,32 @@ class SO101Config(RobotConfig):
     )
     base_frame: str = "Base"
     target_frame: str = "Fixed_Gripper"
+
+
+def make_robot_config(robot_type: str, **kwargs) -> RobotConfig:  # noqa: ANN003
+    """Factory function to create robot configuration objects.
+
+    Args:
+        robot_type (str): Type of robot ('franka', 'kinova', 'iiwa', 'so101')
+        **kwargs: Additional keyword arguments to override default configuration
+
+    Returns:
+        RobotConfig: Configured robot configuration object
+
+    Raises:
+        ValueError: If robot_type is not supported
+    """
+    robot_type = robot_type.lower()
+
+    if robot_type == "franka":
+        return FrankaConfig(**kwargs)
+    elif robot_type == "kinova":
+        return KinovaConfig(**kwargs)
+    elif robot_type == "iiwa":
+        return IiwaConfig(**kwargs)
+    elif robot_type == "so101":
+        return SO101Config(**kwargs)
+    else:
+        raise ValueError(
+            f"Unsupported robot type: {robot_type}. Supported types: franka, kinova, iiwa, so101"
+        )

{crisp_python-1.3.1 → crisp_python-1.5.0}/crisp_py/sensors/sensor.py

@@ -5,11 +5,11 @@ from abc import ABC, abstractmethod
 
 import numpy as np
 import rclpy
+from geometry_msgs.msg import WrenchStamped
 from rclpy.callback_groups import ReentrantCallbackGroup
 from rclpy.executors import MultiThreadedExecutor, SingleThreadedExecutor
 from rclpy.node import Node
 from rclpy.qos import qos_profile_sensor_data
-from sensor_msgs.msg import JointState
 from std_msgs.msg import Float32MultiArray
 
 from crisp_py.sensors.sensor_config import SensorConfig
@@ -54,7 +54,7 @@ class Sensor(ABC):
         self._value: np.ndarray | None = None
         self._baseline: np.ndarray | None = None
         self._callback_monitor = CallbackMonitor(
-            self.node, f"Sensor '{self.config.name}'", self.config.max_data_delay
+            self.node, stale_threshold=self.config.max_data_delay
         )
 
         self._create_subscription()
@@ -72,7 +72,6 @@ class Sensor(ABC):
         """Get the latest calibrated sensor value."""
         if self._value is None or self._baseline is None:
             raise ValueError("Sensor value is not available yet.")
-        self._callback_monitor.check_callback_health()
         return self._value - self._baseline
 
     def _spin_node(self):
@@ -132,7 +131,7 @@ class Float32ArraySensor(Sensor):
         self.node.create_subscription(
             Float32MultiArray,
             self.config.data_topic,
-            self._callback_monitor.monitor(self._callback_sensor_data),
+            self._callback_monitor.monitor(name="float32", func=self._callback_sensor_data),
             qos_profile_sensor_data,
             callback_group=ReentrantCallbackGroup(),
         )
@@ -144,21 +143,49 @@ class Float32ArraySensor(Sensor):
             self._baseline = np.zeros_like(self._value)
 
 
-class TorqueSensor(Sensor):
-    """Torque sensor that subscribes to JointState messages."""
+class ForceTorqueSensor(Sensor):
+    """Torque sensor that subscribes to WrenchStamped messages."""
 
     def _create_subscription(self):
-        """Create the ROS2 subscription for JointState messages."""
+        """Create the ROS2 subscription for WrenchStamped messages."""
         self.node.create_subscription(
-            JointState,
+            WrenchStamped,
             self.config.data_topic,
-            self._callback_monitor(self._callback_joint_state),
+            self._callback_monitor.monitor(name="force_torque_monitor", func=self._callback_wrench),
             qos_profile_sensor_data,
             callback_group=ReentrantCallbackGroup(),
         )
 
-    def _callback_joint_state(self, msg: JointState):
-        """Callback for joint state data."""
-        self._value = np.array(msg.effort, dtype=np.float32)
+    def _callback_wrench(self, msg: WrenchStamped):
+        """Callback for wrench data."""
+        self._value = np.array(
+            [
+                msg.wrench.force.x,
+                msg.wrench.force.y,
+                msg.wrench.force.z,
+                msg.wrench.torque.x,
+                msg.wrench.torque.y,
+                msg.wrench.torque.z,
+            ],
+            dtype=np.float32,
+        )
         if self._baseline is None:
             self._baseline = np.zeros_like(self._value)
+
+
+def make_sensor(
+    sensor_config: SensorConfig,
+    **kwargs,  # noqa: ANN003
+) -> Sensor:
+    """Factory function to create a sensor based on the configuration."""
+    if sensor_config.sensor_type == "float32":
+        return Float32ArraySensor(
+            sensor_config=sensor_config,
+            **kwargs,
+        )
+    elif sensor_config.sensor_type == "force_torque":
+        return ForceTorqueSensor(
+            sensor_config=sensor_config,
+            **kwargs,
+        )
+    raise ValueError(f"Unknown sensor type: {sensor_config.sensor_type}")

crisp_python-1.5.0/crisp_py/sensors/sensor_config.py

@@ -0,0 +1,61 @@
+"""Sensor configuration classes for sensor objects."""
+
+from dataclasses import dataclass
+from typing import Literal
+
+
+@dataclass(kw_only=True)
+class SensorConfig:
+    """Default sensor configuration."""
+
+    shape: tuple[int, ...]
+    sensor_type: Literal["empty", "float32", "force_torque"]
+
+    name: str = "sensor"
+    data_topic: str = "sensor_data"
+    max_data_delay: float = 1.0
+
+
+@dataclass
+class EmptySensorConfig(SensorConfig):
+    """Configuration for an empty sensor, used when using no sensor."""
+
+    shape: tuple[int, ...] = (0,)
+    sensor_type: Literal["empty", "float32", "force_torque"] = "empty"
+
+    name: str = "empty_sensor"
+
+
+@dataclass
+class AnySkinSensorConfig(SensorConfig):
+    """Configuration for the AnySkin sensor broadcasted by a node using the anyskin_ros2 wrapper: https://github.com/danielsanjosepro/anyskin_ros2."""
+
+    shape: tuple[int, ...] = (5,)
+    name: str = "anyskin_sensor"
+    data_topic: str = "tactile_data"
+    sensor_type: Literal["empty", "float32", "force_torque"] = "float32"
+
+
+@dataclass
+class ForceTorqueSensorConfig(SensorConfig):
+    """Configuration for a force-torque (i.e. wrench) sensor."""
+
+    shape: tuple[int, ...] = (6,)
+    name: str = "ft_sensor"
+    data_topic: str = "external_wrench"
+    sensor_type: Literal["empty", "float32", "force_torque"] = "force_torque"
+
+
+def make_sensor_config(
+    sensor_type: Literal["empty", "float32", "force_torque"],
+    **kwargs,  # noqa: ANN003
+) -> SensorConfig:
+    """Factory function to create a sensor configuration based on the type."""
+    if sensor_type == "empty":
+        return EmptySensorConfig(**kwargs)
+    elif sensor_type == "float32":
+        return AnySkinSensorConfig(**kwargs)
+    elif sensor_type == "force_torque":
+        return ForceTorqueSensorConfig(**kwargs)
+    else:
+        raise ValueError(f"Unknown sensor type: {sensor_type}")

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.3.1 → crisp_python-1.5.0/crisp_python.egg-info}/PKG-INFO

@@ -1,6 +1,6 @@
 Metadata-Version: 2.4
 Name: crisp-python
-Version: 1.3.1
+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.3.1 → 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.3.1 → crisp_python-1.5.0}/pyproject.toml

@@ -1,6 +1,6 @@
 [project]
 name = "crisp-python"
-version = "1.3.1"
+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"]

crisp_python-1.3.1/crisp_py/camera/__init__.py

@@ -1,6 +0,0 @@
-"""Initialize the camera module."""
-
-from crisp_py.camera.camera import Camera
-from crisp_py.camera.camera_config import CameraConfig, FrankaCameraConfig
-
-__import__ = [Camera, CameraConfig, FrankaCameraConfig]

crisp_python-1.3.1/crisp_py/sensors/sensor_config.py

@@ -1,31 +0,0 @@
-"""Sensor configuration classes for sensor objects."""
-
-from dataclasses import dataclass
-
-
-@dataclass
-class SensorConfig:
-    """Default sensor configuration."""
-
-    shape: tuple[int, ...]
-
-    name: str = "sensor"
-    data_topic: str = "sensor_data"
-    max_data_delay: float = 1.0
-
-
-@dataclass
-class EmptySensorConfig(SensorConfig):
-    """Configuration for an empty sensor, used when using no sensor."""
-
-    shape: tuple[int, ...] = (0,)
-    name: str = "empty_sensor"
-
-
-@dataclass
-class AnySkinSensorConfig(SensorConfig):
-    """Configuration for the AnySkin sensor broadcasted by a node using the anyskin_ros2 wrapper: https://github.com/danielsanjosepro/anyskin_ros2."""
-
-    shape: tuple[int, ...] = (5,)
-    name: str = "anyskin_sensor"
-    data_topic: str = "tactile_data"