working chain from linear_slider -> ur5 -> mock_pruner. Fragile.

README.md

@@ -1,3 +1,6 @@
+urdf generic launch CLI test:
+`xacro robot.urdf.xacro > test.urdf end_effector_type:=mock_pruner eef_parent:=ur5e__tool0 arm_type:=ur5 ur_type:=ur5e tf_prefix:=ur5e__ base_attachment_type:=linear_slider`
+
 ## TODO
 1. For Claire: 
     1. Generic URDF to Generic Robot class

main.py

@@ -21,8 +21,8 @@ def main():
     penv = PruningEnv(
         pbutils=pbutils, load_robot=True, robot_pos=[0, 1, 0], verbose=True, cam_width=cam_width, cam_height=cam_height
     )
-    
-    penv.activate_shape(shape="cylinder", radius=2*0.0254, height=2.0, orientation=[0,np.pi/2,0])
+
+    penv.activate_shape(shape="cylinder", radius=2 * 0.0254, height=2.0, orientation=[0, np.pi / 2, 0])
     # penv.load_tree(
     #     pbutils=pbutils,
     #     scale=1.0,
@@ -39,15 +39,15 @@ def main():
     for i in range(100):
         pbutils.pbclient.stepSimulation()
         time.sleep(0.1)
-        
+
     # Simulation loop
     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)
+            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)
             keys_pressed = penv.get_key_pressed()
             action = penv.get_key_action(keys_pressed=keys_pressed)
-            action = action.reshape((6,1))
+            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)
@@ -63,4 +63,3 @@ def main():
 
 if __name__ == "__main__":
     main()
-

pybullet_tree_sim/camera.py

@@ -22,12 +22,12 @@ def __init__(self, pbutils, sensor_name: str, sensor_type: str = "camera") -> No
         super().__init__(sensor_name, sensor_type)
         self.pan = 0
         self.tilt = 0
-        self.tf_frame: str = ''
+        self.tf_frame: str = ""
 
         # 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"]
@@ -37,9 +37,11 @@ def __init__(self, pbutils, sensor_name: str, sensor_type: str = "camera") -> No
         far_val = self.params["depth"]["far_plane"]
 
         self.depth_pixel_coords = np.array(list(np.ndindex((self.depth_width, self.depth_height))), dtype=int)
-        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_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
         )

pybullet_tree_sim/pruning_environment.py

@@ -398,7 +398,9 @@ def deproject_pixels_to_points(
 
         plot = False
         if plot:
-            self.debug_plots(camera=camera, data=data, cam_coords=cam_coords, world_coords=world_coords, view_matrix=view_matrix)
+            self.debug_plots(
+                camera=camera, data=data, cam_coords=cam_coords, world_coords=world_coords, view_matrix=view_matrix
+            )
 
         return world_coords
 
@@ -562,7 +564,6 @@ def get_key_action(self, keys_pressed: list) -> np.ndarray:
             keys_pressed = []
         return action
 
-
     def run_sim(self) -> int:
 
         return 0