haosulab / mplib Goto Github PK
View Code? Open in Web Editor NEWa Lightweight Motion Planning Package
Home Page: https://motion-planning-lib.readthedocs.io/
License: MIT License
a Lightweight Motion Planning Package
Home Page: https://motion-planning-lib.readthedocs.io/
License: MIT License
As of now, we have the ability to check collision between arm and world objects as well as arm and point-cloud. However, the collision code is a little jumbled up and not clear to read for devs.
Additionally, it is not intuitive for the user to use the set of APIs we have provided. In particular, I personally never know when I do a collision query, what is and is not turned on.
Finally, we allow the user to update the point cloud but not the ability to add/remove collision objects from the planning world.
Some items to do:
When the user does not pass in an srdf, we first look for it by changing the urdf file extension to srdf in the hope there will be one in the same directory. If that fails, we resort to generating an srdf ourselves.
However, it seems the generation function isn't quite correct and doesn't even record all the adjacent links.
Maybe you could check whether the file already ends with .convex.stl, and also check if the convex file exists?
https://github.com/haosulab/MPlib/blob/main/src/fcl_model.cpp#L32
Does it work and how is represented?
As titled, this might be due to a tolerance issue inside the planning algorithm. Look around for parameters to tune.
Hello, this is not a bug, but more a call for help :)
I am new into robot control and looked through the examples and I have been trying to create a motion plan, but I cannot really make it work.
My setup is a bit different. I have a panda robot and I am using ManiSkill. But somehow I am struggling to make a simple trajectory.
My move_group is the panda_hand_tcp
and initially its pose is Pose([0.00369545, 0.029311, 0.184837], [0.00760545, 0.999395, 0.0339194, 0.000981587])
(I got this information from SAPIEN Viewer). I am trying to make a planning to Pose([0, 0, 0.05], [0, 1, 0, 0])
, but I am constantly getting this inverse kinematics error.. I am not sure if there is any configuration I am doing wrong.
My ManiSkill environment control_mode is pd_ee_delta_pose
.
I appreciate if anyone has a tip on how I could proceed, or debug what is my problem :)
Since tag is often not provided in simulations, it would be better to allow URDF without it.
I would also be better to include a default SRDF collision pair generator.
FCL seems dead and lots of bugs.
Kolin has been running into half-plane bugs and segfaults.
The plane collision also segfaults.
Some users do not care about the rotation of the end-effector, as long as the arm reaches a certain position. This seems like a good opportunity to optimize the time needed to plan a motion.
One idea is to solve the IK without rotation constraints and then feed all the solutions to the planner asking it to return the first successful plan to any of the targets.
Before implementing the whole thing, it is useful to benchmark how much faster IK is without rotation constraints and how much faster planning is when multiple targets are admissible.
Hello,
I am trying to install MPlib using pip
to use it in my python project. But when I try to install it in a MacOS, I am getting the error below:
Building wheel for mplib (pyproject.toml) ... error
error: subprocess-exited-with-error
× Building wheel for mplib (pyproject.toml) did not run successfully.
│ exit code: 1
╰─> [287 lines of output]
running bdist_wheel
running build
running build_py
creating build
creating build/lib.macosx-10.9-x86_64-cpython-312
creating build/lib.macosx-10.9-x86_64-cpython-312/mplib
copying mplib/planner.py -> build/lib.macosx-10.9-x86_64-cpython-312/mplib
copying mplib/__init__.py -> build/lib.macosx-10.9-x86_64-cpython-312/mplib
copying mplib/urdf_utils.py -> build/lib.macosx-10.9-x86_64-cpython-312/mplib
creating build/lib.macosx-10.9-x86_64-cpython-312/mplib/sapien_utils
copying mplib/sapien_utils/conversion.py -> build/lib.macosx-10.9-x86_64-cpython-312/mplib/sapien_utils
copying mplib/sapien_utils/__init__.py -> build/lib.macosx-10.9-x86_64-cpython-312/mplib/sapien_utils
copying mplib/sapien_utils/srdf_exporter.py -> build/lib.macosx-10.9-x86_64-cpython-312/mplib/sapien_utils
copying mplib/sapien_utils/urdf_exporter.py -> build/lib.macosx-10.9-x86_64-cpython-312/mplib/sapien_utils
creating build/lib.macosx-10.9-x86_64-cpython-312/mplib/planning
copying mplib/planning/__init__.py -> build/lib.macosx-10.9-x86_64-cpython-312/mplib/planning
copying mplib/planning/ompl.py -> build/lib.macosx-10.9-x86_64-cpython-312/mplib/planning
creating build/lib.macosx-10.9-x86_64-cpython-312/mplib/kinematics
copying mplib/kinematics/pinocchio.py -> build/lib.macosx-10.9-x86_64-cpython-312/mplib/kinematics
copying mplib/kinematics/__init__.py -> build/lib.macosx-10.9-x86_64-cpython-312/mplib/kinematics
copying mplib/kinematics/kdl.py -> build/lib.macosx-10.9-x86_64-cpython-312/mplib/kinematics
creating build/lib.macosx-10.9-x86_64-cpython-312/mplib/examples
copying mplib/examples/moving_robot.py -> build/lib.macosx-10.9-x86_64-cpython-312/mplib/examples
copying mplib/examples/detect_collision.py -> build/lib.macosx-10.9-x86_64-cpython-312/mplib/examples
copying mplib/examples/constrained_planning.py -> build/lib.macosx-10.9-x86_64-cpython-312/mplib/examples
copying mplib/examples/__init__.py -> build/lib.macosx-10.9-x86_64-cpython-312/mplib/examples
copying mplib/examples/demo_setup.py -> build/lib.macosx-10.9-x86_64-cpython-312/mplib/examples
copying mplib/examples/collision_avoidance.py -> build/lib.macosx-10.9-x86_64-cpython-312/mplib/examples
copying mplib/examples/two_stage_motion.py -> build/lib.macosx-10.9-x86_64-cpython-312/mplib/examples
copying mplib/examples/demo.py -> build/lib.macosx-10.9-x86_64-cpython-312/mplib/examples
creating build/lib.macosx-10.9-x86_64-cpython-312/mplib/collision_detection
copying mplib/collision_detection/__init__.py -> build/lib.macosx-10.9-x86_64-cpython-312/mplib/collision_detection
copying mplib/collision_detection/fcl.py -> build/lib.macosx-10.9-x86_64-cpython-312/mplib/collision_detection
running egg_info
writing mplib.egg-info/PKG-INFO
writing dependency_links to mplib.egg-info/dependency_links.txt
writing requirements to mplib.egg-info/requires.txt
writing top-level names to mplib.egg-info/top_level.txt
reading manifest file 'mplib.egg-info/SOURCES.txt'
adding license file 'LICENSE'
writing manifest file 'mplib.egg-info/SOURCES.txt'
/private/var/folders/3z/3c7znnwn4xx154m06z799rpm0000gn/T/pip-build-env-kbhifi26/overlay/lib/python3.12/site-packages/setuptools/command/build_py.py:207: _Warning: Package 'mplib.pymp' is absent from the `packages` configuration.
!!
********************************************************************************
############################
# Package would be ignored #
############################
Python recognizes 'mplib.pymp' as an importable package[^1],
but it is absent from setuptools' `packages` configuration.
This leads to an ambiguous overall configuration. If you want to distribute this
package, please make sure that 'mplib.pymp' is explicitly added
to the `packages` configuration field.
Alternatively, you can also rely on setuptools' discovery methods
(for example by using `find_namespace_packages(...)`/`find_namespace:`
instead of `find_packages(...)`/`find:`).
You can read more about "package discovery" on setuptools documentation page:
- https://setuptools.pypa.io/en/latest/userguide/package_discovery.html
If you don't want 'mplib.pymp' to be distributed and are
already explicitly excluding 'mplib.pymp' via
`find_namespace_packages(...)/find_namespace` or `find_packages(...)/find`,
you can try to use `exclude_package_data`, or `include-package-data=False` in
combination with a more fine grained `package-data` configuration.
You can read more about "package data files" on setuptools documentation page:
- https://setuptools.pypa.io/en/latest/userguide/datafiles.html
[^1]: For Python, any directory (with suitable naming) can be imported,
even if it does not contain any `.py` files.
On the other hand, currently there is no concept of package data
directory, all directories are treated like packages.
********************************************************************************
!!
check.warn(importable)
/private/var/folders/3z/3c7znnwn4xx154m06z799rpm0000gn/T/pip-build-env-kbhifi26/overlay/lib/python3.12/site-packages/setuptools/command/build_py.py:207: _Warning: Package 'mplib.pymp.collision_detection' is absent from the `packages` configuration.
!!
********************************************************************************
############################
# Package would be ignored #
############################
Python recognizes 'mplib.pymp.collision_detection' as an importable package[^1],
but it is absent from setuptools' `packages` configuration.
This leads to an ambiguous overall configuration. If you want to distribute this
package, please make sure that 'mplib.pymp.collision_detection' is explicitly added
to the `packages` configuration field.
Alternatively, you can also rely on setuptools' discovery methods
(for example by using `find_namespace_packages(...)`/`find_namespace:`
instead of `find_packages(...)`/`find:`).
You can read more about "package discovery" on setuptools documentation page:
- https://setuptools.pypa.io/en/latest/userguide/package_discovery.html
If you don't want 'mplib.pymp.collision_detection' to be distributed and are
already explicitly excluding 'mplib.pymp.collision_detection' via
`find_namespace_packages(...)/find_namespace` or `find_packages(...)/find`,
you can try to use `exclude_package_data`, or `include-package-data=False` in
combination with a more fine grained `package-data` configuration.
You can read more about "package data files" on setuptools documentation page:
- https://setuptools.pypa.io/en/latest/userguide/datafiles.html
[^1]: For Python, any directory (with suitable naming) can be imported,
even if it does not contain any `.py` files.
On the other hand, currently there is no concept of package data
directory, all directories are treated like packages.
********************************************************************************
!!
check.warn(importable)
/private/var/folders/3z/3c7znnwn4xx154m06z799rpm0000gn/T/pip-build-env-kbhifi26/overlay/lib/python3.12/site-packages/setuptools/command/build_py.py:207: _Warning: Package 'mplib.pymp.kinematics' is absent from the `packages` configuration.
!!
********************************************************************************
############################
# Package would be ignored #
############################
Python recognizes 'mplib.pymp.kinematics' as an importable package[^1],
but it is absent from setuptools' `packages` configuration.
This leads to an ambiguous overall configuration. If you want to distribute this
package, please make sure that 'mplib.pymp.kinematics' is explicitly added
to the `packages` configuration field.
Alternatively, you can also rely on setuptools' discovery methods
(for example by using `find_namespace_packages(...)`/`find_namespace:`
instead of `find_packages(...)`/`find:`).
You can read more about "package discovery" on setuptools documentation page:
- https://setuptools.pypa.io/en/latest/userguide/package_discovery.html
If you don't want 'mplib.pymp.kinematics' to be distributed and are
already explicitly excluding 'mplib.pymp.kinematics' via
`find_namespace_packages(...)/find_namespace` or `find_packages(...)/find`,
you can try to use `exclude_package_data`, or `include-package-data=False` in
combination with a more fine grained `package-data` configuration.
You can read more about "package data files" on setuptools documentation page:
- https://setuptools.pypa.io/en/latest/userguide/datafiles.html
[^1]: For Python, any directory (with suitable naming) can be imported,
even if it does not contain any `.py` files.
On the other hand, currently there is no concept of package data
directory, all directories are treated like packages.
********************************************************************************
!!
check.warn(importable)
/private/var/folders/3z/3c7znnwn4xx154m06z799rpm0000gn/T/pip-build-env-kbhifi26/overlay/lib/python3.12/site-packages/setuptools/command/build_py.py:207: _Warning: Package 'mplib.pymp.planning' is absent from the `packages` configuration.
!!
********************************************************************************
############################
# Package would be ignored #
############################
Python recognizes 'mplib.pymp.planning' as an importable package[^1],
but it is absent from setuptools' `packages` configuration.
This leads to an ambiguous overall configuration. If you want to distribute this
package, please make sure that 'mplib.pymp.planning' is explicitly added
to the `packages` configuration field.
Alternatively, you can also rely on setuptools' discovery methods
(for example by using `find_namespace_packages(...)`/`find_namespace:`
instead of `find_packages(...)`/`find:`).
You can read more about "package discovery" on setuptools documentation page:
- https://setuptools.pypa.io/en/latest/userguide/package_discovery.html
If you don't want 'mplib.pymp.planning' to be distributed and are
already explicitly excluding 'mplib.pymp.planning' via
`find_namespace_packages(...)/find_namespace` or `find_packages(...)/find`,
you can try to use `exclude_package_data`, or `include-package-data=False` in
combination with a more fine grained `package-data` configuration.
You can read more about "package data files" on setuptools documentation page:
- https://setuptools.pypa.io/en/latest/userguide/datafiles.html
[^1]: For Python, any directory (with suitable naming) can be imported,
even if it does not contain any `.py` files.
On the other hand, currently there is no concept of package data
directory, all directories are treated like packages.
********************************************************************************
!!
check.warn(importable)
copying mplib/py.typed -> build/lib.macosx-10.9-x86_64-cpython-312/mplib
creating build/lib.macosx-10.9-x86_64-cpython-312/mplib/pymp
copying mplib/pymp/__init__.pyi -> build/lib.macosx-10.9-x86_64-cpython-312/mplib/pymp
creating build/lib.macosx-10.9-x86_64-cpython-312/mplib/pymp/collision_detection
copying mplib/pymp/collision_detection/__init__.pyi -> build/lib.macosx-10.9-x86_64-cpython-312/mplib/pymp/collision_detection
copying mplib/pymp/collision_detection/fcl.pyi -> build/lib.macosx-10.9-x86_64-cpython-312/mplib/pymp/collision_detection
creating build/lib.macosx-10.9-x86_64-cpython-312/mplib/pymp/kinematics
copying mplib/pymp/kinematics/__init__.pyi -> build/lib.macosx-10.9-x86_64-cpython-312/mplib/pymp/kinematics
copying mplib/pymp/kinematics/kdl.pyi -> build/lib.macosx-10.9-x86_64-cpython-312/mplib/pymp/kinematics
copying mplib/pymp/kinematics/pinocchio.pyi -> build/lib.macosx-10.9-x86_64-cpython-312/mplib/pymp/kinematics
creating build/lib.macosx-10.9-x86_64-cpython-312/mplib/pymp/planning
copying mplib/pymp/planning/__init__.pyi -> build/lib.macosx-10.9-x86_64-cpython-312/mplib/pymp/planning
copying mplib/pymp/planning/ompl.pyi -> build/lib.macosx-10.9-x86_64-cpython-312/mplib/pymp/planning
running build_ext
CMake Warning:
Ignoring extra path from command line:
"/private/var/folders/3z/3c7znnwn4xx154m06z799rpm0000gn/T/pip-install-xnxme_ni/mplib_61644204c213440fbd06ef1a1ff895a0"
CMake Error: The source directory "/private/var/folders/3z/3c7znnwn4xx154m06z799rpm0000gn/T/pip-install-xnxme_ni/mplib_61644204c213440fbd06ef1a1ff895a0" does not appear to contain CMakeLists.txt.
Specify --help for usage, or press the help button on the CMake GUI.
['-DCMAKE_LIBRARY_OUTPUT_DIRECTORY=/private/var/folders/3z/3c7znnwn4xx154m06z799rpm0000gn/T/pip-install-xnxme_ni/mplib_61644204c213440fbd06ef1a1ff895a0/build/lib.macosx-10.9-x86_64-cpython-312/mplib/', '-DPYTHON_EXECUTABLE=/usr/local/anaconda3/envs/myenv/bin/python', '-DCMAKE_BUILD_TYPE=Release'] ['-j16']
Traceback (most recent call last):
File "/usr/local/anaconda3/envs/myenv/lib/python3.12/site-packages/pip/_vendor/pyproject_hooks/_in_process/_in_process.py", line 353, in <module>
main()
File "/usr/local/anaconda3/envs/myenv/lib/python3.12/site-packages/pip/_vendor/pyproject_hooks/_in_process/_in_process.py", line 335, in main
json_out['return_val'] = hook(**hook_input['kwargs'])
^^^^^^^^^^^^^^^^^^^^^^^^^^^^
File "/usr/local/anaconda3/envs/myenv/lib/python3.12/site-packages/pip/_vendor/pyproject_hooks/_in_process/_in_process.py", line 251, in build_wheel
return _build_backend().build_wheel(wheel_directory, config_settings,
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
File "/private/var/folders/3z/3c7znnwn4xx154m06z799rpm0000gn/T/pip-build-env-kbhifi26/overlay/lib/python3.12/site-packages/setuptools/build_meta.py", line 410, in build_wheel
return self._build_with_temp_dir(
^^^^^^^^^^^^^^^^^^^^^^^^^^
File "/private/var/folders/3z/3c7znnwn4xx154m06z799rpm0000gn/T/pip-build-env-kbhifi26/overlay/lib/python3.12/site-packages/setuptools/build_meta.py", line 395, in _build_with_temp_dir
self.run_setup()
File "/private/var/folders/3z/3c7znnwn4xx154m06z799rpm0000gn/T/pip-build-env-kbhifi26/overlay/lib/python3.12/site-packages/setuptools/build_meta.py", line 311, in run_setup
exec(code, locals())
File "<string>", line 74, in <module>
File "/private/var/folders/3z/3c7znnwn4xx154m06z799rpm0000gn/T/pip-build-env-kbhifi26/overlay/lib/python3.12/site-packages/setuptools/__init__.py", line 103, in setup
return distutils.core.setup(**attrs)
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
File "/private/var/folders/3z/3c7znnwn4xx154m06z799rpm0000gn/T/pip-build-env-kbhifi26/overlay/lib/python3.12/site-packages/setuptools/_distutils/core.py", line 184, in setup
return run_commands(dist)
^^^^^^^^^^^^^^^^^^
File "/private/var/folders/3z/3c7znnwn4xx154m06z799rpm0000gn/T/pip-build-env-kbhifi26/overlay/lib/python3.12/site-packages/setuptools/_distutils/core.py", line 200, in run_commands
dist.run_commands()
File "/private/var/folders/3z/3c7znnwn4xx154m06z799rpm0000gn/T/pip-build-env-kbhifi26/overlay/lib/python3.12/site-packages/setuptools/_distutils/dist.py", line 969, in run_commands
self.run_command(cmd)
File "/private/var/folders/3z/3c7znnwn4xx154m06z799rpm0000gn/T/pip-build-env-kbhifi26/overlay/lib/python3.12/site-packages/setuptools/dist.py", line 968, in run_command
super().run_command(command)
File "/private/var/folders/3z/3c7znnwn4xx154m06z799rpm0000gn/T/pip-build-env-kbhifi26/overlay/lib/python3.12/site-packages/setuptools/_distutils/dist.py", line 988, in run_command
cmd_obj.run()
File "/private/var/folders/3z/3c7znnwn4xx154m06z799rpm0000gn/T/pip-build-env-kbhifi26/normal/lib/python3.12/site-packages/wheel/bdist_wheel.py", line 368, in run
self.run_command("build")
File "/private/var/folders/3z/3c7znnwn4xx154m06z799rpm0000gn/T/pip-build-env-kbhifi26/overlay/lib/python3.12/site-packages/setuptools/_distutils/cmd.py", line 316, in run_command
self.distribution.run_command(command)
File "/private/var/folders/3z/3c7znnwn4xx154m06z799rpm0000gn/T/pip-build-env-kbhifi26/overlay/lib/python3.12/site-packages/setuptools/dist.py", line 968, in run_command
super().run_command(command)
File "/private/var/folders/3z/3c7znnwn4xx154m06z799rpm0000gn/T/pip-build-env-kbhifi26/overlay/lib/python3.12/site-packages/setuptools/_distutils/dist.py", line 988, in run_command
cmd_obj.run()
File "/private/var/folders/3z/3c7znnwn4xx154m06z799rpm0000gn/T/pip-build-env-kbhifi26/overlay/lib/python3.12/site-packages/setuptools/_distutils/command/build.py", line 132, in run
self.run_command(cmd_name)
File "/private/var/folders/3z/3c7znnwn4xx154m06z799rpm0000gn/T/pip-build-env-kbhifi26/overlay/lib/python3.12/site-packages/setuptools/_distutils/cmd.py", line 316, in run_command
self.distribution.run_command(command)
File "/private/var/folders/3z/3c7znnwn4xx154m06z799rpm0000gn/T/pip-build-env-kbhifi26/overlay/lib/python3.12/site-packages/setuptools/dist.py", line 968, in run_command
super().run_command(command)
File "/private/var/folders/3z/3c7znnwn4xx154m06z799rpm0000gn/T/pip-build-env-kbhifi26/overlay/lib/python3.12/site-packages/setuptools/_distutils/dist.py", line 988, in run_command
cmd_obj.run()
File "/private/var/folders/3z/3c7znnwn4xx154m06z799rpm0000gn/T/pip-build-env-kbhifi26/overlay/lib/python3.12/site-packages/setuptools/command/build_ext.py", line 91, in run
_build_ext.run(self)
File "/private/var/folders/3z/3c7znnwn4xx154m06z799rpm0000gn/T/pip-build-env-kbhifi26/overlay/lib/python3.12/site-packages/setuptools/_distutils/command/build_ext.py", line 359, in run
self.build_extensions()
File "/private/var/folders/3z/3c7znnwn4xx154m06z799rpm0000gn/T/pip-build-env-kbhifi26/overlay/lib/python3.12/site-packages/setuptools/_distutils/command/build_ext.py", line 479, in build_extensions
self._build_extensions_serial()
File "/private/var/folders/3z/3c7znnwn4xx154m06z799rpm0000gn/T/pip-build-env-kbhifi26/overlay/lib/python3.12/site-packages/setuptools/_distutils/command/build_ext.py", line 505, in _build_extensions_serial
self.build_extension(ext)
File "<string>", line 66, in build_extension
File "/usr/local/anaconda3/envs/myenv/lib/python3.12/subprocess.py", line 571, in run
raise CalledProcessError(retcode, process.args,
subprocess.CalledProcessError: Command '['cmake', '/private/var/folders/3z/3c7znnwn4xx154m06z799rpm0000gn/T/pip-install-xnxme_ni/mplib_61644204c213440fbd06ef1a1ff895a0', '-DCMAKE_LIBRARY_OUTPUT_DIRECTORY=/private/var/folders/3z/3c7znnwn4xx154m06z799rpm0000gn/T/pip-install-xnxme_ni/mplib_61644204c213440fbd06ef1a1ff895a0/build/lib.macosx-10.9-x86_64-cpython-312/mplib/', '-DPYTHON_EXECUTABLE=/usr/local/anaconda3/envs/myenv/bin/python', '-DCMAKE_BUILD_TYPE=Release']' returned non-zero exit status 1.
[end of output]
note: This error originates from a subprocess, and is likely not a problem with pip.
ERROR: Failed building wheel for mplib
Failed to build mplib
ERROR: Could not build wheels for mplib, which is required to install pyproject.toml-based projects
It seems that it is trying to compile something using CMake. I am familiar with C++ and CMake, but I am quite new to python and wheel, an so on. Any pointers on how could I solve this?
Kind regards,
Rafael
It would be nice to have a function take a sapien world object and populate the planning world accordingly.
After I attach an already vhacded nonconvex mesh to the end effector of panda arm with the command planner.update_attached_mesh(mesh_path, pose=Pose([0, 0, 0], [1, 0, 0, 0]))
, I try to print the FCLObject of the mesh by using planner.planning_world.get_object('panda_9_mesh')
, the output is [<mplib.pymp.collision_detection.fcl.CollisionObject object at 0x7fb631140470>]
, which seems that it only contains one single CollisionObject, does this mean that the mesh is actually loaded as a convex mesh? I try to compute IK of a non-collision pose, but every time it gives the failure that the attached mesh is in collision with the scene_pcd, of which the reason might be the mesh is actually loaded as a convex one.
The MPLib version I am using is 0.2.0a0. Thank you for you help in advance.
When the mesh files contain units other than meters, the object is not loaded up to the correct scale.
Does self.planning_world.collide_full()
(from here) check for collisions with the pointcloud added using update_point_cloud
(from here), or does it just check for collisions between links of the robot? Additionally, update_point_cloud
takes in pointcloud from the robot, as is obtained using the gym environment but centered around the robot's base, correct?
I tried to construct the following simple example: Here, the scene simply consists of a robot and a box (loaded using a custom .obj file -- I know the Sapien collision avoidance tutorial creates the box directly, but I want to be able to do this using custom .obj files). I place the robot to be in collision with the box, and I would like to detect this collision. I obtain a 360 degree pointcloud (as done in the ManiSkill gym environment), center the pointcloud to be around the robot's base, and then use update_point_cloud
, but somehow self.planning_world.collide_full()
is still empty:
import sapien.core as sapien
from sapien.utils import Viewer
import numpy as np
from scipy.spatial.transform import Rotation as R
from PIL import Image
from copy import deepcopy
from ManiSkill.mani_skill.env.camera import CombinedCamera, read_images_from_camera, read_pointclouds_from_camera
# these first four functions are from ManiSkill/mani_skill/env/base_env.py, modified to not use 'self'
def _load_camera(cam_info, agent, scene):
cam_info = deepcopy(cam_info)
if 'parent' in cam_info:
if cam_info['parent'] == 'robot':
parent = agent.get_base_link()
else:
assert False
parent = self.objects[cam_info['parent']]
if isinstance(parent, sapien.Articulation):
parent = parent.get_links()[0]
camera_mount_actor = parent
del cam_info['parent']
else:
camera_mount_actor = scene.create_actor_builder().build_kinematic()
pose = sapien.Pose(cam_info['position'], cam_info['rotation'])
del cam_info['position'], cam_info['rotation']
camera = scene.add_mounted_camera(
actor=camera_mount_actor, pose=pose, **cam_info, fovx=0
)
return camera
def render(mode='color_image', depth=False, seg=None, camera_names=None, scene=None, cameras=None):
scene.update_render()
if mode == 'human':
if self._viewer is None:
self._viewer = Viewer(self._renderer)
self._setup_viewer()
self._viewer.render()
return self._viewer
else:
if seg is not None:
if seg == 'visual':
seg_idx = 0
elif seg == 'actor':
seg_idx = 1
elif seg == 'both':
seg_idx = [0, 1]
else:
raise NotImplementedError()
else:
seg_idx = None
if camera_names is None:
cameras = self.cameras
else:
cameras_new = []
for camera in cameras:
if camera.get_name() in camera_names:
cameras_new.append(camera)
cameras = cameras_new
if mode == 'color_image' or mode == 'pointcloud':
views = {}
get_view_func = read_images_from_camera if mode == 'color_image' else read_pointclouds_from_camera
for cam in cameras:
cam.take_picture()
for cam in cameras:
if isinstance(cam, CombinedCamera):
view = cam.get_combined_view(mode, depth, seg_idx) # list of dict for image, dict for pointcloud
else:
view = get_view_func(cam, depth, seg_idx) # dict
views[cam.get_name()] = view
return views
def _post_process_view(view_dict, robot_link_ids=None):
actor_id_seg = view_dict['seg'] # (n, m, 1)
mask = np.zeros(actor_id_seg.shape, dtype=np.bool)
for actor_id in robot_link_ids:
mask = mask | ( actor_id_seg == actor_id )
view_dict['seg'] = mask
def post_processing(obs, obs_mode, robot_link_ids):
views = obs[obs_mode]
for cam_name, view in views.items():
if isinstance(view, list):
for view_dict in view:
_post_process_view(view_dict, robot_link_ids=robot_link_ids)
combined_view = {}
for key in view[0].keys():
combined_view[key] = np.concatenate([view_dict[key] for view_dict in view], axis=-1)
views[cam_name] = combined_view
else: # view is a dict
_post_process_view(view, robot_link_ids=robot_link_ids)
if len(views) == 1:
view = next(iter(views.values()))
obs[obs_mode] = view
return obs
def main():
# setup
engine = sapien.Engine()
renderer = sapien.VulkanRenderer()
engine.set_renderer(renderer)
scene = engine.create_scene()
scene.set_timestep(1 / 100.0)
rscene = scene.get_renderer_scene()
rscene.set_ambient_light([0.5, 0.5, 0.5])
rscene.add_directional_light([0, 1, -1], [0.5, 0.5, 0.5], shadow=True)
rscene.add_point_light([1, 2, 2], [1, 1, 1], shadow=True)
rscene.add_point_light([1, -2, 2], [1, 1, 1], shadow=True)
rscene.add_point_light([-1, 0, 1], [1, 1, 1], shadow=True)
# viewer
viewer = Viewer(renderer) # Create a viewer (window)
viewer.set_scene(scene) # Bind the viewer and the scene
viewer.set_camera_xyz(x=-4, y=0, z=2)
viewer.set_camera_rpy(r=0, p=-np.arctan2(2, 4), y=0)
viewer.window.set_camera_parameters(near=0.05, far=100, fovy=1)
# load box object
loader: sapien.URDFLoader = scene.create_urdf_loader()
urdf = 'box.urdf'
asset = loader.load(urdf)
rotation = np.array([1, 0, 0, 0])
translation = np.array([-1, -1, 0])
asset.set_pose(sapien.Pose(p=translation, q=rotation))
# load and place robot (using code from the gym environment so we can obtain pointclouds)
import pathlib
from mani_skill.utils.config_parser import (
preprocess,
process_variables,
process_variants,
)
import importlib
config_file = pathlib.Path('ManiSkill/mani_skill/assets/config_files/open_cabinet_drawer.yml')
yaml_config = preprocess(config_file)
config = deepcopy(yaml_config)
_level_rng = np.random.RandomState(seed=0)
variant_config = {'partnet_mobility_id': '1004'}
config = process_variables(config, _level_rng)
level_config, level_variant_config = process_variants(
config, _level_rng, variant_config
)
agent_config = level_config['agent']
module_name, class_name = agent_config['agent_class'].rsplit('.', 1)
module = importlib.import_module(module_name)
AgentClass = getattr(module, class_name)
agent = AgentClass(engine, scene, agent_config)
# choose robot configuration such that arm is colliding with box
cur_state = agent.get_state()
cur_state[12:14] = np.array([-1, 0])
cur_state[18:28] = np.zeros(10)
cur_state[19] = 1.45
cur_state[20] = -0.7 # 1.75
agent.set_state(cur_state)
# get pointcloud from this position
robot_link_ids = agent.get_link_ids()
cam_infos = [level_config['render']['cameras'][1]] # only keep robot cameras, no world camera
cameras = []
for cam_info in cam_infos:
if 'sub_cameras' in cam_info:
sub_cameras = [_load_camera(sub_cam_info, agent, scene) for sub_cam_info in cam_info['sub_cameras']]
combined_camera = CombinedCamera(cam_info['name'], sub_cameras)
cameras.append(combined_camera)
else:
assert False
camera = self._load_camera(cam_info)
cameras.append(camera)
seg='actor'
obs = {'agent': agent.get_state(with_controller_state=False), 'pointcloud': render(mode='pointcloud', camera_names=['robot'], seg=seg, scene=scene, cameras=cameras)}
obs = post_processing(obs, obs_mode='pointcloud', robot_link_ids=robot_link_ids)
np.save('pointcloud_test.npy', obs['pointcloud'])
# adjust pointcloud to be centered around robot
obs['pointcloud']['xyz'][:, 0] += 1
'''
# collision detection
import mplib
link_names = ['panda_link0', 'panda_link1', 'panda_link2', 'panda_link3', 'panda_link4', 'panda_link5', 'panda_link6', 'panda_link7', 'panda_link8', 'panda_hand', 'panda_leftfinger', 'panda_rightfinger']
joint_names = ['panda_joint1', 'panda_joint2', 'panda_joint3', 'panda_joint4', 'panda_joint5', 'panda_joint6', 'panda_joint7', 'panda_finger_joint1', 'panda_finger_joint2']
planner = mplib.Planner(
urdf="./assets/robot/panda/panda.urdf",
srdf="./assets/robot/panda/panda.srdf",
user_link_names=link_names,
user_joint_names=joint_names,
move_group="panda_hand",
joint_vel_limits=np.ones(7),
joint_acc_limits=np.ones(7))
planner.update_point_cloud(obs['pointcloud']['xyz'])
path = planner.plan(goal_pose=np.zeros(9),current_qpos=agent.get_state(by_dict=True)['qpos'][1:],use_point_cloud=True)
'''
while not viewer.closed: # Press key q to quit
viewer.render()
if __name__ == '__main__':
main()
With the collision detection portion at the end commented out, we can see in the viewer that the robot is clearly in collision with the box. However, when we uncomment this part, we see that self.planning_world.collide_full()
is still empty -- this is not the expected behavior, is it?
For completeness, box.urdf
:
<?xml version="1.0" ?>
<robot name="box">
<link name="link_box">
<visual name="box">
<origin xyz="0 0 0"/>
<geometry>
<mesh filename="box.obj"/>
</geometry>
</visual>
<collision>
<origin xyz="0 0 0"/>
<geometry>
<mesh filename="box.obj"/>
</geometry>
</collision>
</link>
</robot>
and box.obj
:
o Mesh
v -0.15 -0.15 -0.25
v 0.15 -0.15 -0.25
v 0.15 0.15 -0.25
v -0.15 0.15 -0.25
v -0.15 -0.15 0.5
v 0.15 -0.15 0.5
v 0.15 0.15 0.5
v -0.15 0.15 0.5
vn 0 -1 0.5
vn 0 1 0.5
vn -1 0 0.5
vn 1 0 0
vn 0 0 1
vn 0 0 -1
vt 0 0
vt 1 0
vt 0 1
vt 1 1
f 1/1/1 2/2/1 6/4/1
f 1/1/1 6/4/1 5/3/1
f 3/1/2 4/2/2 8/4/2
f 3/1/2 8/4/2 7/3/2
f 4/1/3 1/2/3 5/4/3
f 4/1/3 5/4/3 8/3/3
f 2/1/4 3/2/4 7/4/4
f 2/1/4 7/4/4 6/3/4
f 5/1/5 6/2/5 7/4/5
f 5/1/5 7/4/5 8/3/5
f 4/1/6 3/2/6 2/4/6
f 4/1/6 2/4/6 1/3/6
Apologies for the incredibly long post.
The general idea of the feature is that we want the robot arm not to be fixed anymore and instead sit on a cart that can move around in the environment. This can allow for interesting applications.
The feature will be realized in two parts:
After finished, make a tutorial/demo.
Two functions that given the pose of a robot (w/wo end effector attach) can be added to the planner:
The two functions should return boolean values.
Current algorithms ensure collision-free paths. However, some of these trajectories can come uncomfortably close to potential collisions, which is not ideal for real-world deployment.
Are there methods to prevent such extreme trajectories or introduce a 'buffer threshold' option to ensure the generated paths maintain a safe distance from potential collisions?
Every time we set move group, we also need to recalculate the ompl sample space.
See code here. It should be Cylinder instead of Capsule
Line 51 in 6418a68
Currently, when user supplies the wrong number joints values or such, the error message is very cryptic, and I quote:
python3.11: /usr/include/eigen3/Eigen/src/Core/DenseCoeffsBase.h:163: Eigen::DenseCoeffsBase<Derived, 0>::CoeffReturnType Eigen::DenseCoeffsBase<Derived, 0>::operator const [with Derived = Eigen::Matrix<double, -1, 1>; Eigen::DenseCoeffsBase<Derived, 0>::CoeffReturnType = const double&; Eigen::Index = long int]: Assertion `index >= 0 && index < size()' failed.
This gives an idea of there's some indexing issues but the library does not tell the user where the error happened. In fact, there are multiple places that can cause this message to be shown. For users' sanity, add array bound checks for each of these places so they don't need to spend hours debugging what's going on.
A lot of tasks require steady hands. For example, when grasping a glass of water or a plate of cookies, the robot needs to keep the end effector level throughout the execution of some movements.
Add a parameter inside the plan
API to indicate whether we want end-effector rotation to be fixed for the trajectory.
Currently, the collision API is partially exposed to the Python end user. In particular, there is a collision request function that will return a CollisionResult
object. This is unintuitive to use, and moreover, it does not seem to support self-collision.
Instead, we want to make the API dead simple and make a tutorial/example for people to copy off when they want to use this feature.
There will be two APIs:
check_for_self_collision
takes in an articulation object as well as a pose and returns pairs of links in contact with each other.check_for_env_collision
takes in an articulation object as well as a pose and returns its collisions with all other objects in the planning world.A lot of sapien collision geometry is composite. In particular, there are multiple collision geometries associated with a single collision link.
System:
OS version: Ubuntu 20.04
Python version (if applicable): Python 3.8
SAPIEN version (pip freeze | grep sapien): 2.2.2
Environment: Desktop
Describe the bug
I create an env with sapien but when I execute a planned action with env.step(action), where action is the target pose of the end effector (8-dim: 3 for pos, 4 for quat, 1 for gripper), it cannot reach my desired pose (but it manages to move in the correct direction). I guess it may be some problem with my simulation time or planning timestep, etc. But I cannot discover how to fix this problem.
Additional context
Some reference code here:
self._sim_freq = 500
self._control_freq = 100
self._sim_steps_per_control = self._sim_freq // self._control_freq
self.dt = 1.0 / (self._control_freq / self._sim_freq)
def step(self, action):
self.current_step += 1
self.success_planned = self.move_to_pose(action)
def move_to_pose(self, action):
pose = action[:-1]
open_gripper = action[-1]
self.plan_timestep = 0.1
result = self.planner.plan_screw(pose, self.agent.get_qpos(), time_step=self.plan_timestep) # mplib.Planner
if result["status"] != "Success":
self.scene.step()
return 1
self.follow_path(result)
return 0
def follow_path(self, result):
n_step = result["position"].shape[0]
assert n_step > 0, "no path"
min_step = int(self._sim_freq * self.plan_timestep * 2)
for i in range(min(min_step, n_step)):
for j in range(7):
self.active_joints[j].set_drive_target(result["position"][i][j])
self.active_joints[j].set_drive_velocity_target(result["velocity"][i][j])
self.scene.step()
Hi,
Does the library support planning for XArm6 robot? Its urdf is here and srdf is here. My code for setting up the planner is as follows:
link_names = [link.get_name() for link in self.robot.get_links()]
joint_names = [joint.get_name() for joint in self.robot.get_active_joints()]
self.hand_link_name = 'xarm_gripper_base_link'
self.planner = mplib.Planner(
urdf=os.path.join(self.config.robot_asset_path, "xarm/xarm6_with_gripper.urdf"),
srdf=os.path.join(self.config.robot_asset_path, "xarm/xarm6_with_gripper.srdf"),
user_link_names=link_names,
user_joint_names=joint_names,
move_group=self.hand_link_name,
joint_vel_limits=np.ones(6),
joint_acc_limits=np.ones(6))
Some additional stuff to do
When I try to install MPlib using pip install mplib
I get:
ERROR: Could not find a version that satisfies the requirement mplib (from versions: none)
ERROR: No matching distribution found for mplib
I am on MacOS with python 3.7 and pip 21.3.1. Any ideas?
Compilation is quite slow even on my 12900k. Looks like the linking takes the most amount of time but need to verify.
Profile the compilation process and identify the bottleneck. Then, look for solutions to speed it up.
I am creating a Planner object without passing the SRDF path, and the software is crashing:
self.__path_planner = Planner(urdf=ROBOT_INFO.urdf_path,
srdf="",
user_link_names=ROBOT_INFO.links,
user_joint_names=ROBOT_INFO.joints,
move_group=TARGET_LINK_TO_MOVE,
joint_vel_limits=np.ones(7),
joint_acc_limits=np.ones(7)
)
in the same place as the urdf, the srdf file is present, and the constructor is finding the srdf file. The reason for the crash is this line in version 0.1.1 (copied bellow)
if srdf == "":
self.generate_collision_pair() # it is expecting the object to have self.link_name_2_idx but only added later to object
self.robot.update_SRDF(self.srdf)
I see this is not main anymore. Do you have a forecast for an official release where this is fixed?
Document all the public apis
Could you please provide a packaged version for Python 3.10?
I've tried installing manually from source, but get hung up on the following errors:
/home/hartzj/MPlib/src/ompl_planner.h:1:10: fatal error: pinocchio/fwd.hpp: No such file or directory
1 | #include <pinocchio/fwd.hpp>
| ^~~~~~~~~~~~~~~~~~~
compilation terminated.
gmake[2]: *** [CMakeFiles/mp.dir/build.make:118: CMakeFiles/mp.dir/src/ompl_planner.cpp.o] Error 1
gmake[2]: *** Waiting for unfinished jobs....
/home/hartzj/MPlib/src/articulated_model.cpp:1:10: fatal error: pinocchio/algorithm/aba.hpp: No such file or directory
1 | #include <pinocchio/algorithm/aba.hpp>
| ^~~~~~~~~~~~~~~~~~~~~~~~~~~~~
compilation terminated.
gmake[2]: *** [CMakeFiles/mp.dir/build.make:76: CMakeFiles/mp.dir/src/articulated_model.cpp.o] Error 1
In file included from /home/hartzj/MPlib/src/kdl_model.h:4,
from /home/hartzj/MPlib/src/kdl_model.cpp:1:
/home/hartzj/MPlib/src/urdf_utils.h:3:10: fatal error: pinocchio/algorithm/jacobian.hpp: No such file or directory
3 | #include <pinocchio/algorithm/jacobian.hpp>
| ^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
compilation terminated.
gmake[2]: *** [CMakeFiles/mp.dir/build.make:104: CMakeFiles/mp.dir/src/kdl_model.cpp.o] Error 1
In file included from /home/hartzj/MPlib/src/fcl_model.cpp:2:
/home/hartzj/MPlib/src/urdf_utils.h:3:10: fatal error: pinocchio/algorithm/jacobian.hpp: No such file or directory
3 | #include <pinocchio/algorithm/jacobian.hpp>
| ^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
compilation terminated.
I've tried installing Pinocchio as described here, first from apt, then from pip.
It seems that there is some bug at:
Line 481 in 62e666e
relative_transform = np.linalg.inv(current_p) @ target_p
Hi, in planner.py, Planner.init(...), if the srdf == '', then on line 74, it calles self.generate_collision_pair(), before assign self_user_link_names. However, inside self.generate_collision_pair(), on line 138, n_link = len(self.user_link_names). This leads to AttributeError: 'Planner" object has no attribute 'user_link_names'.
As titled, some users may want to have robots interact with each other.
This is a low-priority item.
When planning with rrt connect, the arm frequently goes back and forth before finally reaching the goal pose. Moreover, even when there is a straight collision-free trajectory, the arm does not usually take this path.
As far as I remember, the moveit planner does not make such movements using the same rrt connect.
Steps to take:
examples/avoid_collision.py
Currently urdf_util.cpp
always finds the last occurrence of /
and treat everything before as the package directory. However, when the urdf is in the same dir as the working directory, user may skip the ./
which causes file not found error.
A declarative, efficient, and flexible JavaScript library for building user interfaces.
🖖 Vue.js is a progressive, incrementally-adoptable JavaScript framework for building UI on the web.
TypeScript is a superset of JavaScript that compiles to clean JavaScript output.
An Open Source Machine Learning Framework for Everyone
The Web framework for perfectionists with deadlines.
A PHP framework for web artisans
Bring data to life with SVG, Canvas and HTML. 📊📈🎉
JavaScript (JS) is a lightweight interpreted programming language with first-class functions.
Some thing interesting about web. New door for the world.
A server is a program made to process requests and deliver data to clients.
Machine learning is a way of modeling and interpreting data that allows a piece of software to respond intelligently.
Some thing interesting about visualization, use data art
Some thing interesting about game, make everyone happy.
We are working to build community through open source technology. NB: members must have two-factor auth.
Open source projects and samples from Microsoft.
Google ❤️ Open Source for everyone.
Alibaba Open Source for everyone
Data-Driven Documents codes.
China tencent open source team.