Stabilizer gains applied in world coordinates

[ibergonzani]

in com_admittance there are have separated gains for x and y axis. The problem is that these are applied to the error in world coordinates. If gains have different values on x and y, then the admittance behavior changes based on the robot orientation in world coordinates.

inria_wbc/src/stabilizers/stabilizer.cpp

Lines 31 to 58 in 9266e64

	void com_admittance(
	double dt,
	const Eigen::VectorXd& p,
	const Eigen::Vector2d& cop_filtered,
	const tsid::trajectories::TrajectorySample& model_current_com,
	const tsid::trajectories::TrajectorySample& com_ref,
	tsid::trajectories::TrajectorySample& se3_sample)
	{
	IWBC_ASSERT("you need 6 coefficient in p for com admittance", p.size() == 6);
	if (std::abs(cop_filtered(0)) >= 10 && std::abs(cop_filtered(1)) >= 10)
	IWBC_ERROR("com_admittance : something is wrong with input cop_filtered, check sensor measurment: ", std::abs(cop_filtered(0)), " ", std::abs(cop_filtered(1)));
	Eigen::Vector2d ref = com_to_zmp(model_current_com); //because this is the target
	Eigen::Vector2d cor = ref.head(2) - cop_filtered;
	Eigen::Vector2d error = p.segment(0, 2).array() * cor.array();
	Eigen::VectorXd ref_m = com_ref.getValue() - Eigen::Vector3d(error(0), error(1), 0);
	error = p.segment(2, 2).array() * cor.array();
	Eigen::VectorXd vref_m = com_ref.getDerivative() - (Eigen::Vector3d(error(0), error(1), 0) / dt);
	error = p.segment(4, 2).array() * cor.array();
	Eigen::VectorXd aref_m = com_ref.getSecondDerivative() - (Eigen::Vector3d(error(0), error(1), 0) / (dt * dt));
	se3_sample.setValue(ref_m);
	se3_sample.setDerivative(vref_m);
	se3_sample.setSecondDerivative(aref_m);

this problem might be in other stabilizer too but I haven't used them and check.

I believe the admittance should be applied in a coordinate frame based on the floating base.

A solution would be to have a yaw rotation that express the forward direction of the robot (in the sample i create it from 2d vector), applied its inverse to the error and then bring it back in world coordinates after the gains product

`

    Eigen::Matrix2d fwd_rotation;
    fwd_rotation.col(0) = forward;
    fwd_rotation.col(1) << -forward(1), forward(0); // rotate forward 90 degrees counterclockwise

    Eigen::Vector2d cor = desired_zmp - center_of_pressure;

    // x-y gains are wrt sagittal and frontal axis of robot (not world coordinate)
    // error is rotated in robot frame, gains are applied then. Finally, the contribution is rotated back in world frame
    Eigen::Vector2d error = fwd_rotation * gains.segment<2>(0).cwiseProduct(fwd_rotation.transpose() * cor);
    Eigen::VectorXd ref_m = com_ref.getValue() - Eigen::Vector3d(error(0), error(1), 0);

    error = fwd_rotation * gains.segment<2>(2).cwiseProduct(fwd_rotation.transpose() * cor);
    Eigen::VectorXd vref_m = com_ref.getDerivative() - (Eigen::Vector3d(error(0), error(1), 0) / dt);

    error = fwd_rotation * gains.segment<2>(4).cwiseProduct(fwd_rotation.transpose() * cor);
    Eigen::VectorXd aref_m = com_ref.getSecondDerivative() - (Eigen::Vector3d(error(0), error(1), 0) / (dt * dt));

    se3_sample.setValue(ref_m);
    se3_sample.setDerivative(vref_m);
    se3_sample.setSecondDerivative(aref_m);`

[jbmouret]

I agree with you! Do you use this code? If so , this is good news.

Can you make a PR with the change?

[ibergonzani]

I am linking to inria_wbc but behaviors/controllers I made are in a different repo. The snippet of code is the one I used and tested. I will move it to the base library, test and make a PR.