formatting

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

pybullet_tree_sim/tree.py

@@ -447,10 +447,10 @@ def load_tree_urdf(
             urdf_path = os.path.join(Tree._tree_generated_urdf_path, f"{namespace}{tree_type}_tree{tree_id}.urdf")
             if not os.path.exists(urdf_path):
                 log.info(f"Could not find file '{urdf_path}'. Generating URDF from xacro.")
-                
+
                 if not os.path.isdir(Tree._tree_generated_urdf_path):
                     os.mkdir(Tree._tree_generated_urdf_path)
-                
+
                 urdf_mappings = {
                     "namespace": namespace,
                     "tree_id": str(tree_id),

pybullet_tree_sim/urdf/robot/generic/robot.urdf.xacro

@@ -7,29 +7,29 @@
     <xacro:property name="base_attachment_parent" value="world"/>
     <xacro:property name="robot_arm_parent" value="world"/>
     <xacro:property name="end_effector_parent" value="world"/>
-    
+
     <!-- create link fixed to the "world" -->
     <link name="world" />
-    
-    
+
+
     <!-- Base attachment -->
     <xacro:arg name="base_attachment_type" default="" />
 
     <xacro:if value="${ '$(arg base_attachment_type)' == 'linear_slider' }" >
         <!-- linear slider -->
         <xacro:include filename="$(find linear_slider_description)/urdf/macro/linear_slider_base.urdf.xacro" />
-    
+
         <xacro:arg name="linear_slider_parent" default="${base_attachment_parent}"/>
         <xacro:arg name="linear_slider_robot_ip" default="169.254.57.177"/> <!-- TODO: add this into the macro, but instead get from yaml file-->
         <xacro:arg name="base_prefix" default="linear_slider__" />
         <xacro:arg name="use_mock_hardware" default="false" />
         <xacro:arg name="mock_sensor_commands" default="false" />
         <xacro:arg name="sim_gazebo" default="false" />
         <xacro:arg name="sim_gazebo_classic" default="false"/>
-        
+
         <!-- Set slider moving base as arm parent joint -->
         <xacro:property name="robot_arm_parent" value="$(arg base_prefix)tool0" />
-            
+
         <xacro:linear_slider
             parent="$(arg linear_slider_parent)"
             prefix="$(arg base_prefix)"
@@ -52,7 +52,7 @@
         <!-- the default value should raise an error in case this was called without defining the type -->
         <xacro:arg name="ur_type" default="ur5x"/>
         <xacro:arg name="ur_parent" default="${robot_arm_parent}"/>
-        
+
         <!-- parameters -->
         <xacro:arg name="tf_prefix" default="ur5__" />
         <xacro:arg name="joint_limit_params" default="$(find ur_description)/config/$(arg ur_type)/joint_limits.yaml"/>
@@ -84,20 +84,20 @@
         <xacro:arg name="tool_tx_idle_chars" default="3.5" />
         <xacro:arg name="tool_device_name" default="/tmp/ttyUR" />
         <xacro:arg name="tool_tcp_port" default="54321" />
-        
+
             <!-- Simulation parameters -->
         <xacro:arg name="use_fake_hardware" default="false" />
         <xacro:arg name="fake_sensor_commands" default="false" />
         <xacro:arg name="sim_gazebo" default="false" />
         <xacro:arg name="sim_ignition" default="false" />
         <xacro:arg name="simulation_controllers" default="" />
-        
+
         <!-- initial position for simulations (Fake Hardware, Gazebo, Ignition) -->
         <xacro:arg name="initial_positions_file" default="$(find ur_description)/config/initial_positions.yaml"/>
-        
+
         <!-- convert to property to use substitution in function -->
         <xacro:property name="initial_positions_file" default="$(arg initial_positions_file)"/>
-        
+
         <!-- arm -->
         <xacro:ur_robot
             name="$(arg name)"
@@ -138,11 +138,11 @@
             >
             <origin xyz="0 0 0" rpy="0 0 0" />          <!-- position robot in the world -->
         </xacro:ur_robot>
-        
+
         <!-- Update end-effector parent -->
         <xacro:property name="end_effector_parent" value="$(arg tf_prefix)tool0"/>
 
-        
+
     </xacro:if>
     <xacro:if value="${ '$(arg arm_type)' == 'fr3' }">
         <xacro:include filename="$(find franka_description)/robots/common/franka_robot.xacro"/>
@@ -181,32 +181,34 @@
                             fake_sensor_commands="$(arg fake_sensor_commands)"
                             gazebo_effort="$(arg gazebo_effort)">
         </xacro:franka_robot>
-        
+
         <!-- <xacro:property name="end_effector_parent" value="$(arg fr3_prefix)tool0"/> -->
-        
+
     </xacro:if>
-    
+
     <!-- Robot arm base/eef attachments -->
     <xacro:arg name="end_effector_type" default=""/> -->
     <xacro:if value="${ '$(arg end_effector_type)' == 'apple_picker' }">
-        
+
     </xacro:if>
     <xacro:if value="${ '$(arg end_effector_type)' == 'mock_pruner' }" >
         <xacro:include filename="../../end_effectors/mock_pruner/macro/mock_pruner_macro.urdf.xacro"/>
-        
+
         <!-- Args -->
         <xacro:arg name="eef_parent" default="${end_effector_parent}" />
         <xacro:arg name="eef_prefix" default="mock_pruner__" />
-
+        <xacro:arg name="tof0_offset" default='0.02'/>
+        <xacro:arg name="tof1_offset" default='-0.02'/>
+
         <xacro:mock_pruner_macro
             eef_prefix="$(arg eef_prefix)"
             eef_parent="$(arg eef_parent)"
-            tof0_offset='0.02'
-            tof1_offset='-0.02'
-            camera_offset='' 
+            tof0_offset='$(arg tof0_offset)'
+            tof1_offset='$(arg tof1_offset)'
+            camera_offset=''
         />
-        
-        
+
+
     </xacro:if>
-    
-</robot>
+
+</robot>

pybullet_tree_sim/utils/camera_helpers.py

@@ -39,6 +39,7 @@ def get_fov_from_dfov(camera_width: int, camera_height: int, dFoV: Union[int, fl
     fov_w = 2 * np.arctan(np.tan(_dfov / 2) * camera_width / camera_diag)
     return (np.rad2deg(fov_w), np.rad2deg(fov_h))
 
+
 # def get_pyb_proj_mat(vfov: float, aspect: float, nearVal: float, farVal: float):
 #     return pbutils.pbclient.computeProjectionMatrixFOV(
 #         fov=vfov, aspect=(self.depth_width / self.depth_height), nearVal=near_val, farVal=far_val

pybullet_tree_sim/utils/pyb_utils.py

@@ -1,5 +1,6 @@
 #!/usr/bin/env python3
 from __future__ import annotations
+
 """
 Author (s): Abhinav Jain, Luke Strohbehn
 """
@@ -13,6 +14,7 @@
 from scipy.constants import g as grav
 
 from pybullet_tree_sim import MESHES_PATH, URDF_PATH, TEXTURES_PATH
+
 # from pybullet_tree_sim.camera import Camera
 # from pybullet_tree_sim.utils.camera_helpers import get_fov_from_dfov
 
@@ -144,7 +146,7 @@ def get_image_at_curr_pose(self, camera, type, view_matrix=None) -> List:
             if view_matrix is None:
                 raise ValueError("view_matrix cannot be None for robot view")
             return self.pbclient.getCameraImage(
-                width=camera.depth_width, # TODO: make separate function for rgb?
+                width=camera.depth_width,  # TODO: make separate function for rgb?
                 height=camera.depth_height,
                 viewMatrix=view_matrix,
                 projectionMatrix=camera.depth_proj_mat,

test/test_camera.py

@@ -3,8 +3,8 @@
 
 import unittest
 
+
 class TestCamera(unittest.TestCase):
     def test_deproject_transform(self):
-        
+
         return
-

test/test_helpers.py

@@ -6,7 +6,6 @@
 
 
 class TestHelpers(unittest.TestCase):
-
     def test_negative_dfov(self):
         dfov = -65
         cam_w = 100
@@ -15,7 +14,7 @@ def test_negative_dfov(self):
         with self.assertRaises(ValueError) as cm:
             helpers.get_fov_from_dfov(cam_w, cam_h, dfov)
         return
-        
+
     def test_negative_cam_width(self):
         dfov = 65
         cam_w = -100
@@ -24,7 +23,7 @@ def test_negative_cam_width(self):
         with self.assertRaises(ValueError) as cm:
             helpers.get_fov_from_dfov(cam_w, cam_h, dfov)
         return
-        
+
     def test_negative_cam_height(self):
         dfov = 65
         cam_w = 100
@@ -33,10 +32,6 @@ def test_negative_cam_height(self):
         with self.assertRaises(ValueError) as cm:
             helpers.get_fov_from_dfov(cam_w, cam_h, dfov)
         return
-
-
-
-
 
 
 # class TestStringMethods(unittest.TestCase):
@@ -55,5 +50,5 @@ def test_negative_cam_height(self):
 #         with self.assertRaises(TypeError):
 #             s.split(2)
 
-if __name__ == '__main__':
+if __name__ == "__main__":
     unittest.main()