Feature/generic robot class urdf merge

.github/workflows/black.yaml

@@ -1,6 +1,8 @@
-name: Lint
+name: pybullet-tree-sim-workflow
 
-on: [push, pull_request, pull_request_review]
+on: 
+    pull_request:
+        branches: ['main', 'develop']
 
 jobs:
     lint:
@@ -9,4 +11,4 @@ jobs:
             - uses: actions/checkout@v4
             - uses: psf/black@stable
               with:
-                  options: "--check --line-length=120"
+                  options: "--check --verbose --diff --line-length=120 ./"

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,6 +1,8 @@
 #!/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.robot import Robot
 from pybullet_tree_sim.tree import Tree
 from pybullet_tree_sim.utils.pyb_utils import PyBUtils
 from pybullet_tree_sim.utils.camera_helpers import get_fov_from_dfov
@@ -12,17 +14,19 @@
 
 
 def main():
-    # TODO: CLI args?
-    cam_dfov = 65
-    cam_height = 8
-    cam_width = 8
+    pbutils = PyBUtils(renders=True)
+
+    robot = Robot(pbclient=pbutils.pbclient, position=[0, 1, 0], orientation=[0, 0, 0, 1])
 
-    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,
+        verbose=True,
     )
 
-    penv.activate_shape(shape="cylinder", radius=2 * 0.0254, height=2.0, orientation=[0, np.pi / 2, 0])
+    _1_inch = 0.0254
+    penv.activate_shape(shape="cylinder", radius=_1_inch * 2, height=2.85, orientation=[0, np.pi / 2, 0])
+    # penv.activate_shape(shape="cylinder", radius=0.01, height=2.85, orientation=[0, np.pi / 2, 0])
+
     # penv.load_tree(
     #     pbutils=pbutils,
     #     scale=1.0,
@@ -35,29 +39,40 @@ def main():
     # )
     # penv.activate_tree(tree_id_str="LPy_envy_tree1")
 
-    # Run the sim a little just to get the environment properly loaded.
+    # # Run the sim a little just to get the environment properly loaded.
     for i in range(100):
         pbutils.pbclient.stepSimulation()
         time.sleep(0.1)
 
-    # Simulation loop
+    # log.debug(robot.sensors['tof0'].tf_frame)
+
     while True:
         try:
-            view_matrix = penv.ur5.get_view_mat_at_curr_pose(0, 0, [0, 0, 0])
-            rgbd = penv.pbutils.get_rgbd_at_cur_pose(type="robot", view_matrix=view_matrix)
+            # log.debug(f"{robot.sensors['tof0']}")
+            tof0_view_matrix = robot.get_view_mat_at_curr_pose(camera=robot.sensors["tof0"])
+            tof0_rgbd = penv.pbutils.get_rgbd_at_cur_pose(
+                camera=robot.sensors["tof0"], type="robot", view_matrix=tof0_view_matrix
+            )
+            tof1_view_matrix = robot.get_view_mat_at_curr_pose(camera=robot.sensors["tof1"])
+            tof1_rgbd = penv.pbutils.get_rgbd_at_cur_pose(
+                camera=robot.sensors["tof1"], type="robot", view_matrix=tof1_view_matrix
+            )
+            # tof0_view_matrix = np.asarray(tof0_view_matrix).reshape((4, 4), order="F")
+            # log.debug(f"{tof0_view_matrix[:3, 3]}")
+
             keys_pressed = penv.get_key_pressed()
-            action = penv.get_key_action(keys_pressed=keys_pressed)
+            action = penv.get_key_action(robot=robot, 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)
+            joint_vel, jacobian = robot.calculate_joint_velocities_from_ee_velocity_dls(end_effector_velocity=action)
+            robot.action = joint_vel
+            singularity = robot.set_joint_velocities(robot.action)
             penv.pbutils.pbclient.stepSimulation()
             time.sleep(0.01)
         except KeyboardInterrupt:
             break
 
-    # penv.deactivate_tree(tree_id_str="LPy_envy_tree1")
-    penv.pbutils.pbclient.disconnect()
+    # # penv.deactivate_tree(tree_id_str="LPy_tree1")
+    # penv.pbutils.pbclient.disconnect()
     return
 
 

pybullet_tree_sim/camera.py

@@ -17,41 +17,50 @@
 
 
 class Camera(Sensor):
-    def __init__(self, pbutils, sensor_name: str, sensor_type: str = "camera") -> None:
+    def __init__(self, pbclient, sensor_name: str, sensor_type: str = "camera") -> None:
         # load sensor parameters from yaml file
         super().__init__(sensor_name, sensor_type)
         self.pan = 0
         self.tilt = 0
-        self.tf_frame: str = ""
+        self.xyz_offset = np.array([0, 0, 0])
 
         # Only dealing with depth data for now, TODO: add RGB data
         self.depth_width = self.params["depth"]["width"]
         self.depth_height = self.params["depth"]["height"]
 
         # Some optical sensors only provide diagonal field of view, get horizontal and vertical from diagonal
         try:
-            vfov = self.params["depth"]["vfov"]
+            self.vfov = self.params["depth"]["vfov"]
+            self.hfov = self.params["depth"]["hfov"]
         except KeyError:
-            vfov = ch.get_fov_from_dfov(self.params["depth"]["dfov"], self.depth_width, self.depth_height)[1]
-        near_val = self.params["depth"]["near_plane"]
-        far_val = self.params["depth"]["far_plane"]
+            self.dfov = self.params["depth"]["dfov"]
+            self.hfov, self.vfov = ch.get_fov_from_dfov(self.depth_width, self.depth_height, self.dfov)
+        self.near_val = self.params["depth"]["near_plane"]
+        self.far_val = self.params["depth"]["far_plane"]
 
+        # Pixel coordinates, indexed by depth_width, depth_height, nx1 array COLUMN MAJOR
         self.depth_pixel_coords = np.array(list(np.ndindex((self.depth_width, self.depth_height))), dtype=int)
+        # Film coordinates projected to [-1, 1], nx1 array COLUMN MAJOR
         self.depth_film_coords = (
             2
             * (self.depth_pixel_coords + np.array([0.5, 0.5]) - np.array([self.depth_width / 2, self.depth_height / 2]))
             / np.array([self.depth_width, self.depth_height])
         )
-        self.depth_proj_mat = pbutils.pbclient.computeProjectionMatrixFOV(
-            fov=vfov, aspect=(self.depth_width / self.depth_height), nearVal=near_val, farVal=far_val
+        # Depth projection matrix from camera intrinsics
+        self.depth_proj_mat = pbclient.computeProjectionMatrixFOV(
+            fov=self.vfov, aspect=(self.depth_width / self.depth_height), nearVal=self.near_val, farVal=self.far_val
         )
         return
 
 
 def main():
+    import pprint as pp
+
     pbutils = PyBUtils(renders=False)
     camera = Camera(pbutils, sensor_name="realsense_d435i")
-    print(camera.depth_film_coords)
+    pp.pprint(camera.depth_pixel_coords)
+    pp.pprint(camera.depth_film_coords)
+    pp.pprint(camera.depth_proj_mat)
     return
 
 

pybullet_tree_sim/config/description/robot/cart.yaml

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

pybullet_tree_sim/config/description/robot/fr3.yaml

@@ -0,0 +1,3 @@
+arm_id: fr3
+hand: false
+# TODO: add the rest of the URDF args

pybullet_tree_sim/config/description/robot/linear_slider.yaml

@@ -0,0 +1 @@
+linear_slider_prefix: linear_slider__

pybullet_tree_sim/config/description/robot/mock_pruner.yaml

@@ -0,0 +1,4 @@
+eef_type: mock_pruner
+mock_pruner_prefix: mock_pruner__
+tof0_offset_x: "0.1"
+tof1_offset_x: "-0.1"

pybullet_tree_sim/config/description/robot/robot.yaml

@@ -0,0 +1,14 @@
+robot_stack:
+    # - farm-ng
+    - cart
+    # - linear_slider
+    - ur5e
+    - mock_pruner
+# initial_joint_angles:
+#     - 0.0
+#     - 0.0
+#     - 0.0
+#     - 0.0
+#     - 0.0
+#     - 0.0
+disable_self_collisions: true

pybullet_tree_sim/config/description/robot/ur5e.yaml

@@ -0,0 +1,2 @@
+ur_prefix: ur5e__
+ur_type: ur5e

pybullet_tree_sim/config/tof/vl6180.yaml → pybullet_tree_sim/config/description/tof/vl6180.yaml

@@ -3,6 +3,7 @@ data_type: "d"
 depth:
     dfov: 25.0
     near_plane: 0.002
-    far_plane: 0.150
+    # far_plane: 0.150
+    far_plane: 0.40
     width: 1
     height: 1

pybullet_tree_sim/config/runtime/mock_pruner.yaml

@@ -0,0 +1,22 @@
+sensors:
+    camera0:
+        type: camera
+        name: realsense_d435i
+        # x_offset: "0.02"
+        tf_frame: "camera0"
+        pan: 0.0
+        tilt: 0.0
+    tof0:
+        type: tof
+        name: vl6180
+        # x_offset: "0.02"
+        tf_frame: "tof0"
+        pan: 0.0
+        tilt: 0.0
+    tof1:
+        type: tof
+        name: vl6180
+        # x_offset: "-0.02"
+        tf_frame: "tof1"
+        pan: 0.0
+        tilt: 0.0