Updated for use with rtb specified path for ghost robot

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

Hide file tree

Showing 3 changed files with 8 additions and 3 deletions.
diff --git a/armer/models/URDFRobot.py b/armer/models/URDFRobot.py
@@ -30,6 +30,8 @@ def __init__(self,
                **kwargs):
 
     if urdf_file:
+      # Update a common variable for the specific toolbox (rtb) path for use as a ghost
+      self.urdf_rtb_path = urdf_file
       links, name, urdf_string, urdf_filepath = self.URDF_read(urdf_file)
     else:
       links, name, urdf_string, urdf_filepath = URDFRobot.URDF_read_description(wait=wait_for_description)

diff --git a/armer/robots/ROSRobot.py b/armer/robots/ROSRobot.py
@@ -41,7 +41,7 @@
 from armer_msgs.msg import *
 from armer_msgs.srv import *
 
-# TESTING NEW IMPORTS FOR TRAJ DISPLAY (MOVEIT)
+# TESTING NEW IMPORTS FOR TRAJ DISPLAY 
 from trajectory_msgs.msg import JointTrajectoryPoint, JointTrajectory
 
 # TESTING NEW IMPORTS FOR DYNAMIC OBJECT MARKER DISPLAY (RVIZ)
@@ -210,10 +210,11 @@ def __init__(self,
         # Initialise a 'ghost' robot instance for trajectory collision checking prior to execution
         # NOTE: Used to visualise the trajectory as a simple marker list in RVIZ for debugging
         # NOTE: there was an issue identified around using the latest franka-emika panda description
+        # NOTE: the urdf_rtb_path is set by URDFRobot on initialisation if a path to the toolbox descriptions is specified
         self.robot_ghost = URDFRobot(
             wait_for_description=False, 
             gripper=self.gripper,
-            urdf_file=self.urdf_filepath,
+            urdf_file=self.urdf_rtb_path,
             collision_check_start_link=self.collision_sliced_links[-1].name, 
             collision_check_stop_link=self.collision_sliced_links[0].name
         )

diff --git a/cfg/panda_sim.yaml b/cfg/panda_sim.yaml
@@ -3,7 +3,9 @@ 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_link8'
+      gripper: 'panda_hand'
+      collision_check_start_link: panda_hand
+      collision_check_stop_link: panda_link7
     singularity_thresh: 0.02
 backend: 
   type: roboticstoolbox.backends.swift.Swift