Merge pull request #4 from OSUrobotics/refactor/robot

Refactor/robot

Author: lukestroh

README.md

@@ -2,14 +2,15 @@ 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. Figure out better sensor -> camera -> ToF inheritance (ask Cindy)
+1. Tree/robot/env interaction
 1. For Claire: 
-    1. Generic URDF to Generic Robot class
+    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. 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.
-1. Separate camera class
-    1. ToF class inherits camera classs
 1. Dynamically populate UR URDF. Allow for various end-effectors and robot configurations.
     1. Make sure to include camera and other sensors. (Source manifold mesh -- utils -> camera class (C++))
     1. Dynamic parent joint for Panda to slider/farm-ng (like UR5)
@@ -19,7 +20,7 @@ urdf generic launch CLI test:
 1. Make mini orchard from a set of URDFs
     1. Space out like normal orchard
     1. Be aware of memory issues in PyB
-1. Change pkl files to hdf5. (lower priority)
+1. Change pkl files to hdf5. (lower priority) -- Ask Abhinav for code
 1. Test
     1. Various tof configurations
 

main.py

@@ -1,6 +1,4 @@
 #!/usr/bin/env python3
-from pybullet_tree_sim.camera import Camera
-from pybullet_tree_sim.time_of_flight import TimeOfFlight
 from pybullet_tree_sim.pruning_environment import PruningEnv
 from pybullet_tree_sim.robot import Robot
 from pybullet_tree_sim.tree import Tree
@@ -50,24 +48,29 @@ def main():
         try:
             # log.debug(f"{robot.sensors['tof0']}")
             tof0_view_matrix = robot.get_view_mat_at_curr_pose(camera=robot.sensors["tof0"])
-            tof0_rgbd = penv.pbutils.get_rgbd_at_cur_pose(
-                camera=robot.sensors["tof0"], type="robot", view_matrix=tof0_view_matrix
+            tof0_rgbd = robot.get_rgbd_at_cur_pose(
+                camera=robot.sensors["tof0"], type="sensor", view_matrix=tof0_view_matrix
             )
             tof1_view_matrix = robot.get_view_mat_at_curr_pose(camera=robot.sensors["tof1"])
-            tof1_rgbd = penv.pbutils.get_rgbd_at_cur_pose(
-                camera=robot.sensors["tof1"], type="robot", view_matrix=tof1_view_matrix
+            tof1_rgbd = robot.get_rgbd_at_cur_pose(
+                camera=robot.sensors["tof1"], type="sensor", view_matrix=tof1_view_matrix
             )
             # tof0_view_matrix = np.asarray(tof0_view_matrix).reshape((4, 4), order="F")
             # log.debug(f"{tof0_view_matrix[:3, 3]}")
 
+            # Get user keyboard input, map to robot movement, camera capture, controller action
             keys_pressed = penv.get_key_pressed()
-            action = penv.get_key_action(robot=robot, keys_pressed=keys_pressed)
-            action = action.reshape((6, 1))
-            joint_vel, jacobian = robot.calculate_joint_velocities_from_ee_velocity_dls(end_effector_velocity=action)
-            robot.action = joint_vel
-            singularity = robot.set_joint_velocities(robot.action)
-            penv.pbutils.pbclient.stepSimulation()
-            time.sleep(0.01)
+            move_action = robot.get_key_move_action(keys_pressed=keys_pressed)
+            sensor_data = robot.get_key_sensor_action(keys_pressed=keys_pressed)
+
+            joint_vels, jacobian = robot.calculate_joint_velocities_from_ee_velocity_dls(
+                end_effector_velocity=move_action
+            )
+            singularity = robot.set_joint_velocities(joint_velocities=joint_vels)
+
+            # Step simulation
+            pbutils.pbclient.stepSimulation()
+            time.sleep(0.001)
         except KeyboardInterrupt:
             break