diff --git a/.gitignore b/.gitignore index c4b0ad0..6678ea7 100644 --- a/.gitignore +++ b/.gitignore @@ -170,4 +170,4 @@ cython_debug/ *.pkl pkl/ -meshes/ \ No newline at end of file +meshes*/ \ No newline at end of file diff --git a/README.md b/README.md index bf665aa..35d1e35 100644 --- a/README.md +++ b/README.md @@ -1,70 +1,61 @@ ## 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. +### Installation -``` -# Pattern: -{tree_namespace}_{tree_type}_{tree_id} +#### Docker +If using Ubuntu, you may skip this step. All other distributions must create a Docker environment. This container includes ROS2 Humble. -# Examples: -LPy_envy_00027 -prosser_ufo_00762 +``` +docker run -it --net=host --device /dev/dri/ -e DISPLAY=$DISPLAY -v $HOME/.Xauthority:/root/.Xauthority:ro osrf/ros:humble-desktop ``` -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` - -## TODO -1. Figure out better sensor -> camera -> ToF inheritance (ask Cindy) -1. Tree/robot/env interaction -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 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. -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) -1. Add generic robot class (from Abhinav's code) - 1. Panda/UR5 - 1. End-effector -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) -- Ask Abhinav for code -1. Test - 1. Various tof configurations +After the docker environment has downloaded, update the system and download the UR robot drivers: +``` +apt update -y && apt upgrade -y +apt install python3-venv -y +apt install ros-$ROS_DISTRO-ur -y +cd ~ +``` -## Installation -#### Requirements -ur, linear_slider, franka-emika +#### Installing this package +1. Clone this repository into your local directory: +``` +git clone https://github.com/OSUrobotics/pybullet-tree-sim.git +``` +2. Create a virtual environment. Python's `venv` is encouraged: +``` +cd pybullet-tree-sim +python3 -m venv venv +source venv/bin/activate +``` -#### General use +3. Install using pip: ``` -python -m pip install . +python3 -m pip install --upgrade pip +pip install . ``` -#### Development +4. Download the required mesh files from Zenodo +If successfully installed, a CLI arg `mesh_downloader` should now be active in your environment. ``` -python -m pip install -e. +mesh_downloader ``` +This command will extract the zip file for you. It is a large file and will take several minutes. -### Useful RegEx commands... -For replacing .mtl file paths +### 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. + ``` -From: ([a-zA-Z0-9_]*).mtl\n -To: ../mtl/$1.mtl\n +# 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 by `TODO: TALK TO ABHINAV` and added to the Zenodo storage. + diff --git a/TODO.md b/TODO.md new file mode 100644 index 0000000..6f92ea0 --- /dev/null +++ b/TODO.md @@ -0,0 +1,37 @@ +# 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` + +## TODO +1. Figure out better sensor -> camera -> ToF inheritance (ask Cindy) +1. Tree/robot/env interaction +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 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. +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) +1. Add generic robot class (from Abhinav's code) + 1. Panda/UR5 + 1. End-effector +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) -- Ask Abhinav for code +1. Test + 1. Various tof configurations + + +### Useful RegEx commands... +For replacing .mtl file paths +``` +From: ([a-zA-Z0-9_]*).mtl\n +To: ../mtl/$1.mtl\n + +``` diff --git a/main.py b/main.py index 6e07c7c..310d3be 100644 --- a/main.py +++ b/main.py @@ -15,7 +15,7 @@ def main(): pbutils = PyBUtils(renders=True) - robot_start_orientation = Rotation.from_euler('xyz', [0, 0,180], degrees=True).as_quat() + 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( @@ -42,7 +42,7 @@ def main(): save_tree_urdf=False, # randomize_pose=True ) - penv.activate_tree(tree_id_str=tree_name) + penv.activate_tree_by_id_str(tree_id_str=tree_name) # # Run the sim a little just to get the environment properly loaded. for i in range(100): diff --git a/pybullet_tree_sim/pruning_environment.py b/pybullet_tree_sim/pruning_environment.py index 85a949b..2502ca9 100644 --- a/pybullet_tree_sim/pruning_environment.py +++ b/pybullet_tree_sim/pruning_environment.py @@ -69,7 +69,7 @@ def __init__( # make_trees: bool = False, name: str = "PruningEnv", # num_trees: int | None = None, - renders: bool = False, + # renders: bool = False, verbose: bool = True, ) -> None: """Initialize the Pruning Environment @@ -83,7 +83,7 @@ def __init__( # Pybullet GUI variables self.render_mode = "rgb_array" - self.renders = renders + # self.renders = renders # self.eval = evaluate # Gym variables @@ -143,17 +143,20 @@ def load_tree( # TODO: Clean up Tree init vs create_tree, probably not needed. randomize_pose=randomize_pose, ) - tree_id_str = f"{tree_namespace}{tree_type}_tree{tree_id}" - # urdf_path = os.path.join(URDF_PATH, "trees", tree_type, "generated", f"{tree.id_str}.urdf") - # Add tree to dict of trees self.trees[tree.id_str] = tree - return tree_id_str + return tree.id_str + + def get_tree_from_id_str(self, tree_id_str: str) -> Tree: + try: + tree = self.trees[tree_id_str] + except KeyError as e: + raise TreeException(f"{e}: Tree with ID {tree_id_str} not found") + return tree def activate_tree( self, - tree: Tree | None = None, - tree_id_str: str | None = None, + tree: Tree, include_support_posts: bool = True, ) -> None: """Activate a tree by object or by tree_id_str. Can include support posts. Must provide either a Tree or tree_id_str. @@ -161,43 +164,33 @@ def activate_tree( @param tree_id_str (str/None): String including the identification characteristics of the tree. @return None """ - - if tree is None and tree_id_str is None: - raise TreeException("Parameters 'tree' and 'tree_id_str' cannot both be None") - - if tree is None and tree_id_str is not None: - try: - tree = self.trees[tree_id_str] - except KeyError as e: - raise TreeException(f"{e}: Tree with ID {tree_id_str} not found") - - if tree is not None: + if tree: if self.verbose: log.info("Activating tree") - tree.pyb_tree_id = self.pbutils.pbclient.loadURDF(tree.urdf_path, useFixedBase=True) - log.info(f"Tree {tree.id_str} activated with PyBID {tree.pyb_tree_id}") + tree.pyb_id = self.pbutils.pbclient.loadURDF(tree.urdf_path, useFixedBase=True) + log.info(f"Tree {tree.id_str} activated with PyBID {tree.pyb_id}") if include_support_posts: self.activate_support_posts(associated_tree=tree) return - def deactivate_tree(self, tree: Tree | None = None, tree_id_str: str | None = None) -> None: - """Deactivate a tree by object or by tree_id_str""" - if tree is None and tree_id_str is None: - raise TreeException("Parameters 'tree' and 'tree_id_str' cannot both be None") - - if tree is None and tree_id_str is not None: - try: - tree = self.trees[tree_id_str] - except KeyError as e: - raise TreeException(f"{e}: Tree with ID {tree_id_str} not found") - - if tree is not None: - try: - self.pbutils.pbclient.removeBody(tree.pyb_tree_id) - log.info(f"Tree {tree.id_str} with PyBID {tree.pyb_tree_id} deactivated") - except Exception as e: - log.error(f"Error deactivating tree: {e}") + def activate_tree_by_id_str(self, tree_id_str: str, include_support_posts: bool = True) -> None: + tree = self.get_tree_from_id_str(tree_id_str=tree_id_str) + self.activate_tree(tree=tree, include_support_posts=include_support_posts) + return + + def deactivate_tree(self, tree: Tree) -> None: + """Deactivate a tree object""" + try: + self.pbutils.pbclient.removeBody(tree.pyb_id) + log.info(f"Tree {tree.id_str} with PyBID {tree.pyb_id} deactivated") + except Exception as e: + log.error(f"Error deactivating tree: {e}") + return + + def deactivate_tree_by_id_str(self, tree_id_str: str) -> None: + tree = self.get_tree_from_id_str(tree_id_str=tree_id_str) + self.deactivate_tree(tree=tree) return def activate_support_posts( diff --git a/pybullet_tree_sim/robot.py b/pybullet_tree_sim/robot.py index 311d4ab..b540eef 100644 --- a/pybullet_tree_sim/robot.py +++ b/pybullet_tree_sim/robot.py @@ -44,7 +44,7 @@ def __init__( self.position = position self.orientation = orientation self.randomize_pose = randomize_pose # TODO: This isn't set up anymore... fix - self.init_joint_angles = ( + self.init_joint_angles = ( # TODO: dynamically assign from defaults (i.e. if linear-slider is loaded in) ( -np.pi / 2 + np.pi / 4, -np.pi * 2 / 3, @@ -172,6 +172,7 @@ def _setup_robot(self): def _get_joints(self) -> dict: """Return a dict of joint information for the robot""" joints = {} + for i in range(self.num_joints): info = self.pbclient.getJointInfo(self.robot, i) joint_name = info[1].decode("utf-8") @@ -187,16 +188,25 @@ def _get_joints(self) -> dict: } } ) + log.warn(joints) return joints - def _assign_control_joints(self, joints: dict) -> list: + def _assign_control_joints(self, joints: dict) -> tuple[list]: """Get list of controllable joints from the joint dict by joint type""" control_joints = [] control_joint_idxs = [] + self.control_joint_lower_limits = [] + self.control_joint_upper_limits = [] + self.control_joint_ranges = [] for joint, joint_info in joints.items(): if joint_info["type"] == 0: # TODO: Check if this works for prismatic joints or just revolute control_joints.append(joint) control_joint_idxs.append(joint_info["id"]) + + # self.joint_upper_limits, + # self.joint_lower_limits, + # self.joint_ranges, # , + return control_joints, control_joint_idxs def _get_links(self) -> dict: @@ -323,6 +333,7 @@ def reset_robot(self, joint_angles: tuple = None) -> None: else joint_angles ) self.set_joint_angles_no_collision(self.init_joint_angles) + # self.pbclient.resetJointState() # TODO Fill out return def remove_robot(self): @@ -565,7 +576,7 @@ def check_collisions(self, collision_objects) -> Tuple[bool, dict]: collisons_self = self.pbclient.getContactPoints(bodyA=self.robot, bodyB=self.robot) collisions_unacceptable = collisons_self for i in range(len(collisions_unacceptable)): - if collisions_unacceptable[i][-6] < -0.001: + if collisions_unacceptable[i][-6] < -0.00: collision_info["collisions_unacceptable"] = True break if self.verbose > 1: @@ -578,7 +589,7 @@ def check_collisions(self, collision_objects) -> Tuple[bool, dict]: def check_success_collision(self, body_b) -> bool: """Check if there are any collisions between the robot and the environment - Returns: Boolw + Returns: Bool """ collisions_success = self.pbclient.getContactPoints( bodyA=self.robot, bodyB=body_b, linkIndexA=self.success_link_index diff --git a/pybullet_tree_sim/tree.py b/pybullet_tree_sim/tree.py index 8c4edc2..7a72680 100644 --- a/pybullet_tree_sim/tree.py +++ b/pybullet_tree_sim/tree.py @@ -18,7 +18,8 @@ import numpy as np import pybullet import pywavefront -from nptyping import NDArray, Shape, Float + +# from nptyping import NDArray, Shape, Float from numpy.typing import ArrayLike from pybullet_tree_sim import RGB_LABEL, URDF_PATH, MESHES_PATH, PKL_PATH from pybullet_tree_sim.utils.pyb_utils import PyBUtils @@ -94,30 +95,26 @@ def __init__( self.tree_namespace = namespace self.tree_id = tree_id self.tree_type = tree_type - self.id_str = self.create_id_string(tree_id=tree_id, tree_type=tree_type, namespace=namespace, urdf_path=urdf_path) - self.urdf_path = os.path.join(self._tree_generated_urdf_path, self.id_str+'.urdf') - self.mesh_path = os.path.join(self._tree_meshes_unlabeled_path, self.id_str+".obj") - self.labeled_mesh_path = os.path.join(self._tree_meshes_labeled_path, self.id_str+"_labeled.obj") + self.id_str = self.create_id_string( + tree_id=tree_id, tree_type=tree_type, namespace=namespace, urdf_path=urdf_path + ) + self.urdf_path = os.path.join(self._tree_generated_urdf_path, self.id_str + ".urdf") + self.mesh_path = os.path.join(self._tree_meshes_unlabeled_path, self.id_str + ".obj") + self.labeled_mesh_path = os.path.join(self._tree_meshes_labeled_path, self.id_str + "_labeled.obj") self.init_pos = position self.init_orientation = orientation - - log.info(f"__init__ {self.id_str}") - - # URDF self.load_tree_urdf(scale=scale, parent=parent) # OBJ tree_obj = self.load_tree_obj() - log.info(f"Tree mesh loaded: {self.mesh_path}") - # Labelled OBJ + # Labeled OBJ labeled_tree_obj = self.load_labeled_tree_obj() # Tree specific parameters self.rgb_label = RGB_LABEL - self.pyb_tree_id = None - - + # PyBullet parameters + self.pyb_id: int = None # Set tree pose if randomize_pose: @@ -185,24 +182,28 @@ def __init__( return - def create_id_string(self, + def create_id_string( + self, tree_id: int | None = None, tree_type: str | None = None, namespace: str | None = None, - urdf_path: str | None = None + urdf_path: str | None = None, ) -> str: if tree_id is None and urdf_path is None: # log.error("Both urdf_path and tree parameters cannot be None.") raise TreeException("Both urdf_path and tree parameters cannot be None.") + if tree_id is not None: + tree_id = str(tree_id).zfill(5) + if urdf_path is None: - id_str = f"{namespace}_{tree_type}_tree{tree_id}" + id_str = f"{namespace}_{tree_type}_{tree_id}" else: id_str = Path(urdf_path).stem - id_str_components = id_str.split('_') + id_str_components = id_str.split("_") self.tree_namespace = id_str_components[0] self.tree_type = id_str_components[1] - self.tree_id = id_str_components[2] + self.tree_id = str(id_str_components[2]).zfill(5) return id_str def _load_points_from_pickle(self, pkl_path): @@ -448,10 +449,8 @@ def load_tree_urdf( orientation: str = "0.0 0.0 0.0", save_urdf: bool = True, regenerate_urdf: bool = False, # TODO: make save/regenerate work well together. Will need to add delete URDF function - ) -> None: - """Load a tree URDF from a given path or generate a tree URDF from a xacro file. Returns the URDF content. - If `tree_urdf_path` is not None, then load that URDF. - Otherwise, process an xacro file with given input parameters. + ) -> str: + """Load a tree URDF from a given path or generate a tree URDF from a xacro file. If content is generated, by default saves the content to /urdf/trees//generated Returns the URDF content. Returns ------- @@ -463,34 +462,37 @@ def load_tree_urdf( if not os.path.isdir(Tree._tree_generated_urdf_path): os.mkdir(Tree._tree_generated_urdf_path) - urdf_mappings = { - "namespace": self.tree_namespace, - "tree_id": str(self.tree_id), - "tree_type": self.tree_type, - "parent": parent, - "xyz": position, - "rpy": orientation, - } - # If the tree macro information doesn't describe a generated file, generate it using the generic tree xacro. - urdf_content = xutils.load_urdf_from_xacro( - xacro_path=Tree._tree_xacro_path, mappings=urdf_mappings - ).toprettyxml() - if save_urdf: - xutils.save_urdf(urdf_content=urdf_content, urdf_path=self.urdf_path) - log.info(f"Saved URDF to file '{self.urdf_path}'.") + _tree_id = str(self.tree_id).zfill(5) + urdf_mappings = { + "namespace": self.tree_namespace, + "tree_id": _tree_id, + "tree_type": self.tree_type, + "parent": parent, + "xyz": position, + "rpy": orientation, + } + + # If the tree macro information doesn't describe a generated file, generate it using the generic tree xacro. + urdf_content = xutils.load_urdf_from_xacro( + xacro_path=Tree._tree_xacro_path, mappings=urdf_mappings + ).toprettyxml() + if save_urdf: + xutils.save_urdf(urdf_content=urdf_content, urdf_path=self.urdf_path) else: urdf_content = xutils.load_urdf_from_xacro(xacro_path=self.urdf_path).toprettyxml() log.info(f"Loaded URDF from file '{self.urdf_path}'.") - return + return urdf_content def load_tree_obj(self): + """Loads a mesh .obj mesh file with its path defined by the tree_id_str""" if not os.path.exists(self.mesh_path): raise TreeException(f"Could not find file '{self.mesh_path}.") tree_obj = pywavefront.Wavefront(self.mesh_path, create_materials=True, collect_faces=True) return tree_obj def load_labeled_tree_obj(self): + """Loads a labeled mesh .obj mesh file with its path defined by the tree_id_str""" if not os.path.exists(self.labeled_mesh_path): raise TreeException(f"Could not find the file {self.labeled_mesh_path}") labeled_tree_obj = pywavefront.Wavefront(self.labeled_mesh_path, create_materials=True, collect_faces=True) @@ -500,11 +502,11 @@ def load_labeled_tree_obj(self): def make_trees_from_ids( pbutils: PyBUtils, tree_ids: list[int], - namespace: str = '', - pos: np.ndarray = np.array([0,0,0]), - orientation: np.ndarray = np.array([0,0,0,1]), + namespace: str = "", + pos: np.ndarray = np.array([0, 0, 0]), + orientation: np.ndarray = np.array([0, 0, 0, 1]), scale: float = 1.0, - randomize_pose: bool = False + randomize_pose: bool = False, ) -> list[Tree]: trees: list[Tree] = [] @@ -513,7 +515,6 @@ def make_trees_from_ids( Tree( pbutils=pbutils, tree_id=tree_id, - ) ) return trees diff --git a/pybullet_tree_sim/utils/mesh_downloader.py b/pybullet_tree_sim/utils/mesh_downloader.py new file mode 100644 index 0000000..1414a12 --- /dev/null +++ b/pybullet_tree_sim/utils/mesh_downloader.py @@ -0,0 +1,82 @@ +#!/usr/bin/env python3 +import os +import requests +from tqdm import tqdm +import zipfile + + +_here = os.path.abspath(__file__) +lib_path = os.path.dirname(os.path.dirname(_here)) +meshes_path = os.path.join(lib_path, "meshes") + + +def get_filename_from_url(url: str) -> str: + local_filename = url.split("/")[-1] + return os.path.join(meshes_path, local_filename) + + +def is_download_needed(target_file_path: str) -> bool: + if not os.path.exists(meshes_path): + os.mkdir(meshes_path) + return True + if not os.path.exists(os.path.join(meshes_path, "trees")): + if not os.path.isfile(os.path.join(meshes_path, "pybullet-tree-sim-meshes.zip")): # TODO: pass name into func + return True + else: + return False + else: + return False + + +def is_unzip_needed(target_file_path: str) -> bool: + if not os.path.exists(os.path.join(meshes_path, "trees")): + return True + else: + return False + + +def download_file(url: str, target_file_path: str) -> bool: + try: + # NOTE the stream=True parameter below + with requests.get(url, stream=True) as r: + r.raise_for_status() + total_size = int(r.headers.get("content-length", 0)) + with tqdm(total=total_size, unit="B", unit_scale=True) as progress_bar: + with open(target_file_path, "wb") as f: + for chunk in r.iter_content(chunk_size=8192): + # If you have chunk encoded response uncomment if + # and set chunk_size parameter to None. + # if chunk: + progress_bar.update(len(chunk)) + f.write(chunk) + + return True + + except Exception as e: + print(f"{e}") + return False + + +def unzip(zip_file: str): + with zipfile.ZipFile(zip_file, "r") as zipper: + zipper.extractall(os.path.dirname(zip_file)) + return + + +def main(): + url = "https://zenodo.org/records/14991250/files/pybullet-tree-sim-meshes.zip" + + file_abs_path = get_filename_from_url(url=url) + + download_needed = is_download_needed(target_file_path=file_abs_path) + if download_needed: + download_file(url=url, target_file_path=file_abs_path) + + unzip_needed = is_unzip_needed(target_file_path=file_abs_path) + if unzip_needed: + unzip(zip_file=file_abs_path) + return + + +if __name__ == "__main__": + main() diff --git a/pyproject.toml b/pyproject.toml index c8240d6..e2dcc1d 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -2,6 +2,14 @@ requires = ["hatchling"] build-backend = "hatchling.build" +# [tool.hatch.metadata] +# allow-direct-references = true + +# [tool.hatch.build.hooks.custom] +# path = "pybullet_tree_sim/utils/mesh_downloader.py" + + + [project] name = "pybullet_tree_sim" version = "0.0.1" @@ -28,7 +36,12 @@ dependencies = [ "pybullet==3.2.6", "pywavefront", "pyyaml", + 'requests', "scikit-image", + 'tqdm', "xacro", "zenlog", ] + +[project.scripts] +mesh_downloader = 'pybullet_tree_sim.utils.mesh_downloader:main' \ No newline at end of file