Expand nullspace stiffness and damping parameters to be vector-valued?

[scastro-nasa]

Hi @danielsanjosepro -- this is the work account of @sea-bass (I recently landed the PR to support Pinocchio 4 in #62).

@ndunkelb-nasa and I were looking at setting up a pure joint impedance controller, and found this is possible by setting the task-space gains to zero and using a projection: none nullspace projector. All clear there.

As we understand, we see that the current mechanism for setting these equivalent joint impedance gains is to set the scalar nullspace_stiffness and nullspace_damping, plus weights as a scaling factor:

crisp_controllers/src/cartesian_controller.cpp

Lines 546 to 557 in 4905273

	auto weights = Eigen::VectorXd(model_.nv);
	for (size_t i = 0; i < params_.joints.size(); ++i) {
	weights[i] = params_.nullspace.weights.joints_map.at(params_.joints.at(i)).value;
	}
	nullspace_stiffness.diagonal() << params_.nullspace.stiffness * weights;
	// For nullspace, use explicit damping if > 0, otherwise compute from stiffness
	if (params_.nullspace.damping > 0) {
	nullspace_damping.diagonal() = params_.nullspace.damping * weights;
	} else {
	nullspace_damping.diagonal() = 2.0 * nullspace_stiffness.diagonal().cwiseSqrt();
	}

This has one glaring limitation in that the coupling of weights means that the ratios of all the joints' P and D gains are coupled, so we can't express any arbitrary combination of equivalent joint impedance PD gains, per-joint.

We were thinking, would you be open to us submitting a change where nullspace_stiffness and nullspace_damping can actually be vectors instead of scalars? If the vector is a single element, then it applies to all the robot's nv, but otherwise it must be the same size as nv but can have different values.

This would be a breaking change, which is why we ask. It would go from:

  nullspace: 
      stiffness: 5.0
      damping: 1.0
      projector_type: none 

to either

nullspace:
      stiffness: [5.0]
      damping: [1.0]
      projector_type: none 

or

nullspace:
      stiffness: [5.0, 4.0, 4.0, 3.0, 3.0, 3.0]
      damping: [1.0, 0.8, 0.8, 0.75, 0.75, 0.75]
      projector_type: none 

The other thing would be that at this point, the weights would also be redundant, so we could choose to keep them around for compatibility or simply absorb them into this new vector support.

Thoughts? Or are we missing something else in the usage?

[scastro-nasa]

Another alternative would be that weights actually get broken up into specific weights for stiffness vs. damping, which again would allow expressing any arbitrary combination of PD gains:

crisp_controllers/src/cartesian_admittance_controller.yaml

Lines 139 to 146 in 4905273

	weights:
	__map_joints:
	value:
	type: double
	default_value: 1.0
	description: "Weights for each joint for the nullspace parameters."
	validation:
	bounds<>: [0.0, 500.0]

This could have a path to being non-breaking, for example doing something like:

    weights:  # could rename to stiffness_weights for clarity, but then that would be a breaking change
      __map_joints:
        value:
          type: double
          default_value: 1.0
          description: "Weights for each joint for the nullspace parameters. This is used for stiffness values, and optionally for damping."
          validation:
            bounds<>: [0.0, 500.0]
    damping_weights:
      __map_joints:
        value:
          type: double
          description: "Weight overrides for each joint for the nullspace damping parameters. If not set, falls back to the stiffness values in 'weights' above."
          validation:
            bounds<>: [0.0, 500.0]

... although, it ends up being more mental math for the users to do in the background vs. just having a vector of stiffness/damping values and calling it a day.

[danielsanjosepro]

Hey @scastro-nasa !
My opinion to this: lets go with the breaking change. This early decision has been haunting me as well and it also makes the config files long and it also makes the configs robot specific:
https://github.com/learnsyslab/crisp_py/blob/6663f8d5019be28e6dc0cea109f96636d3d014a8/config/control/joint_control.yaml#L12-L18

If you have already some changes ready, lets go for it. I would also need to change crisp_py but the change should be minimal there.

[scastro-nasa]

I can get something to you soon!

Just to clarify, by "go with the breaking change", do you also want me to remove the weights map entirely, and simply rely on arrays for both nullspace stiffness and damping?

I'll still plan on expanding out single-element gain lists to model.nv as appropriate...