crisp-python 3.1.0__tar.gz → 3.2.0__tar.gz

{crisp_python-3.1.0/crisp_python.egg-info → crisp_python-3.2.0}/PKG-INFO

@@ -1,6 +1,6 @@
 Metadata-Version: 2.4
 Name: crisp-python
-Version: 3.1.0
+Version: 3.2.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-3.2.0/crisp_py/config/grippers/gripper_robotiq_2f85.yaml

@@ -0,0 +1,6 @@
+  min_value: 0.8
+  max_value: 0.0
+  command_topic: "/robotiq_gripper_controller/gripper_cmd"
+  use_gripper_command_action: True
+  max_delta: 1.0
+  max_effort: 5.0

crisp_python-3.2.0/crisp_py/config/robots/dynaarm.yaml

@@ -0,0 +1 @@
+robot_type: "dynaarm"

crisp_python-3.2.0/crisp_py/config/robots/iiwa14.yaml

@@ -0,0 +1 @@
+robot_type: "iiwa"

{crisp_python-3.1.0 → crisp_python-3.2.0}/crisp_py/control/controller_switcher.py

@@ -120,16 +120,26 @@ class ControllerSwitcherClient:
 
         return response.ok
 
-    def switch_controller(self, controller_name: str) -> bool | None:
+    def switch_controller(
+        self,
+        controller_name: str,
+        controllers_that_should_be_active: list[str] | None = None,
+    ) -> bool | None:
         """Switch to a different ros2_controller that is already loaded using a service.
 
         First we request a list of current controllers.
         If the desired controller is not loaded, then we request to load it and configure it.
-        Finally, we request to switch to the desired controller.
+        Then we prepare a list of controllers to deactivate (all active controllers except broadcasters and those that should remain active).
+        We also prepare a list of controllers to activate (the desired controller and those that should remain active).
+        Finally, we request to switch to the desired controllers.
 
         Args:
             controller_name (str): Name of the controller to switch to.
+            controllers_that_should_be_active (list[str] | None): List of controller names to keep active or activate in the switch. Defaults to None.
         """
+        if controllers_that_should_be_active is None:
+            controllers_that_should_be_active = []
+
         controllers = self.get_controller_list()
 
         active_controllers = [
@@ -144,24 +154,23 @@ class ControllerSwitcherClient:
             return True
 
         if controller_name not in inactive_controllers:
-            ok = self.load_controller(controller_name)
-            if not ok:
-                self.node.get_logger().error(
-                    f"Failed to load controller {controller_name}. Are you sure the controller exists?"
-                )
-                raise RuntimeError(f"Failed to load controller {controller_name}.")
-
-            ok = self.configure_controller(controller_name)
-            if not ok:
-                self.node.get_logger().error(f"Failed to configure controller {controller_name}.")
-                raise RuntimeError(f"Failed to configure controller {controller_name}.")
+            self._try_loading_and_configuring_controller(controller_name)
 
         to_deactivate = []
         for active_controller in active_controllers:
-            if not active_controller.endswith("broadcaster"):  # Do not deactivate broadcasters
-                to_deactivate.append(active_controller)
+            if active_controller in controllers_that_should_be_active:
+                continue  # Keep this controller active
+
+            if active_controller.endswith("broadcaster"):  # Do not deactivate broadcasters
+                continue
 
-        to_activate = [controller_name]
+            to_deactivate.append(active_controller)
+
+        to_activate = [
+            inactive_controller
+            for inactive_controller in inactive_controllers
+            if inactive_controller in controllers_that_should_be_active
+        ] + [controller_name]
 
         ok = self._switch_controller(to_deactivate, to_activate)
 
@@ -170,3 +179,21 @@ class ControllerSwitcherClient:
             raise RuntimeError(f"Failed to switch to controller {controller_name}.")
 
         return True
+
+    def _try_loading_and_configuring_controller(self, controller_name: str) -> None:
+        """Try to load and configure a controller.
+
+        Args:
+            controller_name (str): Name of the controller to load and configure.
+        """
+        ok = self.load_controller(controller_name)
+        if not ok:
+            self.node.get_logger().error(
+                f"Failed to load controller {controller_name}. Are you sure the controller exists?"
+            )
+            raise RuntimeError(f"Failed to load controller {controller_name}.")
+
+        ok = self.configure_controller(controller_name)
+        if not ok:
+            self.node.get_logger().error(f"Failed to configure controller {controller_name}.")
+            raise RuntimeError(f"Failed to configure controller {controller_name}.")

{crisp_python-3.1.0 → crisp_python-3.2.0}/crisp_py/gripper/gripper.py

@@ -5,6 +5,8 @@ import threading
 import numpy as np
 import rclpy
 import yaml
+from control_msgs.action import GripperCommand
+from rclpy.action.client import ActionClient
 from rclpy.callback_groups import ReentrantCallbackGroup
 from rclpy.executors import MultiThreadedExecutor
 from rclpy.node import Node
@@ -61,12 +63,27 @@ class Gripper:
             self.node, stale_threshold=self.config.max_joint_delay
         )
 
-        self._command_publisher = self.node.create_publisher(
-            Float64MultiArray,
-            self.config.command_topic,
-            qos_profile_system_default,
-            callback_group=ReentrantCallbackGroup(),
+        self._command_publisher = (
+            self.node.create_publisher(
+                Float64MultiArray,
+                self.config.command_topic,
+                qos_profile_system_default,
+                callback_group=ReentrantCallbackGroup(),
+            )
+            if not self.config.use_gripper_command_action
+            else None
+        )
+        self._command_action_client = (
+            ActionClient(
+                self.node,
+                GripperCommand,
+                self.config.command_topic,
+                callback_group=ReentrantCallbackGroup(),
+            )
+            if self.config.use_gripper_command_action
+            else None
         )
+
         self._joint_subscriber = self.node.create_subscription(
             JointState,
             self.config.joint_state_topic,
@@ -216,7 +233,12 @@ class Gripper:
 
     def is_ready(self) -> bool:
         """Returns True if the gripper is fully ready to operate."""
-        return self._value is not None
+        action_client_ready = (
+            self._command_action_client.wait_for_server(timeout_sec=0.0)
+            if self._command_action_client
+            else True
+        )
+        return self._value is not None and action_client_ready
 
     def wait_until_ready(self, timeout: float = 10.0, check_frequency: float = 10.0):
         """Wait until the gripper is available."""
@@ -247,6 +269,27 @@ class Gripper:
         """Publish the target command."""
         if self._target is None:
             return
+
+        if self.config.use_gripper_command_action:
+            if self._command_action_client is None:
+                raise RuntimeError("Command action client is not initialized.")
+
+            goal = GripperCommand.Goal()
+            goal.command.position = self._unnormalize(
+                self.value
+                + np.clip(
+                    self._normalize(self._target) - self.value,
+                    -self.config.max_delta,
+                    self.config.max_delta,
+                )
+            )
+            goal.command.max_effort = self.config.max_effort
+            self._command_action_client.send_goal_async(goal)
+            return
+
+        if self._command_publisher is None:
+            raise RuntimeError("Command publisher is not initialized.")
+
         msg = Float64MultiArray()
         msg.data = [
             self._unnormalize(

{crisp_python-3.1.0 → crisp_python-3.2.0}/crisp_py/gripper/gripper_config.py

@@ -13,6 +13,19 @@ class GripperConfig:
     """Gripper default config.
 
     Can be extented to be used with other grippers.
+
+    Attributes:
+        min_value (float): Minimum gripper value (fully closed).
+        max_value (float): Maximum gripper value (fully open).
+        command_topic (str): Topic to publish gripper commands to.
+        joint_state_topic (str): Topic to subscribe for joint states.
+        reboot_service (str): Service to reboot the gripper.
+        enable_torque_service (str): Service to enable torque on the gripper.
+        index (int): Index of the gripper joint in the joint states message.
+        publish_frequency (float): Frequency to publish gripper state.
+        max_joint_delay (float): Maximum delay for joint state updates.
+        max_delta (float): Maximum change in gripper value per update.
+        use_gripper_command_action (bool): Whether to use GripperCommandAction.
     """
 
     min_value: float
@@ -25,6 +38,8 @@ class GripperConfig:
     publish_frequency: float = 30.0
     max_joint_delay: float = 1.0
     max_delta: float = 0.1
+    use_gripper_command_action: bool = False
+    max_effort: float = 10.0
 
     @classmethod
     def from_yaml(cls, path: str | Path, **overrides) -> "GripperConfig":  # noqa: ANN003

{crisp_python-3.1.0 → crisp_python-3.2.0}/crisp_py/robot/robot_config.py

@@ -202,18 +202,18 @@ class IiwaConfig(RobotConfig):
 
     joint_names: list = field(
         default_factory=lambda: [
-            "joint_a1",
-            "joint_a2",
-            "joint_a3",
-            "joint_a4",
-            "joint_a5",
-            "joint_a6",
-            "joint_a7",
+            "lbr_A1",
+            "lbr_A2",
+            "lbr_A3",
+            "lbr_A4",
+            "lbr_A5",
+            "lbr_A6",
+            "lbr_A7",
         ]
     )
     home_config: list = field(
         default_factory=lambda: [
-            0,
+            np.pi / 2,
             -np.pi / 4,
             0,
             -3 * np.pi / 4,
@@ -222,8 +222,8 @@ class IiwaConfig(RobotConfig):
             np.pi / 4,
         ]
     )
-    base_frame: str = "iiwa_base"
-    target_frame: str = "tool0"
+    base_frame: str = "world"
+    target_frame: str = "lbr_link_ee"
 
 
 @dataclass
@@ -258,11 +258,44 @@ class SO101Config(RobotConfig):
     target_frame: str = "Fixed_Gripper"
 
 
+@dataclass
+class DynaArmConfig(RobotConfig):
+    """Configuration specific to DynaArm robots.
+
+    Provides default values for frame names, joint names, and home configuration
+    specifically for DynaArm robots.
+    """
+
+    joint_names: list = field(
+        default_factory=lambda: [
+            "shoulder_rotation",
+            "shoulder_flexion",
+            "elbow_flexion",
+            "forearm_rotation",
+            "wrist_flexion",
+            "wrist_rotation",
+        ]
+    )
+    home_config: list = field(
+        default_factory=lambda: [
+            2.4,
+            -0.15,
+            2.0,
+            0.0,
+            1.26,
+            0.0,
+        ],
+    )
+    base_frame: str = "base_link"
+    target_frame: str = "flange"
+    cartesian_impedance_controller_name: str = "crisp_cartesian_controller"
+
+
 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')
+        robot_type (str): Type of robot ('franka', 'kinova', 'iiwa', 'so101', 'dynaarm')
         **kwargs: Additional keyword arguments to override default configuration
 
     Returns:
@@ -283,7 +316,9 @@ def make_robot_config(robot_type: str, **kwargs) -> RobotConfig:  # noqa: ANN003
         return IiwaConfig(**kwargs)
     elif robot_type == "so101":
         return SO101Config(**kwargs)
+    elif robot_type == "dynaarm":
+        return DynaArmConfig(**kwargs)
     else:
         raise ValueError(
-            f"Unsupported robot type: {robot_type}. Supported types: franka, panda, kinova, iiwa, so101"
+            f"Unsupported robot type: {robot_type}. Supported types: franka, panda, kinova, iiwa, so101, dynaarm"
         )

{crisp_python-3.1.0 → crisp_python-3.2.0/crisp_python.egg-info}/PKG-INFO

@@ -1,6 +1,6 @@
 Metadata-Version: 2.4
 Name: crisp-python
-Version: 3.1.0
+Version: 3.2.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-3.1.0 → crisp_python-3.2.0}/crisp_python.egg-info/SOURCES.txt

@@ -25,12 +25,15 @@ crisp_py/config/grippers/gripper_left.yaml
 crisp_py/config/grippers/gripper_left_config.yaml
 crisp_py/config/grippers/gripper_right.yaml
 crisp_py/config/grippers/gripper_right_config.yaml
+crisp_py/config/grippers/gripper_robotiq_2f85.yaml
 crisp_py/config/grippers/gripper_uncalibrated.yaml
 crisp_py/config/grippers/gripper_with_trigger.yaml
 crisp_py/config/grippers/trigger_left.yaml
+crisp_py/config/robots/dynaarm.yaml
 crisp_py/config/robots/fr3.yaml
 crisp_py/config/robots/fr3_left.yaml
 crisp_py/config/robots/fr3_right.yaml
+crisp_py/config/robots/iiwa14.yaml
 crisp_py/config/robots/panda.yaml
 crisp_py/config/sensors/anyskin_sensor.yaml
 crisp_py/config/sensors/force_torque_sensor.yaml

{crisp_python-3.1.0 → crisp_python-3.2.0}/pyproject.toml

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