2
2
from pybullet_tree_sim import CONFIG_PATH , MESHES_PATH , URDF_PATH , RGB_LABEL , ROBOT_URDF_PATH
3
3
from pybullet_tree_sim .robot import Robot
4
4
from pybullet_tree_sim .tree import Tree , TreeException
5
+
5
6
# from pybullet_tree_sim.utils.ur5_utils import UR5
6
7
from pybullet_tree_sim .utils .pyb_utils import PyBUtils
7
8
import pybullet_tree_sim .utils .xacro_utils as xutils
@@ -107,8 +108,6 @@ def __init__(
107
108
self .tree_count = tree_count
108
109
self .is_goal_state = False
109
110
110
-
111
-
112
111
# self.cam_width = cam_width
113
112
# self.cam_height = cam_height
114
113
# self.cam_pan = 0
@@ -144,15 +143,15 @@ def __init__(
144
143
# self.ur5 = self.load_robot(
145
144
# type=robot_type, robot_pos=robot_pos, robot_orientation=robot_orientation, randomize_pose=False
146
145
# )
147
-
146
+
148
147
# Load all sensor attributes. # TODO: Load only the required sensor attributes
149
148
self ._load_sensor_attributes ()
150
-
149
+
151
150
self .sensor_config = sensor_config
152
151
self ._assign_tf_frame_to_sensors (self .sensor_config )
153
152
# log.warning(self.sensor_attributes)
154
153
return
155
-
154
+
156
155
def _load_sensor_attributes (self ):
157
156
self .sensor_attributes = {}
158
157
camera_configs_path = os .path .join (CONFIG_PATH , "camera" )
@@ -168,13 +167,13 @@ def _load_sensor_attributes(self):
168
167
for key , value in yamlcontent .items ():
169
168
self .sensor_attributes [key ] = value
170
169
return
171
-
170
+
172
171
def _assign_tf_frame_to_sensors (self , sensor_config : dict ):
173
172
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" ]
176
175
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" ]
178
177
return
179
178
180
179
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
191
190
# verbose=self.verbose,
192
191
# )
193
192
robot = Robot (pbclient = self .pbutils .pbclient )
194
-
193
+
195
194
else :
196
195
raise NotImplementedError (f"Robot type { type } not implemented" )
197
196
return robot
@@ -589,7 +588,7 @@ def main():
589
588
# data = data.reshape((cam_width * cam_height, 1), order="F")
590
589
591
590
# log.warning(f"joint angles: {penv.ur5.get_joint_angles()}")
592
-
591
+
593
592
return
594
593
595
594
0 commit comments