beginning of major URDF refactor

Author: lukestroh

README.md

@@ -25,6 +25,9 @@ urdf generic launch CLI test:
 
 ## Installation
 
+#### Requirements
+ur, linear_slider, franka-emika
+
 #### General use
 ```
 python -m pip install .

main.py

@@ -1,5 +1,6 @@
 #!/usr/bin/env python3
-
+from pybullet_tree_sim.camera import Camera
+from pybullet_tree_sim.time_of_flight import TimeOfFlight
 from pybullet_tree_sim.pruning_environment import PruningEnv
 from pybullet_tree_sim.tree import Tree
 from pybullet_tree_sim.utils.pyb_utils import PyBUtils
@@ -11,16 +12,23 @@
 from zenlog import log
 
 
-def main():
-    # TODO: CLI args?
-    cam_dfov = 65
-    cam_height = 8
-    cam_width = 8
+def main():    
+    pbutils = PyBUtils(renders=False)
+
+    # Init sensors
+    sensor_config = {
+        "camera0": {
+            'sensor': Camera(pbutils, sensor_name="realsense_d435i"),
+            'tf_frame': "mock_pruner__camera0",
+        }
+    }
+    
 
-    pbutils = PyBUtils(renders=True, cam_width=cam_width, cam_height=cam_height, dfov=cam_dfov)
+    
     penv = PruningEnv(
-        pbutils=pbutils, load_robot=True, robot_pos=[0, 1, 0], verbose=True, cam_width=cam_width, cam_height=cam_height
+        pbutils=pbutils, load_robot=True, sensor_config=sensor_config, robot_pos=[0, 2, 0], verbose=True,
     )
+    
 
     penv.activate_shape(shape="cylinder", radius=2 * 0.0254, height=2.0, orientation=[0, np.pi / 2, 0])
     # penv.load_tree(
@@ -43,14 +51,14 @@ def main():
     # Simulation loop
     while True:
         try:
-            view_matrix = penv.ur5.get_view_mat_at_curr_pose(0, 0, [0, 0, 0])
+            tof0_view_matrix = penv.robot.get_view_mat_at_curr_pose(camera=penv.robot.sensors['mock_pruner__camera0'])
             rgbd = penv.pbutils.get_rgbd_at_cur_pose(type="robot", view_matrix=view_matrix)
             keys_pressed = penv.get_key_pressed()
             action = penv.get_key_action(keys_pressed=keys_pressed)
             action = action.reshape((6, 1))
-            jv, jacobian = penv.ur5.calculate_joint_velocities_from_ee_velocity_dls(end_effector_velocity=action)
-            penv.ur5.action = jv
-            singularity = penv.ur5.set_joint_velocities(penv.ur5.action)
+            jv, jacobian = penv.robot.calculate_joint_velocities_from_ee_velocity_dls(end_effector_velocity=action)
+            penv.robot.action = jv
+            singularity = penv.robot.set_joint_velocities(penv.robot.action)
             penv.pbutils.pbclient.stepSimulation()
             time.sleep(0.01)
         except KeyboardInterrupt:

pybullet_tree_sim/config/robot/cart.yaml

@@ -0,0 +1 @@
+cart_prefix: cart__

pybullet_tree_sim/config/robot/linear_slider.yaml

@@ -1,3 +1,2 @@
 base_type: linear_slider
-base_parent: world
 base_prefix: linear_slider__

pybullet_tree_sim/config/robot/mock_pruner.yaml

@@ -1,5 +1,5 @@
 eef_type: mock_pruner
-eef_parent: ur5e__
+eef_parent: ur5e__ # TODO: take care of __ in urdf
 eef_prefix: mock_pruner__
 tof0_offset_x: "0.02"
 tof1_offset_x: "-0.02"

pybullet_tree_sim/config/robot/robot.yaml

@@ -1,4 +1,6 @@
 robot_stack:
+    # - farm-ng
+    - cart
     # - linear_slider
     - ur5e
     - mock_pruner

pybullet_tree_sim/config/robot/ur5e.yaml

@@ -1,5 +1,2 @@
-arm_type: ur5e
-# arm_parent: tool0
-arm_prefix: ur5e__
-tf_prefix: ur5e__
+ur_prefix: ur5e__
 ur_type: ur5e

pybullet_tree_sim/pruning_environment.py

@@ -58,6 +58,7 @@ def __init__(
         # angle_threshold_perp: float = 0.52,
         # angle_threshold_point: float = 0.52,
         pbutils: PyBUtils,
+        sensor_config: dict,
         # cam_width: int = 424,
         # cam_height: int = 240,
         evaluate: bool = False,
@@ -106,23 +107,7 @@ def __init__(
         self.tree_count = tree_count
         self.is_goal_state = False
 
-        # sensor types
-        self.sensor_attributes = {}  # TODO: Load sensor types from config files
-        camera_configs_path = os.path.join(CONFIG_PATH, "camera")
-        camera_configs_files = glob.glob(os.path.join(camera_configs_path, "*.yaml"))
-        for file in camera_configs_files:
-            yamlcontent = yutils.load_yaml(file)
-            for key, value in yamlcontent.items():
-                self.sensor_attributes[key] = value
-
-        tof_configs_path = os.path.join(CONFIG_PATH, "tof")
-        tof_configs_files = glob.glob(os.path.join(tof_configs_path, "*.yaml"))
-        for file in tof_configs_files:
-            yamlcontent = yutils.load_yaml(file)
-            for key, value in yamlcontent.items():
-                self.sensor_attributes[key] = value
-
-        # log.warning(self.sensor_attributes)
+        
 
         # self.cam_width = cam_width
         # self.cam_height = cam_height
@@ -149,24 +134,47 @@ def __init__(
             "SUPPORT": None,
         }
 
-        # Tree parameters
-        # self.tree_goal_pos = None
-        # self.tree_goal_or = None
-        # self.tree_goal_normal = None
-        # self.tree_urdf_path: str | None = None
-        # self.tree_pos = np.zeros(3, dtype=float)
-        # self.tree_orientation = np.zeros(3, dtype=float)
-        # self.tree_scale: float = 1.0
-
         self.trees = {}
         self.debouce_time = 0.5
         self.last_button_push_time = time.time()
 
-        # UR5 Robot
+        # Load Robot
         if load_robot:
-            self.ur5 = self.load_robot(
-                type=robot_type, robot_pos=robot_pos, robot_orientation=robot_orientation, randomize_pose=False
-            )
+            self.robot = Robot(pbclient=self.pbutils.pbclient)
+            # self.ur5 = self.load_robot(
+            #     type=robot_type, robot_pos=robot_pos, robot_orientation=robot_orientation, randomize_pose=False
+            # )
+    
+        # Load all sensor attributes. # TODO: Load only the required sensor attributes
+        self._load_sensor_attributes()
+        
+        self.sensor_config = sensor_config
+        self._assign_tf_frame_to_sensors(self.sensor_config)
+        # log.warning(self.sensor_attributes)
+        return
+        
+    def _load_sensor_attributes(self):
+        self.sensor_attributes = {}
+        camera_configs_path = os.path.join(CONFIG_PATH, "camera")
+        camera_configs_files = glob.glob(os.path.join(camera_configs_path, "*.yaml"))
+        for file in camera_configs_files:
+            yamlcontent = yutils.load_yaml(file)
+            for key, value in yamlcontent.items():
+                self.sensor_attributes[key] = value
+        tof_configs_path = os.path.join(CONFIG_PATH, "tof")
+        tof_configs_files = glob.glob(os.path.join(tof_configs_path, "*.yaml"))
+        for file in tof_configs_files:
+            yamlcontent = yutils.load_yaml(file)
+            for key, value in yamlcontent.items():
+                self.sensor_attributes[key] = value
+        return
+        
+    def _assign_tf_frame_to_sensors(self, sensor_config: dict):
+        for sensor_name, conf in sensor_config.items():
+            sensor = conf['sensor']
+            sensor.tf_frame = conf['tf_frame']
+            log.warn(f"{sensor.params}")
+            sensor.tf_frame_index = self.robot.robot_conf['joint_info']['mock_pruner__base--camera0']['id']
         return
 
     def load_robot(self, type: str, robot_pos: ArrayLike, robot_orientation: ArrayLike, randomize_pose: bool = False):
@@ -182,7 +190,7 @@ def load_robot(self, type: str, robot_pos: ArrayLike, robot_orientation: ArrayLi
             #     randomize_pose=randomize_pose,
             #     verbose=self.verbose,
             # )
-            robot = Robot()
+            robot = Robot(pbclient=self.pbutils.pbclient)
 
         else:
             raise NotImplementedError(f"Robot type {type} not implemented")
@@ -208,7 +216,7 @@ def load_tree(  # TODO: Clean up Tree init vs create_tree, probably not needed.
         if tree_urdf_path is not None:
             if not os.path.exists(tree_urdf_path):
                 raise OSError(
-                    f"There do not seem to be any files of that name, please check your path. Given path was {tree_urdf_path}"
+                    f"There do not seem to be any files of that name, please check your path: {tree_urdf_path}"
                 )
 
         # Get tree object
@@ -567,10 +575,6 @@ def get_key_action(self, keys_pressed: list) -> np.ndarray:
             keys_pressed = []
         return action
 
-    def run_sim(self) -> int:
-
-        return 0
-
 
 def main():
     # data = np.zeros((cam_width, cam_height), dtype=float)