Updated config files for collision checking

qcr · Jan 10, 2024 · d34b62b · d34b62b
1 parent f654620
commit d34b62b
Show file tree

Hide file tree

Showing 6 changed files with 32 additions and 1 deletion.
diff --git a/armer/models/URDFRobot.py b/armer/models/URDFRobot.py
@@ -34,8 +34,14 @@ def __init__(self,
       self.urdf_rtb_path = urdf_file
       links, name, urdf_string, urdf_filepath = self.URDF_read(urdf_file)
     else:
+      self.urdf_rtb_path = None
       links, name, urdf_string, urdf_filepath = URDFRobot.URDF_read_description(wait=wait_for_description)
 
+    # Error handling on missing links (no description was validly provided)
+    if not links:
+      rospy.logerr(f"Could not define links (description error)")
+      return None
+
     self.gripper = gripper if gripper else URDFRobot.resolve_gripper(links)
     gripper_link = list(filter(lambda link: link.name == self.gripper, links))
 

diff --git a/cfg/panda_ur5_sim.yaml b/cfg/panda_ur5_sim.yaml
@@ -3,12 +3,17 @@ robots:
     model: 
       urdf_file: franka_description/robots/panda_arm_hand.urdf.xacro
       qr: [0, -0.3, 0, -2.2, 0, 2.0, 0.78539816339]
+      gripper: 'panda_hand'
+      collision_check_start_link: panda_hand
+      collision_check_stop_link: panda_link7
     singularity_thresh: 0.02
   - name: ur5
     model: 
       urdf_file: ur_description/urdf/ur5_robot.urdf.xacro
       qr: [-1.57, -0.96, -2.18, -1.57, 1.57, 3.14]
       gripper: tool0
+      collision_check_start_link: wrist_3_link
+      collision_check_stop_link: wrist_1_link
     singularity_thresh: 0.02
     origin: [-1, 0, 0, 0, 0, 0] # (xyzrpy)
 publish_transforms: true

diff --git a/cfg/ur10_sim.yaml b/cfg/ur10_sim.yaml
@@ -0,0 +1,12 @@
+robots:
+  - name: arm # default namespace
+    model:
+      urdf_file: ur_description/urdf/ur10_robot.urdf.xacro
+      qr: [-1.57, -0.96, -2.18, -1.57, 1.57, 3.14]
+      gripper: tool0
+      collision_check_start_link: wrist_3_link
+      collision_check_stop_link: wrist_1_link
+    singularity_thresh: 0.02
+backend: 
+  type: roboticstoolbox.backends.swift.Swift
+publish_transforms: true
diff --git a/cfg/ur10_ur10_sim.yaml b/cfg/ur10_ur10_sim.yaml
@@ -4,12 +4,16 @@ robots:
       urdf_file: ur_description/urdf/ur10_robot.urdf.xacro
       qr: [-1.57, -0.96, -2.18, -1.57, 1.57, 3.14]
       gripper: tool0
+      collision_check_start_link: wrist_3_link
+      collision_check_stop_link: wrist_1_link
     singularity_thresh: 0.02
   - name: ur10_2
     model: 
       urdf_file: ur_description/urdf/ur10_robot.urdf.xacro
       qr: [-1.57, -0.96, -2.18, -1.57, 1.57, 3.14]
       gripper: tool0
+      collision_check_start_link: wrist_3_link
+      collision_check_stop_link: wrist_1_link
     singularity_thresh: 0.02
     origin: [1, 0, 0, 0, 0, 3.14] # (xyzrpy)
 publish_transforms: true

diff --git a/cfg/ur3_sim.yaml b/cfg/ur3_sim.yaml
@@ -2,8 +2,10 @@ robots:
   - name: arm # default namespace
     model: 
       urdf_file: ur_description/urdf/ur3_robot.urdf.xacro
-      gripper: tool0
       qr: [-1.57, -0.96, -2.18, -1.57, 1.57, 3.14]
+      gripper: tool0
+      collision_check_start_link: wrist_3_link
+      collision_check_stop_link: wrist_1_link
     singularity_thresh: 0.02
 backend: 
   type: roboticstoolbox.backends.swift.Swift

diff --git a/cfg/ur5_sim.yaml b/cfg/ur5_sim.yaml
@@ -4,6 +4,8 @@ robots:
       urdf_file: ur_description/urdf/ur5_robot.urdf.xacro
       qr: [-1.57, -0.96, -2.18, -1.57, 1.57, 3.14]
       gripper: tool0
+      collision_check_start_link: wrist_3_link
+      collision_check_stop_link: wrist_1_link
     singularity_thresh: 0.02
 backend: 
   type: roboticstoolbox.backends.swift.Swift