From f654620cd88562d2612d6a8e7c6cd3601a01d21f Mon Sep 17 00:00:00 2001
From: Dasun Gunasinghe <dasun.gunasinghe@qut.edu.au>
Date: Wed, 10 Jan 2024 10:11:24 +1000
Subject: [PATCH] Updated for use with rtb specified path for ghost robot

---
 armer/models/URDFRobot.py | 2 ++
 armer/robots/ROSRobot.py  | 5 +++--
 cfg/panda_sim.yaml        | 4 +++-
 3 files changed, 8 insertions(+), 3 deletions(-)

diff --git a/armer/models/URDFRobot.py b/armer/models/URDFRobot.py
index cc5c35d..9e04fd9 100644
--- 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
index ef224e0..d45053d 100644
--- 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
index f332b49..cd512fe 100644
--- 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