From 99b4e82a2e7b8058e60e0bee8e8a417c5f20fa70 Mon Sep 17 00:00:00 2001
From: lukestroh <luke.strohbehn@gmail.com>
Date: Mon, 11 Nov 2024 16:49:19 -0800
Subject: [PATCH] testing the robot class

---
 pybullet_tree_sim/config/robot/robot.yaml |  0
 pybullet_tree_sim/pruning_environment.py  |  1 +
 pybullet_tree_sim/robot.py                | 28 ++++++++++++++++++++++-
 3 files changed, 28 insertions(+), 1 deletion(-)
 create mode 100644 pybullet_tree_sim/config/robot/robot.yaml

diff --git a/pybullet_tree_sim/config/robot/robot.yaml b/pybullet_tree_sim/config/robot/robot.yaml
new file mode 100644
index 0000000..e69de29
diff --git a/pybullet_tree_sim/pruning_environment.py b/pybullet_tree_sim/pruning_environment.py
index 2a624e4..39197c8 100644
--- a/pybullet_tree_sim/pruning_environment.py
+++ b/pybullet_tree_sim/pruning_environment.py
@@ -582,6 +582,7 @@ def main():
     # data = data.reshape((cam_width * cam_height, 1), order="F")
 
     # log.warning(f"joint angles: {penv.ur5.get_joint_angles()}")
+    
     return
 
 
diff --git a/pybullet_tree_sim/robot.py b/pybullet_tree_sim/robot.py
index 82c2ab2..4128b7e 100644
--- a/pybullet_tree_sim/robot.py
+++ b/pybullet_tree_sim/robot.py
@@ -1,3 +1,7 @@
+#!/usr/bin/env python3
+from pybullet_tree_sim.utils.pyb_utils import PyBUtils
+import pybullet_tree_sim.utils.xacro_utils as xutils
+
 from typing import Optional, Tuple
 import numpy as np
 import pybullet
@@ -7,7 +11,7 @@
 class Robot:
     def __init__(
         self,
-        con,
+        pbclient,
         robot_type: str,
         robot_urdf_path: str,
         tool_link_name: str,
@@ -46,6 +50,7 @@ def __init__(
         self.control_joints = control_joints
         self.robot_collision_filter_idxs = robot_collision_filter_idxs
         self.setup_robot()
+        return
 
     def setup_robot(self):
         if self.robot is not None:
@@ -421,3 +426,24 @@ def unset_collision_filter_tree(self, collision_objects):
         for i in collision_objects.values():
             for j in range(self.num_joints):
                 self.con.setCollisionFilterPair(self.robot, i, j, 0, 1)
+
+
+def main():
+    from pybullet_tree_sim.utils.pyb_utils import PyBUtils
+    pbutils = PyBUtils(renders=True)
+    
+    robot = Robot(
+        pbclient=pbutils.pbclient,
+        robot_type='ur5e',
+        robot_urdf_path="",
+        tool_link_name="mock_pruner__tool0",
+        base_link_name="linear_slider__base_link",
+        
+    )
+
+    print(robot)
+
+    return
+
+if __name__ == "__main__":
+    main()