22from pybullet_tree_sim import CONFIG_PATH , MESHES_PATH , URDF_PATH , RGB_LABEL , ROBOT_URDF_PATH
33from pybullet_tree_sim .robot import Robot
44from pybullet_tree_sim .tree import Tree , TreeException
5+
56# from pybullet_tree_sim.utils.ur5_utils import UR5
67from pybullet_tree_sim .utils .pyb_utils import PyBUtils
78import pybullet_tree_sim .utils .xacro_utils as xutils
@@ -107,8 +108,6 @@ def __init__(
107108 self .tree_count = tree_count
108109 self .is_goal_state = False
109110
110-
111-
112111 # self.cam_width = cam_width
113112 # self.cam_height = cam_height
114113 # self.cam_pan = 0
@@ -144,15 +143,15 @@ def __init__(
144143 # self.ur5 = self.load_robot(
145144 # type=robot_type, robot_pos=robot_pos, robot_orientation=robot_orientation, randomize_pose=False
146145 # )
147-
146+
148147 # Load all sensor attributes. # TODO: Load only the required sensor attributes
149148 self ._load_sensor_attributes ()
150-
149+
151150 self .sensor_config = sensor_config
152151 self ._assign_tf_frame_to_sensors (self .sensor_config )
153152 # log.warning(self.sensor_attributes)
154153 return
155-
154+
156155 def _load_sensor_attributes (self ):
157156 self .sensor_attributes = {}
158157 camera_configs_path = os .path .join (CONFIG_PATH , "camera" )
@@ -168,13 +167,13 @@ def _load_sensor_attributes(self):
168167 for key , value in yamlcontent .items ():
169168 self .sensor_attributes [key ] = value
170169 return
171-
170+
172171 def _assign_tf_frame_to_sensors (self , sensor_config : dict ):
173172 for sensor_name , conf in sensor_config .items ():
174- sensor = conf [' sensor' ]
175- sensor .tf_frame = conf [' tf_frame' ]
173+ sensor = conf [" sensor" ]
174+ sensor .tf_frame = conf [" tf_frame" ]
176175 log .warn (f"{ sensor .params } " )
177- sensor .tf_frame_index = self .robot .robot_conf [' joint_info' ][ ' mock_pruner__base--camera0' ][ 'id' ]
176+ sensor .tf_frame_index = self .robot .robot_conf [" joint_info" ][ " mock_pruner__base--camera0" ][ "id" ]
178177 return
179178
180179 def load_robot (self , type : str , robot_pos : ArrayLike , robot_orientation : ArrayLike , randomize_pose : bool = False ):
@@ -191,7 +190,7 @@ def load_robot(self, type: str, robot_pos: ArrayLike, robot_orientation: ArrayLi
191190 # verbose=self.verbose,
192191 # )
193192 robot = Robot (pbclient = self .pbutils .pbclient )
194-
193+
195194 else :
196195 raise NotImplementedError (f"Robot type { type } not implemented" )
197196 return robot
@@ -589,7 +588,7 @@ def main():
589588 # data = data.reshape((cam_width * cam_height, 1), order="F")
590589
591590 # log.warning(f"joint angles: {penv.ur5.get_joint_angles()}")
592-
591+
593592 return
594593
595594
0 commit comments