Add UR robot + generalize example to every robot (#76)

* add UR robot * generalise exemples for every robots
AndrejOrsula · Nov 25, 2024 · b9b82b8 · b9b82b8
1 parent 3ecad18
commit b9b82b8
Show file tree

Hide file tree

Showing 12 changed files with 87 additions and 53 deletions.
diff --git a/examples/ex_allow_collisions.py b/examples/ex_allow_collisions.py
@@ -13,7 +13,7 @@
 from rclpy.node import Node
 
 from pymoveit2 import MoveIt2
-from pymoveit2.robots import panda
+from pymoveit2.robots import panda as robot
 
 
 def main():
@@ -38,10 +38,10 @@ def main():
     # Create MoveIt 2 interface
     moveit2 = MoveIt2(
         node=node,
-        joint_names=panda.joint_names(),
-        base_link_name=panda.base_link_name(),
-        end_effector_name=panda.end_effector_name(),
-        group_name=panda.MOVE_GROUP_ARM,
+        joint_names=robot.joint_names(),
+        base_link_name=robot.base_link_name(),
+        end_effector_name=robot.end_effector_name(),
+        group_name=robot.MOVE_GROUP_ARM,
         callback_group=callback_group,
     )
 

diff --git a/examples/ex_clear_planning_scene.py b/examples/ex_clear_planning_scene.py
@@ -15,7 +15,7 @@
 from rclpy.node import Node
 
 from pymoveit2 import MoveIt2
-from pymoveit2.robots import panda
+from pymoveit2.robots import panda as robot
 
 
 def main():
@@ -42,13 +42,14 @@ def main():
     # Create callback group that allows execution of callbacks in parallel without restrictions
     callback_group = ReentrantCallbackGroup()
 
+
     # Create MoveIt 2 interface
     moveit2 = MoveIt2(
         node=node,
-        joint_names=panda.joint_names(),
-        base_link_name=panda.base_link_name(),
-        end_effector_name=panda.end_effector_name(),
-        group_name=panda.MOVE_GROUP_ARM,
+        joint_names=robot.joint_names(),
+        base_link_name=robot.base_link_name(),
+        end_effector_name=robot.end_effector_name(),
+        group_name=robot.MOVE_GROUP_ARM,
         callback_group=callback_group,
     )
 

diff --git a/examples/ex_collision_mesh.py b/examples/ex_collision_mesh.py
@@ -17,7 +17,7 @@
 from rclpy.node import Node
 
 from pymoveit2 import MoveIt2
-from pymoveit2.robots import panda
+from pymoveit2.robots import panda as robot 
 
 DEFAULT_EXAMPLE_MESH = path.join(
     path.dirname(path.realpath(__file__)), "assets", "suzanne.stl"
@@ -50,10 +50,10 @@ def main():
     # Create MoveIt 2 interface
     moveit2 = MoveIt2(
         node=node,
-        joint_names=panda.joint_names(),
-        base_link_name=panda.base_link_name(),
-        end_effector_name=panda.end_effector_name(),
-        group_name=panda.MOVE_GROUP_ARM,
+        joint_names=robot.joint_names(),
+        base_link_name=robot.base_link_name(),
+        end_effector_name=robot.end_effector_name(),
+        group_name=robot.MOVE_GROUP_ARM,
         callback_group=callback_group,
     )
 

diff --git a/examples/ex_collision_primitive.py b/examples/ex_collision_primitive.py
@@ -14,7 +14,7 @@
 from rclpy.node import Node
 
 from pymoveit2 import MoveIt2
-from pymoveit2.robots import panda
+from pymoveit2.robots import panda as robot
 
 
 def main():
@@ -42,10 +42,10 @@ def main():
     # Create MoveIt 2 interface
     moveit2 = MoveIt2(
         node=node,
-        joint_names=panda.joint_names(),
-        base_link_name=panda.base_link_name(),
-        end_effector_name=panda.end_effector_name(),
-        group_name=panda.MOVE_GROUP_ARM,
+        joint_names=robot.joint_names(),
+        base_link_name=robot.base_link_name(),
+        end_effector_name=robot.end_effector_name(),
+        group_name=robot.MOVE_GROUP_ARM,
         callback_group=callback_group,
     )
 

diff --git a/examples/ex_fk.py b/examples/ex_fk.py
@@ -12,7 +12,7 @@
 from rclpy.node import Node
 
 from pymoveit2 import MoveIt2, MoveIt2State
-from pymoveit2.robots import panda
+from pymoveit2.robots import panda as robot
 
 
 def main():
@@ -42,10 +42,10 @@ def main():
     # Create MoveIt 2 interface
     moveit2 = MoveIt2(
         node=node,
-        joint_names=panda.joint_names(),
-        base_link_name=panda.base_link_name(),
-        end_effector_name=panda.end_effector_name(),
-        group_name=panda.MOVE_GROUP_ARM,
+        joint_names=robot.joint_names(),
+        base_link_name=robot.base_link_name(),
+        end_effector_name=robot.end_effector_name(),
+        group_name=robot.MOVE_GROUP_ARM,
         callback_group=callback_group,
     )
 

diff --git a/examples/ex_gripper.py b/examples/ex_gripper.py
@@ -13,7 +13,7 @@
 from rclpy.node import Node
 
 from pymoveit2 import GripperInterface
-from pymoveit2.robots import panda
+from pymoveit2.robots import panda as robot
 
 
 def main():
@@ -34,10 +34,10 @@ def main():
     # Create gripper interface
     gripper_interface = GripperInterface(
         node=node,
-        gripper_joint_names=panda.gripper_joint_names(),
-        open_gripper_joint_positions=panda.OPEN_GRIPPER_JOINT_POSITIONS,
-        closed_gripper_joint_positions=panda.CLOSED_GRIPPER_JOINT_POSITIONS,
-        gripper_group_name=panda.MOVE_GROUP_GRIPPER,
+        gripper_joint_names=robot.gripper_joint_names(),
+        open_gripper_joint_positions=robot.OPEN_GRIPPER_JOINT_POSITIONS,
+        closed_gripper_joint_positions=robot.CLOSED_GRIPPER_JOINT_POSITIONS,
+        gripper_group_name=robot.MOVE_GROUP_GRIPPER,
         callback_group=callback_group,
         gripper_command_action_name="gripper_action_controller/gripper_cmd",
     )

diff --git a/examples/ex_ik.py b/examples/ex_ik.py
@@ -12,7 +12,7 @@
 from rclpy.node import Node
 
 from pymoveit2 import MoveIt2, MoveIt2State
-from pymoveit2.robots import panda
+from pymoveit2.robots import panda as robot
 
 
 def main():
@@ -32,10 +32,10 @@ def main():
     # Create MoveIt 2 interface
     moveit2 = MoveIt2(
         node=node,
-        joint_names=panda.joint_names(),
-        base_link_name=panda.base_link_name(),
-        end_effector_name=panda.end_effector_name(),
-        group_name=panda.MOVE_GROUP_ARM,
+        joint_names=robot.joint_names(),
+        base_link_name=robot.base_link_name(),
+        end_effector_name=robot.end_effector_name(),
+        group_name=robot.MOVE_GROUP_ARM,
         callback_group=callback_group,
     )
 

diff --git a/examples/ex_joint_goal.py b/examples/ex_joint_goal.py
@@ -13,8 +13,7 @@
 from rclpy.node import Node
 
 from pymoveit2 import MoveIt2, MoveIt2State
-from pymoveit2.robots import panda
-
+from pymoveit2.robots import panda as robot
 
 def main():
     rclpy.init()
@@ -47,10 +46,10 @@ def main():
     # Create MoveIt 2 interface
     moveit2 = MoveIt2(
         node=node,
-        joint_names=panda.joint_names(),
-        base_link_name=panda.base_link_name(),
-        end_effector_name=panda.end_effector_name(),
-        group_name=panda.MOVE_GROUP_ARM,
+        joint_names=robot.joint_names(),
+        base_link_name=robot.base_link_name(),
+        end_effector_name=robot.end_effector_name(),
+        group_name=robot.MOVE_GROUP_ARM,
         callback_group=callback_group,
     )
     moveit2.planner_id = (

diff --git a/examples/ex_orientation_path_constraint.py b/examples/ex_orientation_path_constraint.py
@@ -12,7 +12,7 @@
 from rclpy.node import Node
 
 from pymoveit2 import MoveIt2, MoveIt2State
-from pymoveit2.robots import panda
+from pymoveit2.robots import panda as robot
 
 
 def main():
@@ -64,10 +64,10 @@ def main():
     # Create MoveIt 2 interface
     moveit2 = MoveIt2(
         node=node,
-        joint_names=panda.joint_names(),
-        base_link_name=panda.base_link_name(),
-        end_effector_name=panda.end_effector_name(),
-        group_name=panda.MOVE_GROUP_ARM,
+        joint_names=robot.joint_names(),
+        base_link_name=robot.base_link_name(),
+        end_effector_name=robot.end_effector_name(),
+        group_name=robot.MOVE_GROUP_ARM,
         callback_group=callback_group,
     )
 

diff --git a/examples/ex_pose_goal.py b/examples/ex_pose_goal.py
@@ -13,7 +13,7 @@
 from rclpy.node import Node
 
 from pymoveit2 import MoveIt2, MoveIt2State
-from pymoveit2.robots import panda
+from pymoveit2.robots import panda as robot
 
 
 def main():
@@ -43,10 +43,10 @@ def main():
     # Create MoveIt 2 interface
     moveit2 = MoveIt2(
         node=node,
-        joint_names=panda.joint_names(),
-        base_link_name=panda.base_link_name(),
-        end_effector_name=panda.end_effector_name(),
-        group_name=panda.MOVE_GROUP_ARM,
+        joint_names=robot.joint_names(),
+        base_link_name=robot.base_link_name(),
+        end_effector_name=robot.end_effector_name(),
+        group_name=robot.MOVE_GROUP_ARM,
         callback_group=callback_group,
     )
     moveit2.planner_id = (

diff --git a/examples/ex_servo.py b/examples/ex_servo.py
@@ -12,7 +12,7 @@
 from rclpy.node import Node
 
 from pymoveit2 import MoveIt2Servo
-from pymoveit2.robots import panda
+from pymoveit2.robots import panda as robot
 
 
 def main():
@@ -27,7 +27,7 @@ def main():
     # Create MoveIt 2 Servo interface
     moveit2_servo = MoveIt2Servo(
         node=node,
-        frame_id=panda.base_link_name(),
+        frame_id=robot.base_link_name(),
         callback_group=callback_group,
     )
 

diff --git a/pymoveit2/robots/ur.py b/pymoveit2/robots/ur.py
@@ -0,0 +1,34 @@
+from typing import List
+
+MOVE_GROUP_ARM: str = "ur_manipulator"
+MOVE_GROUP_GRIPPER: str = "gripper"
+
+prefix: str = ""
+
+OPEN_GRIPPER_JOINT_POSITIONS: List[float] = [0.04, 0.04]
+CLOSED_GRIPPER_JOINT_POSITIONS: List[float] = [0.0, 0.0]
+
+
+def joint_names(prefix: str = prefix) -> List[str]:
+    return [
+        prefix + "shoulder_pan_joint",
+        prefix + "shoulder_lift_joint",
+        prefix + "elbow_joint",
+        prefix + "wrist_1_joint",
+        prefix + "wrist_2_joint",
+        prefix + "wrist_3_joint",
+    ]
+
+
+def base_link_name(prefix: str = prefix) -> str:
+    return prefix + "base_link"
+
+
+def end_effector_name(prefix: str = prefix) -> str:
+    return prefix + "tool0"
+
+
+def gripper_joint_names(prefix: str = prefix) -> List[str]:
+    return [
+
+    ]