Added manipulator poses, joint count, and end effector link to config

clearpathrobotics · Feb 7, 2025 · e1aa8a3 · e1aa8a3
1 parent aecd53a
commit e1aa8a3
Show file tree

Hide file tree

Showing 10 changed files with 320 additions and 1 deletion.
diff --git a/clearpath_config/manipulators/types/arms.py b/clearpath_config/manipulators/types/arms.py
@@ -43,6 +43,7 @@ class BaseArm(BaseManipulator):
     DEFAULT_IP_PORT = 10000
 
     URDF_PARAMETERS = {}
+    END_EFFECTOR_LINK = "end_effector"
 
     def __init__(
             self,
@@ -80,7 +81,7 @@ def set_idx(self, idx: int) -> None:
             self.ip = self.get_ip_from_idx(idx)
         if self.gripper:
             self.gripper.name = self.name + '_gripper'
-            self.gripper.parent = self.name + '_end_effector_link'
+            self.gripper.parent = self.name + '_' + self.END_EFFECTOR_LINK
 
     @property
     def ip(self) -> str:
@@ -138,18 +139,26 @@ def get_urdf_parameters(self) -> dict:
 
 class KinovaGen3Dof6(BaseArm):
     MANIPULATOR_MODEL = "kinova_gen3_6dof"
+    JOINT_COUNT = 6
+    END_EFFECTOR_LINK = "end_effector_link"
 
 
 class KinovaGen3Dof7(BaseArm):
     MANIPULATOR_MODEL = "kinova_gen3_7dof"
+    JOINT_COUNT = 7
+    END_EFFECTOR_LINK = "end_effector_link"
 
 
 class KinovaGen3Lite(BaseArm):
     MANIPULATOR_MODEL = "kinova_gen3_lite"
+    JOINT_COUNT = 6
+    END_EFFECTOR_LINK = "end_effector_link"
 
 
 class UniversalRobots(BaseArm):
     MANIPULATOR_MODEL = "universal_robots"
+    JOINT_COUNT = 6
+    END_EFFECTOR_LINK = "tool0"
 
     # Description Variables
     UR_TYPE = 'ur_type'

diff --git a/clearpath_config/manipulators/types/grippers.py b/clearpath_config/manipulators/types/grippers.py
@@ -35,14 +35,17 @@ class BaseGripper(BaseManipulator):
 
 class Kinova2FLite(BaseGripper):
     MANIPULATOR_MODEL = "kinova_2f_lite"
+    JOINT_COUNT = 1
 
 
 class Robotiq2F85(BaseGripper):
     MANIPULATOR_MODEL = "robotiq_2f_85"
+    JOINT_COUNT = 1
 
 
 class Robotiq2F140(BaseGripper):
     MANIPULATOR_MODEL = "robotiq_2f_140"
+    JOINT_COUNT = 1
 
 
 class Gripper():

diff --git a/clearpath_config/manipulators/types/manipulator.py b/clearpath_config/manipulators/types/manipulator.py
@@ -33,11 +33,66 @@
 from clearpath_config.common.utils.dictionary import flatten_dict, unflatten_dict
 
 
+class ManipulatorPose():
+
+    def __init__(
+            self,
+            joint_count,
+            name: str = None,
+            joints: List = None,
+            ) -> None:
+        self.joint_count = joint_count
+        if name:
+            self.name = name
+        else:
+            self._name = ''
+        if joints:
+            self.joints = joints
+        else:
+            self._joints = []
+
+    @property
+    def name(self) -> str:
+        return self._name
+
+    @name.setter
+    def name(self, name: str) -> None:
+        assert isinstance(name, str), (
+            "Manipulator pose name must be of type str")
+        self._name = name
+
+    @property
+    def joints(self) -> list:
+        return self._joints
+
+    @joints.setter
+    def joints(self, joints: List) -> None:
+        assert isinstance(joints, list), (
+            "Manipulator pose joints must be of type list")
+        assert len(joints) == self.joint_count, (
+            f"Manipulator pose joints must of length {self.joint_count}, got {len(joints)}")
+        self._joints = joints
+
+    def to_dict(self) -> dict:
+        return {
+            'name': self.name,
+            'joints': self.joints,
+        }
+
+    def from_dict(self, d: dict) -> None:
+        assert isinstance(d, dict), ("Poses in list must be of type dict.")
+        assert 'name' in d, ("Pose must have a name entry.")
+        assert 'joints' in d, ("Pose must have a joints entry.")
+        self.name = d['name']
+        self.joints = d['joints']
+
+
 class BaseManipulator(IndexedAccessory):
     MANIPULATOR_MODEL = "base"
     MANIPULATOR_TYPE = "manipulator"
     ROS_PARAMETERS = {}
     ROS_PARAMETERS_TEMPLATE = {}
+    JOINT_COUNT = 0
 
     class ROSParameter:
         def __init__(
@@ -60,6 +115,7 @@ def __init__(
             xyz: List[float] = Accessory.XYZ,
             rpy: List[float] = Accessory.RPY
             ) -> None:
+        self.poses = []
         # ROS Parameters
         self.ros_parameters_template = ros_parameters_template
         self.ros_parameters = ros_parameters
@@ -72,6 +128,9 @@ def to_dict(self) -> dict:
         d['xyz'] = self.get_xyz()
         d['rpy'] = self.get_rpy()
         d['ros_parameters'] = self.get_ros_parameters()
+        d['poses'] = []
+        for pose in self.poses:
+            d['poses'].append(pose.to_dict())
         return d
 
     def from_dict(self, d: dict) -> None:
@@ -83,6 +142,8 @@ def from_dict(self, d: dict) -> None:
             self.set_rpy(d['rpy'])
         if 'ros_parameters' in d:
             self.set_ros_parameters(d['ros_parameters'])
+        if 'poses' in d:
+            self.poses = d['poses']
 
     @classmethod
     def get_manipulator_model(cls) -> str:
@@ -144,3 +205,17 @@ def setter(self, prop: property):
 
     def getter(self, prop: property):
         return prop.fget.__get__(self)
+
+    @property
+    def poses(self) -> List:
+        return self._poses
+
+    @poses.setter
+    def poses(self, pose_list: List) -> None:
+        assert isinstance(pose_list, list), ("List of poses must be of type list.")
+        poses_ = []
+        for pose in pose_list:
+            manipulator_pose = ManipulatorPose(self.JOINT_COUNT)
+            manipulator_pose.from_dict(pose)
+            poses_.append(manipulator_pose)
+        self._poses = poses_
diff --git a/clearpath_config/sample/manipulators/arms/kinova_gen3_6dof.yaml b/clearpath_config/sample/manipulators/arms/kinova_gen3_6dof.yaml
@@ -0,0 +1,29 @@
+serial_number: a200-0000
+version: 0
+system:
+  hosts:
+    - hostname: cpr-a200-0000
+      ip: 192.168.131.1
+  ros2:
+    namespace: a200_0000
+platform:
+  attachments:
+    - name: front_bumper
+      type: a200.bumper
+      parent: front_bumper_mount
+    - name: rear_bumper
+      type: a200.bumper
+      parent: rear_bumper_mount
+    - name: top_plate
+      type: a200.top_plate
+manipulators:
+  arms:
+    - model: kinova_gen3_6dof
+      parent: default_mount
+      xyz: [0.0, 0.0, 0.0]
+      rpy: [0.0, 0.0, 0.0]
+      ip: 192.168.131.40
+      port: 10000
+      poses:
+        - name: custom
+          joints: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
diff --git a/clearpath_config/sample/manipulators/arms/kinova_gen3_7dof.yaml b/clearpath_config/sample/manipulators/arms/kinova_gen3_7dof.yaml
@@ -0,0 +1,29 @@
+serial_number: a200-0000
+version: 0
+system:
+  hosts:
+    - hostname: cpr-a200-0000
+      ip: 192.168.131.1
+  ros2:
+    namespace: a200_0000
+platform:
+  attachments:
+    - name: front_bumper
+      type: a200.bumper
+      parent: front_bumper_mount
+    - name: rear_bumper
+      type: a200.bumper
+      parent: rear_bumper_mount
+    - name: top_plate
+      type: a200.top_plate
+manipulators:
+  arms:
+    - model: kinova_gen3_7dof
+      parent: default_mount
+      xyz: [0.0, 0.0, 0.0]
+      rpy: [0.0, 0.0, 0.0]
+      ip: 192.168.131.40
+      port: 10000
+      poses:
+        - name: custom
+          joints: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
diff --git a/clearpath_config/sample/manipulators/arms/kinova_gen3_lite.yaml b/clearpath_config/sample/manipulators/arms/kinova_gen3_lite.yaml
@@ -0,0 +1,36 @@
+serial_number: a200-0000
+version: 0
+system:
+  hosts:
+    - hostname: cpr-a200-0000
+      ip: 192.168.131.1
+  ros2:
+    namespace: a200_0000
+platform:
+  attachments:
+    - name: front_bumper
+      type: a200.bumper
+      parent: front_bumper_mount
+    - name: rear_bumper
+      type: a200.bumper
+      parent: rear_bumper_mount
+    - name: top_plate
+      type: a200.top_plate
+manipulators:
+  arms:
+    - model: kinova_gen3_lite
+      parent: default_mount
+      xyz: [0.0, 0.0, 0.0]
+      rpy: [0.0, 0.0, 0.0]
+      ip: 192.168.131.40
+      port: 10000
+      poses:
+        - name: custom
+          joints: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+      gripper:
+        model: kinova_2f_lite
+        xyz: [0.0, 0.0, 0.0]
+        rpy: [0.0, 0.0, 0.0]
+        poses:
+          - name: custom
+            joints: [0.0]
diff --git a/clearpath_config/sample/manipulators/arms/universal_robots.yaml b/clearpath_config/sample/manipulators/arms/universal_robots.yaml
@@ -0,0 +1,31 @@
+serial_number: a200-0000
+version: 0
+system:
+  hosts:
+    - hostname: cpr-a200-0000
+      ip: 192.168.131.1
+  ros2:
+    namespace: a200_0000
+platform:
+  attachments:
+    - name: front_bumper
+      type: a200.bumper
+      parent: front_bumper_mount
+    - name: rear_bumper
+      type: a200.bumper
+      parent: rear_bumper_mount
+    - name: top_plate
+      type: a200.top_plate
+manipulators:
+  arms:
+    - model: universal_robots
+      parent: default_mount
+      xyz: [0.0, 0.0, 0.0]
+      rpy: [0.0, 0.0, 0.0]
+      ur_type: ur5e
+      robbot_ip: 192.168.131.40
+      headless_mode: false
+      kinematics_parameters_file: "$(find ur_description)/config/ur5e/default_kinematics.yaml"
+      poses:
+        - name: custom
+          joints: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
diff --git a/clearpath_config/sample/manipulators/grippers/kinova_2f_lite.yaml b/clearpath_config/sample/manipulators/grippers/kinova_2f_lite.yaml
@@ -0,0 +1,36 @@
+serial_number: a200-0000
+version: 0
+system:
+  hosts:
+    - hostname: cpr-a200-0000
+      ip: 192.168.131.1
+  ros2:
+    namespace: a200_0000
+platform:
+  attachments:
+    - name: front_bumper
+      type: a200.bumper
+      parent: front_bumper_mount
+    - name: rear_bumper
+      type: a200.bumper
+      parent: rear_bumper_mount
+    - name: top_plate
+      type: a200.top_plate
+manipulators:
+  arms:
+    - model: kinova_gen3_lite
+      parent: default_mount
+      xyz: [0.0, 0.0, 0.0]
+      rpy: [0.0, 0.0, 0.0]
+      ip: 192.168.131.40
+      port: 10000
+      poses:
+        - name: custom
+          joints: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+      gripper:
+        model: kinova_2f_lite
+        xyz: [0.0, 0.0, 0.0]
+        rpy: [0.0, 0.0, 0.0]
+        poses:
+          - name: custom
+            joints: [0.0]
diff --git a/clearpath_config/sample/manipulators/grippers/robotiq_2f_140.yaml b/clearpath_config/sample/manipulators/grippers/robotiq_2f_140.yaml
@@ -0,0 +1,38 @@
+serial_number: a200-0000
+version: 0
+system:
+  hosts:
+    - hostname: cpr-a200-0000
+      ip: 192.168.131.1
+  ros2:
+    namespace: a200_0000
+platform:
+  attachments:
+    - name: front_bumper
+      type: a200.bumper
+      parent: front_bumper_mount
+    - name: rear_bumper
+      type: a200.bumper
+      parent: rear_bumper_mount
+    - name: top_plate
+      type: a200.top_plate
+manipulators:
+  arms:
+    - model: universal_robots
+      parent: default_mount
+      xyz: [0.0, 0.0, 0.0]
+      rpy: [0.0, 0.0, 0.0]
+      ur_type: ur5e
+      robbot_ip: 192.168.131.40
+      headless_mode: false
+      kinematics_parameters_file: "$(find ur_description)/config/ur5e/default_kinematics.yaml"
+      poses:
+        - name: custom
+          joints: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+      gripper:
+        model: robotiq_2f_140
+        xyz: [0.0, 0.0, 0.0]
+        rpy: [0.0, 0.0, 0.0]
+        poses:
+          - name: custom
+            joints: [0.0]