updates to robot.py for generic robot construction

Author: lukestroh

README.md

@@ -1,3 +1,7 @@
+
+## TODO:
+Look up Cantera
+
 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`
 
@@ -7,7 +11,7 @@ urdf generic launch CLI test:
 1. For Claire: 
     1. Figure out best way to manage tree/robot/environment interaction. I removed robot from penv, but self.trees still exists. 
     1. Fill out the `object_loader.py` class. Activate/deactivate trees, supports, robots.
-    1. Find the `TODO`s in all the code. Ask Luke what they mean and discuss solutions.
+    1. Find the `TODO`s in all the code. Ask Luke what they meana and discuss solutions.
 1. Format the final approach controller as a python subpackage?
     1. https://packaging.python.org/en/latest/guides/packaging-namespace-packages/#packaging-namespace-packages
 1. Add basic cylinder to world. Dynamically create URDF.

pybullet_tree_sim/robot.py

@@ -46,12 +46,12 @@ def __init__(
         self.randomize_pose = randomize_pose  # TODO: This isn't set up anymore... fix
         self.init_joint_angles = (
             (
-                -np.pi / 2,
+                -np.pi / 2 + np.pi / 4,
                 -np.pi * 2 / 3,
                 np.pi * 2 / 3,
                 -np.pi,
                 -np.pi / 2,
-                np.pi,
+                0,
             )
             if init_joint_angles is None
             else init_joint_angles
@@ -112,7 +112,8 @@ def _generate_robot_urdf(self) -> None:
                 self.robot_conf.update(part_conf)
             else:
                 raise ValueError(f"Robot part {robot_part} not found in {self._robot_configs_path}")
-
+        
+        log.warn(self.robot_conf)
         # Generate URDF from mappings
         robot_urdf = xutils.load_urdf_from_xacro(
             xacro_path=self._robot_xacro_path,
@@ -183,7 +184,7 @@ def _get_joints(self) -> dict:
         return joints
 
     def _assign_control_joints(self, joints: dict) -> list:
-        """Get list of controllabe joints from the joint dict by joint type"""
+        """Get list of controllable joints from the joint dict by joint type"""
         control_joints = []
         control_joint_idxs = []
         for joint, joint_info in joints.items():
@@ -712,6 +713,7 @@ def deproject_pixels_to_points(
             return sensor_coords
         elif return_frame == "world":
             world_coords = (mr.TransInv(view_matrix) @ sensor_coords.T).T
+            log.err("HELLO WORLD")
             if debug:
                 plot.debug_deproject_pixels_to_points(
                     sensor=sensor,
@@ -831,7 +833,7 @@ def get_key_sensor_action(self, keys_pressed: list) -> dict | None:
                             depth = depth.reshape((sensor.depth_width * sensor.depth_height, 1), order="F")
 
                             camera_points = self.deproject_pixels_to_points(
-                                sensor=sensor, data=depth, view_matrix=view_matrix, return_frame="sensor"
+                                sensor=sensor, data=depth, view_matrix=view_matrix, return_frame="world", debug=False,
                             )
 
                             sensor_data.update(