Adding support for a joint constrained to move along a planar curve.

[beaupollard]

We want to simulate robots moving along a curved path via a motorized carriage, similar to the robots shown below.

I was planning on modeling the curves as a piecewise combination linear and circular curvatures and creating a new joint class based heavily off of the planar joints. I was going to restrict the curves to lie on a plane and be parameterized by path length (s). So given s, \dot{s}, \ddot{s} and the curvature we can calculate the Motion, Forces, etc. This implementation would be similar to what was implemented here RobotLocomotion/drake#22196.

Some questions I have:

• Is the Pinocchio team already planned on implementing something similar in the future?
• Does it makes sense to base the new joint off of a planar joint?

I've only just begun to dive into the source code so any guidance or advice would be greatly appreciated.

[jorisv]

Hello @beaupollard,

• Is the Pinocchio team already planned on implementing something similar in the future?

We didn't plan to add this kind of joint

• Does it makes sense to base the new joint off of a planar joint?

It make sense to use the transformation computed from the planar joint since the new joint you propose will also produce a planar transformation.

Here some differences I have in mind (let's call your joint PathJoint):

• planar joint have 4 configuration parameters (NQ) (x, y, $cos{\theta}$, $sin{\theta}$) and 3 velocity parameters ($\dot{x}$, $\dot{y}$, $\dot{\theta}$).
• PathJoint will have 1 configuration parameters ($s$) and one velocity parameters ($\dot{s}$).
• I think PathJoint motion subspace (S) will not be constant. Then $\dot{S}$ should be calculated and you joint will have a bias ($c_J$).
• This is documented Featherstone Rigid Body Algorithm in Joint Models chapter.
• joint-universal.hpp and joint-spherical-ZYX.hpp implement it, but $c_J$ computation is commented for some reason (@jcarpent do you know why ?)

Joint code is heavily templated with a lot of specialization. It's not mandatory to implement all the specialization. You can probably make a first draft by using:

• JointMotionSupsaceTpl as Constraint_t
• SE3Tpl as Transformation_t
• MotionTpl as Motion_t and Bias_t

In a second time, you can specialize these structures to take advantage of any sparsity pattern in PathJoint.

Also, do you know why the drake implementation use piecewise combination linear and circular curvatures ? I would have used spline to model this kind of behavior.