sea-bass / pyroboplan

Educational Python library for manipulator motion planning

Hi everyone 👋

I'm a robotics engineer and applied scientist working at The AI Institute.

Previously, I was at:

• (2022-2024) PickNik Robotics working on MoveIt Pro for robot behavior and planning in the ROS ecosystem.
• (2021-2022) Boston Dynamics working on behaviors for the Stretch warehouse robot.
• (2020-2021) The Robust Robotics Group at MIT CSAIL.
• (2017-2019) MathWorks, where I supported student competitions such as RoboCup. I still remain somewhat involved through initiatives like RoboCup@Home Education.

On my spare time, I like to teach things related to robotics, AI, and software development. Learn more at https://roboticseabass.com/.

pyroboplan's Issues

Consider adding a Ruckig example

Ruckig has some nice and straightforward support for trajectory generation, e.g. https://github.com/pantor/ruckig/blob/main/examples/03_waypoints.py

Unfortunately, doing multi-waypoint trajectories requires either calling out to their cloud API or their "Pro" version, but I guess people need to eat.

Consider adding a TOPP-RA example

https://github.com/hungpham2511/toppra has some examples we can use along with this library.

`max_angle_step` parameter is not necessarily an angle

There are several functions with a max_angle_step parameter described as:

max_angle_step : float
            Maximum angle step, in radians, for collision checking along path segments.

However, this is not necessarily an angle or in radians, as joints can also be prismatic.

Making this issue to do a sweep over all the instances of this.

Show motion planning while avoiding Octrees

Pinocchio already supports Octrees in HPP-FCL (for collision checking) and Meshcat (for visualization).

It would be nice to have official support, and an example, of motion planning with an Octree representing a custom collision geometry.

Create mobile base IK example

I think all these planners / IK solvers should be able to handle whole-body IK.

It would be nice to have a model of an arm on a mobile base (3DOF is okay), and also adding weighted pseudoinverses to the IK solver to balance between base and arm motion.

Add type hints

I want to do this so badly! But if someone wants to help, I won't be mad.

MyPy is promising: https://mypy-lang.org/, which has pre-commit hooks at https://github.com/pre-commit/mirrors-mypy

Would like to see how this plays with the docs generation as well.

Implement path shortcutting

There is a simple and (kind of) effective technique for shortcutting paths from e.g. RRT/PRM, which involves randomly picking non-adjacent points along a path and seeing if they can be connected directly.

See section 3.5.3 here: https://motion.cs.illinois.edu/RoboticSystems/MotionPlanningHigherDimensions.html

This would be a nice addition to this library.

Implement basic graph search algorithms

You mention them in the docs, and they'll be useful for future search algorithm implementations. I think a basic A* implementation for the graph class would be generally useful. As part of this, there are some improvements/modifications to the Graph interface that we might need to consider.

For instance, providing access to node objects through robot configurations may be necessary. As it currently stands, it is non-trivial to get access to a nodes neighbors without already having a pointer. Things like the following will be necessary:

>>> g = Graph()
>>> g.add_node(Node([0,0]))
>>> Node([0,0]) in g.nodes
False

I think adding some basic hashing and equality checks to the node classes will probably be worth it, and then maybe storing neighbors in a hashmap instead of sets to allow external consumers of the class to access them.

Once those are in place a basic A* is straightforward.

Consider adding blending to Cartesian planning

Instead of having to stop exactly at the intermediate points, there are common approaches to blending Cartesian paths.

This is one resource I like: https://www.diag.uniroma1.it/~deluca/rob1_en/14_TrajectoryPlanningCartesian.pdf

Whether just blending on position is enough, or we also need to find ways to do the same with rotation e.g., here remains TBD.

Upgrade to Pinocchio 3.x

Once Pinocchio 3.0 is out (which I believe is pending the release of HPP-FCL 3.0 as well), we should switch this library over.

This should give us the ability to get nearest/witness points from the CollisionRequest objects which should help with the collision-aware IK / trajectory optimization... I hope.

Support planning with joint groups

Right now, all planning is happening with the entire URDF; meaning all joints are accounted for in planning.

To be more in accordance with most motion planning frameworks, it would be nice to have this concept of "joint groups" so that planning can be done by fixing a subset of joint values and only planning for the desired joints.

At its core, this will require a wrapper class around the Pinocchio models, which can contain these joint groups as well as the model data. It will look closer to, e.g., a MoveIt planning scene or a Drake scene graph.

Support setting / getting named joint states

There should be an API to set/get joint states by name by storing them in dictionaries.

Additionally, this could support different groups, as well as loading the states from SRDF (which Pinocchio sort of already has).