How add passive joints that is not included in DoF and linearly relate to another joint?

Question

I'm working on a motion planning problem with a continuum robot which is basically like: base_link --> actuation_joint --> link1 --> passive_joint1 --> link2 --> passive_joint2 --> .... Here this kinematic chain only has one DoF, the values of passive_joint1 and passive_joint2 are scalaractuation_joint, 2scalar*actuation_joint, etc. Now since this kinematic chain only has one DoF, I want only to include the actuation_joint in the kinematic calculation, such as the function RationalForwardKinematics. I also want to include the other passive_links in the collision check.

I now only know import robot using sdf/ urdf file but cannot find passive configurations in these formats. Then this sdf/ urdf file would be added to the plant and the plant will be sent to the IRIS functions like IrisInConfigurationSpace (GCS-SR) or IrisInRationalConfigurationSpace (C-IRIS). In this pipoeline, all the joints written in the sdf/ urdf file will be calculated as independent joint.

Thanks all in advance!

Answer 1

sorry for late update. I've been trying your suggestions. So far, Thomas Cohn's method works for me. In summary, firstly build a urdf file with mimic tag and import as a multibodyplant and set DiscreteContactApproximation and DiscreteContactSolver. Then doing the IRIS-NP for that robot_plant and taking the intersection. One interesting find is that even directly doing GcsTrajectoryOptimization without taking the intersection, the mimic property of the mimic joints still preserves. Intuitively, it's because the mimic joints and the mimicked joints holds the same initial and target value (mimic multiplier=1). But this maybe affected by the ContinuityConstraints options.

Answer 2

I think the easiest way to do this right now is to grow the regions in the full configuration space (as if all joints were actuated), and then take the intersection of those regions with the subspace described by the affine equality constraints induced by the mimic joints. (You can just add the halfspaces whose intersection describes the subspace to the polytope.) This would still preserve all the convexity rules we care about, and you could definitely use GCS with the resulting regions. Of course, IRIS-NP won't know it's trying to grow regions to maximize volume on the subspace (as opposed to volume in the full configuration space), so I'm not sure how useful the regions will be.

Long-term, we definitely want to provide support for directly growing regions along subspaces. It absolutely works well (we've even done it for curved submanifolds), but it will require some pretty significant rewriting to get the functionality available in Drake.

Answer 3

If I understand you correctly, the "mimic" tag in urdf is a good way to capture the fact that the passive joints are linear functions of your one active dof. For SDF, see #20704. Drake will parse that properly into a discrete-time MultibodyPlant, and it should work for simulation.

For motion planning, it will depend on which toolchain you are using. It looks like you'd like to do GcsTrajectoryOptimization? (great!) In that toolchain specifically, there is support for adding additional constraints, but the mimic constraints from multibody plant is not supported directly. In IrisOptions, you can pass prog_with_additional_constraints which includes the linear equality constraints; that should work with IrisInConfigurationSpace at least. And once you define the GcsTrajectoryOptimization problem using those regions, you can again add those linear constraints to the path decision variables. It's true that the regions will be defined in the original configuration space of the plant (not the reduced space of the active dof only); and that this will potentially be less efficient, but the linear equality constraints should not be too bad.

It would be a reasonable request to teach IrisInConfigurationSpace and GcsTrajectoryOptimization to understand the MultibodyPlant coupler constraints directly; but we haven't done that yet.