Should be possible to designate a different root link for planning purposes.

[hersh]

A URDF file has a fixed idea of which link is the root link. This limits the planning you can do, since the link closest (graph-wise) to the root link in a planning group is assumed to be fixed during the motion. However in case of a humanoid for example, the URDF root may be a link in the torso, making it difficult to make a full-body plan for coordinated arm and leg motion, for instance to pick something up off the ground.

I'm not sure about the solution exactly, but perhaps adding to RobotState a setRoot() function (or setRoots() to make multiple roots, like for a 2-legged robot). Or perhaps this belongs in JointModelGroup? So each group could have a different set of roots designated. Or maybe set it in PlanningScene?

[isucan]

This actually used to be implemented for the Groovy version of MoveIt. Having a setRoot() function for the kinematic model is pretty complicated as that changes the model, and the model is assumed to be constant throughout the system. What I did was to add a re-parenting option for the model at construction time. This means you could pick an arbitrary link in the model and construct it from the URDF, as if the URDF was rooted at that link. This requires changing some joint origin transforms, but it is not too hard to do. The reason I did not like that either and removed it at the refactoring of RobotModel in Hydro is that you then have different models, different states could be constructed for different models, frames of reference no longer match, things get pretty complicated. Instead, what I have done is rewrite the updateStateWithLinkAt() function, to allow specifying a "backward" flag, which defaults to false. Say you want to do planning for a humanoid robot and you want to see its configuration when its foot is at some location. You can figure out the pose of the foot link, call updateStateWithLinkAt(), set backward to true, and FK will be done 'in reverse' for the rest of the robot links, so the robot moves to the desired location. At that point you can do IK for instance, for the other foot. You could also pre-set the first food at a 'knees-bent' configuration before calling updateStateWithLinkAt(). I believe this is enough for implementing planning functionality and comes at a much smaller cost in terms of complexity for the robot_model/state libraries.

[hersh]

I'm glad to see some effort was put in towards this type of issue, but I'm not convinced yet that it would really solve the problem I'm looking at.

I would like the planner to decide how much to bend the knees, for instance. So I would like to be able to say something like "keeping the feet in their current poses relative to the ground, compute joint values for the legs, torso, and right arm such that the right wrist is 15cm above the object."

I don't see how updateStateWithLinkAt(reverse=true) will allow that. What I see that functionality doing is setting the pose of the root (say the torso) at a pose in space that would put the foot at a given place on the ground, but without moving any joints. Like picking up a rigid copy of the robot and gluing its foot to the floor. I see that you could also pre-compute a knees-bent joint state and do the same thing in order to get the robot into a state where the arms have a chance of reaching the object, but that doesn't use the full flexibility of the planners and solvers to find a good state or trajectory for all the joints together.

In fact, RobotState::setFromIK() includes options for solving for multiple end-effectors ("tips") simultaneously. Seems to me that this could do what I want with a change in coordinate frames or something. Imagine we ask it to plan for the left foot and right foot and right hand to be in the given poses, but specify one of the feet as the root link for this particular function call. Does that sound feasible? That way we aren't creating a different model, and the resulting contents of the RobotState should be all the same type of data as currently exists. The only thing being that the virtual root joint would have been repositioned by the call in accordance with the resulting pose of the torso link. Clearly it would need to be a "floating" root joint for this to make sense.