Merge pull request #5 from OSUrobotics/refactor/robot

Refactor/robot -- Merging to run BFG without HEAD pointing at protected files.

Author: lukestroh

.github/workflows/black.yaml

@@ -0,0 +1,45 @@
+name: pybullet-tree-sim-workflow
+
+on:
+    pull_request:
+        branches: ['main', 'develop']
+
+jobs:
+    build:
+        strategy:
+            fail-fast: true
+            matrix:
+                os: [ubuntu-latest, macos-latest, windows-latest]
+                python-version: ['3.9', '3.10', '3.11', '3.12']
+
+        runs-on: ${{ matrix.os }}
+        steps:
+            - uses: actions/checkout@v4
+            
+            - name: Set up Python ${{ matrix.python-version }}
+              uses: actions/setup-python@v2
+              with:
+                  python-version: ${{ matrix.python-version }}
+                  
+            - name: Install dependencies
+              run: |
+                python -m pip install --upgrade pip
+                pip install .
+                pip install flake8 black pytest
+                
+            - name: Format with black
+              uses: psf/black@stable
+              with:
+                options: "--check --verbose --diff --line-length=120 ./"
+            
+            - name: Lint with flake8
+              uses: py-actions/flake8@v2
+              # run: |
+              #   # stop the build if there are Python syntax errors or undefined names
+              #   $ flake8 . --count --select=E9,F63,F7,F82 --show-source --statistics
+              with:
+                  args: "--count --select=E9,F63,F7,F82 --show-source --statistics"
+                
+            # - name: Run tests
+            #   run: |
+            #     pytest

.github/workflows/run_tests.yaml

@@ -164,4 +164,10 @@ cython_debug/
 #
 
 # don't upload tree generations
-**/trees/*/generated/*
+**/trees/*/generated/*
+
+# Don't upload pkl files
+*.pkl
+pkl/
+
+meshes/

.gitignore

@@ -1,3 +1,24 @@
+
+## Using this package
+
+### Adding trees
+All trees should be defined by their origin namespace, the tree type, and the tree id. Tree ids should be zero-padded by 5 spaces.
+
+```
+# Pattern:
+{tree_namespace}_{tree_type}_{tree_id}
+
+# Examples:
+LPy_envy_00027
+prosser_ufo_00762
+```
+
+Trees should include a generic mesh and and a labeled mesh. LPy trees can be generated `TODO: TALK TO ABHINAV`
+
+
+## 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 +28,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.

README.md

@@ -10,32 +10,39 @@
 import time
 from zenlog import log
 
+from scipy.spatial.transform import Rotation
+
 
 def main():
     pbutils = PyBUtils(renders=True)
-
-    robot = Robot(pbclient=pbutils.pbclient, position=[0, 1, 0], orientation=[0, 0, 0, 1])
+    robot_start_orientation = Rotation.from_euler('xyz', [0, 0,180], degrees=True).as_quat()
+    robot = Robot(pbclient=pbutils.pbclient, position=[0, 1, 0], orientation=robot_start_orientation)
 
     penv = PruningEnv(
         pbutils=pbutils,
         verbose=True,
     )
 
-    _1_inch = 0.0254
-    penv.activate_shape(shape="cylinder", radius=_1_inch * 2, height=2.85, orientation=[0, np.pi / 2, 0])
+    # _1_inch = 0.0254
+    # penv.activate_shape(
+    #     shape="cylinder",
+    #     radius=_1_inch * 2,
+    #     height=2.85,
+    #     orientation=[0, np.pi / 2, 0],
+    # )
     # penv.activate_shape(shape="cylinder", radius=0.01, height=2.85, orientation=[0, np.pi / 2, 0])
 
-    # penv.load_tree(
-    #     pbutils=pbutils,
-    #     scale=1.0,
-    #     tree_id=1,
-    #     tree_type="envy",
-    #     tree_namespace="LPy_",
-    #     # tree_urdf_path=os.path.join(URDF_PATH, "trees", "envy", "generated", "LPy_envy_tree0.urdf"),
-    #     save_tree_urdf=False,
-    #     # randomize_pose=True
-    # )
-    # penv.activate_tree(tree_id_str="LPy_envy_tree1")
+    tree_name = penv.load_tree(
+        pbutils=pbutils,
+        scale=1.0,
+        tree_id=2,
+        tree_type="envy",
+        tree_namespace="LPy",
+        # tree_urdf_path=os.path.join(URDF_PATH, "trees", "envy", "generated", "LPy_envy_tree0.urdf"),
+        save_tree_urdf=False,
+        # randomize_pose=True
+    )
+    penv.activate_tree(tree_id_str=tree_name)
 
     # # Run the sim a little just to get the environment properly loaded.
     for i in range(100):
@@ -49,11 +56,15 @@ def main():
             # log.debug(f"{robot.sensors['tof0']}")
             tof0_view_matrix = robot.get_view_mat_at_curr_pose(camera=robot.sensors["tof0"])
             tof0_rgbd = robot.get_rgbd_at_cur_pose(
-                camera=robot.sensors["tof0"], type="sensor", view_matrix=tof0_view_matrix
+                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 = robot.get_rgbd_at_cur_pose(
-                camera=robot.sensors["tof1"], type="sensor", view_matrix=tof1_view_matrix
+                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]}")

main.py

@@ -0,0 +1 @@
+amiga_prefix: amiga__

pybullet_tree_sim/config/description/robot/amiga.yaml

@@ -1,4 +1,4 @@
 eef_type: mock_pruner
 mock_pruner_prefix: mock_pruner__
-tof0_offset_x: "0.1"
-tof1_offset_x: "-0.1"
+tof0_offset: "0.10 -0.025 0.2"
+tof1_offset: "-0.10 -0.025 0.2"

pybullet_tree_sim/config/description/robot/mock_pruner.yaml

@@ -1,6 +1,6 @@
 robot_stack:
-    # - farm-ng
-    - cart
+    - amiga
+    # - cart
     # - linear_slider
     - ur5e
     - mock_pruner

pybullet_tree_sim/config/description/robot/robot.yaml

@@ -0,0 +1,8 @@
+name: "VL53L4CD"
+data_type: "d"
+depth:
+    dfov: 18.0
+    near_plane: 0.02
+    far_plane: 1.20
+    width: 1
+    height: 1

pybullet_tree_sim/config/description/tof/vl53l4cd.yaml

@@ -3,7 +3,6 @@ data_type: "d"
 depth:
     dfov: 25.0
     near_plane: 0.002
-    # far_plane: 0.150
-    far_plane: 0.40
+    far_plane: 0.150
     width: 1
     height: 1