Incosistent end-effector position from PinocchioEndEffectorKinematicsCppAd

[IoannisDadiotis]

Hi,

I am trying to add a soft constraint (EndEffectorConstraint similar to mobile manipulator example) for an arm EE tracking of a quadruped manipulator. This is done by adding an additional cost term on top of the default costs on state and input which are defined in the ocs2_legged_robot example. For tracking an EE pose a new buffer is created at the ReferenceManager and a new ros topic at the RosReferenceManager for communicating TargetTrajectories of the EE pose (although not sure if this is the best way to go, see issue 49).

I am not achieving good tracking and it does not seem to be a tuning issue. I have noticed the below unexpected behavior:

Within the EndEffectorConstraint::getValue() I print the position of the EE to be tracked through endEffectorKinematicsPtr_->getPosition(state).front() and I am comparing it with the published frames in the /tf topic (by simply rosrun tf tf_echo odom ee_frame). When the frame to be tracked belongs to the lower body (frames up to torso_2 link for centauro) it seems to work well and the position is consistent. In contrast, for any of the frames of the arm links the endEffectorKinematicsPtr_ gives position that is way different from the tf.

Note that:

• endEffectorKinematicsPtr_ points to a PinocchioEndEffectorKinematicsCppAd object constructed similarly with the feet EE kinematics in your ocs2_legged_robot ddp example. In particular with the following piece of code:

// define EE kinematics object
std::unique_ptr<EndEffectorKinematics<scalar_t>> eeKinematicsPtr;
const auto infoCppAd = centroidalModelInfo_.toCppAd();
const CentroidalModelPinocchioMappingCppAd pinocchioMappingCppAd(infoCppAd);
auto velocityUpdateCallback = [&infoCppAd](const ad_vector_t& state, PinocchioInterfaceCppAd& pinocchioInterfaceAd) {
  const ad_vector_t q = centroidal_model::getGeneralizedCoordinates(state, infoCppAd);
  updateCentroidalDynamics(pinocchioInterfaceAd, infoCppAd, q);
};
eeKinematicsPtr.reset(new PinocchioEndEffectorKinematicsCppAd(*pinocchioInterfacePtr_, pinocchioMappingCppAd, {name},
                                                              centroidalModelInfo_.stateDim, centroidalModelInfo_.inputDim,
                                                              velocityUpdateCallback, name, modelSettings_.modelFolderCppAd,
                                                              modelSettings_.recompileLibrariesCppAd, modelSettings_.verboseCppAd));

• Since EndEffectorConstraint::getValue() is called multiple times along the horizon of the optimization I get the EE position at those times. In my comparisons with the /tf I actually use the first EE position which corresponds to the time closest to the current one. (In any case the EE position does not change much along the horizon since I am comparing after the robot has entered the "steady state response", i.e. it does not really move.)

• The state vector passed as argument to the endEffectorKinematicsPtr_->getPosition(state) is consistent with the state observation (from /legged_robot_mpc_observation topic).

I was wondering if you have any clues about what can be causing this strange behavior or how to debug this?

Thank you

[IoannisDadiotis]

Solved. Joint ordering within the centroidal model was not consistent with the one within the pinocchio model.