Support planning with joint groups

[sea-bass]

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.

[henrygerardmoore]

I'm going to try this out before trying to merge the PR for #65 (just putting this here so people can see it's WIP :) )

[henrygerardmoore]

I'm working on this now. Currently, I have a class, JointGroup that wraps a pinocchio.Model:

class JointGroup:
    model : pinocchio.Model

    def __init__(self, model : pinocchio.Model, indices):
        self.model = model
        self.joint_indices = indices
    def __getattr__(self, name):
        return getattr(self.model, name)

The idea being that JointGroup just adds functionality, but you can still also use it in place of a pinocchio.Model without having to explicitly wrap every function.

There is a second class, JointGroupManager, that has a pinocchio.Model but doesn't wrap it (since you shouldn't be able to use it in place of a pinocchio.Model). It has a dict mapping str to a JointGroup. Currently, you can create a JointGroup by passing in a list of names like so:

joint_group_manager.make_joint_group("Test", ["panda_finger_joint1", "panda_finger_joint2"])

And that adds a JointGroup associated with those two joints (and that has a reference to the shared pinocchio.Model) to the internal dict at entry "Test".

It seems like this JointGroupManager will need to have a concept of state, so that when planning or visualizing with a specific group, the other joints have some default value. I'm thinking of having a SetState method or something similar that sets all the states of a JointGroupManager. Then, any state-modifying methods called on a JointGroup should be able to modify the parent JointGroupManager's state. So should a JointGroup keep a reference to their parent JointGroupManager or something (e.g. self.manager or self.parent)?

Another question is what should I add overrides for in JointModel? Looking through the codebase where pinocchio.Model is used, it seems that JointModel should override everything that we would want to use in utils.py, among others. I just wanted to check that this is a sensible approach before going down this path, as I'm still not very familiar with python best practices in bigger projects.

You can look at the WIP code here for the actual classes and here for the test.

[sea-bass]

"Because who can come up with a better name than slapping manager on it?"

Absolute genius 🏅

---

Thank you for taking a crack at this! The way that I've implemented this in other settings actually removes one layer of abstraction, in that you don't necessarily need a joint group manager class.

The key idea is that right now, everything we do in PyRoboPlan directly accepts a pinocchio.Model and pinocchio.GeometryModel (for the collisions). My view was basically to wrap this all in one class that contains both of those models plus the necessary data structures to support groups. Something like (in pseudo-Python)

class PlanningContext:  # you could just as well call this a manager lol
    model = ???
    visual_model = ???
    collision_model = ???

    joint_groups_map = {
        "arm": JointGroup(...),
        "gripper": JointGroup(...),
    }

Then, actually all the PyRoboPlan functions could directly accept one of these objects which will shrink down the function signatures in tons of places.

Would something like this make sense to you? I actually had a really minor placeholder for this already in https://github.com/sea-bass/pyroboplan/blob/main/src/pyroboplan/core/planning_context.py

---

Also, re: your last question about requiring state, that is certainly true. In my related implementation, and also in things like MoveIt's RobotModel, you always carry around a full state of the whole model, and when you're dealing with subgroups of that model, you're basically getting/setting the indices that correspond to that group.

[henrygerardmoore]

@sea-bass yes, I think that totally makes sense! Thanks for the response and examples, just wanted to make sure I was on the right track before making more progress. I'll take that into account!