Skip to content

Commit

Permalink
testing the lint checker
Browse files Browse the repository at this point in the history
  • Loading branch information
Clairefollett committed Nov 19, 2024
1 parent cf9d55c commit dbe777d
Show file tree
Hide file tree
Showing 6 changed files with 120 additions and 100 deletions.
17 changes: 9 additions & 8 deletions main.py
Original file line number Diff line number Diff line change
Expand Up @@ -12,23 +12,24 @@
from zenlog import log


def main():
def main():
pbutils = PyBUtils(renders=False)

# Init sensors
sensor_config = {
"camera0": {
'sensor': Camera(pbutils, sensor_name="realsense_d435i"),
'tf_frame': "mock_pruner__camera0",
"sensor": Camera(pbutils, sensor_name="realsense_d435i"),
"tf_frame": "mock_pruner__camera0",
}
}



penv = PruningEnv(
pbutils=pbutils, load_robot=True, sensor_config=sensor_config, robot_pos=[0, 2, 0], verbose=True,
pbutils=pbutils,
load_robot=True,
sensor_config=sensor_config,
robot_pos=[0, 2, 0],
verbose=True,
)


penv.activate_shape(shape="cylinder", radius=2 * 0.0254, height=2.0, orientation=[0, np.pi / 2, 0])
# penv.load_tree(
Expand All @@ -51,7 +52,7 @@ def main():
# Simulation loop
while True:
try:
tof0_view_matrix = penv.robot.get_view_mat_at_curr_pose(camera=penv.robot.sensors['mock_pruner__camera0'])
tof0_view_matrix = penv.robot.get_view_mat_at_curr_pose(camera=penv.robot.sensors["mock_pruner__camera0"])
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)
Expand Down
21 changes: 10 additions & 11 deletions pybullet_tree_sim/pruning_environment.py
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,7 @@
from pybullet_tree_sim import CONFIG_PATH, MESHES_PATH, URDF_PATH, RGB_LABEL, ROBOT_URDF_PATH
from pybullet_tree_sim.robot import Robot
from pybullet_tree_sim.tree import Tree, TreeException

# from pybullet_tree_sim.utils.ur5_utils import UR5
from pybullet_tree_sim.utils.pyb_utils import PyBUtils
import pybullet_tree_sim.utils.xacro_utils as xutils
Expand Down Expand Up @@ -107,8 +108,6 @@ def __init__(
self.tree_count = tree_count
self.is_goal_state = False



# self.cam_width = cam_width
# self.cam_height = cam_height
# self.cam_pan = 0
Expand Down Expand Up @@ -144,15 +143,15 @@ def __init__(
# self.ur5 = self.load_robot(
# type=robot_type, robot_pos=robot_pos, robot_orientation=robot_orientation, randomize_pose=False
# )

# Load all sensor attributes. # TODO: Load only the required sensor attributes
self._load_sensor_attributes()

self.sensor_config = sensor_config
self._assign_tf_frame_to_sensors(self.sensor_config)
# log.warning(self.sensor_attributes)
return

def _load_sensor_attributes(self):
self.sensor_attributes = {}
camera_configs_path = os.path.join(CONFIG_PATH, "camera")
Expand All @@ -168,13 +167,13 @@ def _load_sensor_attributes(self):
for key, value in yamlcontent.items():
self.sensor_attributes[key] = value
return

def _assign_tf_frame_to_sensors(self, sensor_config: dict):
for sensor_name, conf in sensor_config.items():
sensor = conf['sensor']
sensor.tf_frame = conf['tf_frame']
sensor = conf["sensor"]
sensor.tf_frame = conf["tf_frame"]
log.warn(f"{sensor.params}")
sensor.tf_frame_index = self.robot.robot_conf['joint_info']['mock_pruner__base--camera0']['id']
sensor.tf_frame_index = self.robot.robot_conf["joint_info"]["mock_pruner__base--camera0"]["id"]
return

def load_robot(self, type: str, robot_pos: ArrayLike, robot_orientation: ArrayLike, randomize_pose: bool = False):
Expand All @@ -191,7 +190,7 @@ def load_robot(self, type: str, robot_pos: ArrayLike, robot_orientation: ArrayLi
# verbose=self.verbose,
# )
robot = Robot(pbclient=self.pbutils.pbclient)

else:
raise NotImplementedError(f"Robot type {type} not implemented")
return robot
Expand Down Expand Up @@ -589,7 +588,7 @@ def main():
# data = data.reshape((cam_width * cam_height, 1), order="F")

# log.warning(f"joint angles: {penv.ur5.get_joint_angles()}")

return


Expand Down
Loading

0 comments on commit dbe777d

Please sign in to comment.