Computing cartesian error / KDL::diff equivalent

[ahoarau]

I'd like to compute a cartesian error between two frames. I used to do it like this : Twist dX = diff( X_current , X_ref), where X_current the end effector position+rotation w.r.t the root link, X_ref the reference position+rotation from with trajectory planner, and dX (stored in a twist, although it is not really one), which represents [ dx dy dz wx wy wz].

Is there any equivalent of this in iDynTree ? Thanks !

[ahoarau]

Possible implementation :

    Eigen::Vector3d diff(const iDynTree::Rotation& R_a_b1, const iDynTree::Rotation& R_a_b2)
    {
        typedef Eigen::Matrix<double,3,3,Eigen::RowMajor> Matrix3dRowMajor;
        
        iDynTree::Rotation R_b1_b2 = R_a_b1.inverse() * R_a_b2;
        Eigen::AngleAxisd aa(Eigen::Map<const Matrix3dRowMajor>(R_b1_b2.data()));
        return toEigen(R_a_b1) * aa.angle() * aa.axis();
    }

    Eigen::Matrix<double,6,1> diff(const iDynTree::Transform& t_a_b1, const iDynTree::Transform& t_a_b2)
    {
        
        typedef Eigen::Matrix<double,6,1> Vector6d;
        Vector6d dX;
        
        dX.head<3>() = iDynTree::toEigen(t_a_b2.getPosition() - t_a_b1.getPosition());
        dX.tail<3>() = iDynTree::diff(t_a_b1.getRotation(),t_a_b2.getRotation());
        
        return dX;
    }

[ahoarau]

A bit better, in pure Eigen :

    Eigen::Vector3d diffRot(const Eigen::Matrix3d& R_a_b1, const Eigen::Matrix3d& R_a_b2)
    {
        Eigen::Matrix3d R_b1_b2 = R_a_b1.transpose() * R_a_b2;
        Eigen::AngleAxisd aa(Eigen::Map<const Eigen::Matrix3d>(R_b1_b2.data()));
        return R_a_b1 * aa.angle() * aa.axis();
    }
    
    Eigen::Matrix<double,6,1> diffTransform(const Eigen::Matrix4d& t_a_b1, const Eigen::Matrix4d& t_a_b2)
    {
        Eigen::Matrix<double,6,1> d_t1_t2;
        
        d_t1_t2.head<3>() = t_a_b2.block<3,1>(0,3) - t_a_b1.block<3,1>(0,3);
        d_t1_t2.tail<3>() = diffRot(t_a_b1.topLeftCorner<3,3>(),t_a_b2.topLeftCorner<3,3>());
        return d_t1_t2;
    }

Eigen::Matrix<double,6,1> err = diffTransform(iDynTree::toEigen(a.asHomogeneousTransform()), iDynTree::toEigen(b.asHomogeneousTransform()));

[ahoarau]

@traversaro gentle ping on this one

[traversaro]

Hi @ahoarau , sorry for the delay but I would like to check the math of the method, and this week I am travelling for work, I hope to check this next week.

[traversaro]

@gabrielenava @S-Dafarra @GiulioRomualdi any comment on this function and on how to implement it? Which kind of error do you tipically use on SO(3)/SE(3) ?

[S-Dafarra]

Actually there are many ways to compute a rotation error. [1] is a good reference for this, comparing also the performances of different methods. The one proposed above seems to be similar to ϕ₆ (equation 23 of [1]). In the past I also used the 2-norm of the difference of the desired and measured quaternions (equation 18 of [1]), or the norm of the euler angles of the "error" rotation matrix (see for example the IK implementation). In general I don't have any specific rule to decide which method to use, it usually depends on the context (for example in the IK code it was already possible to use euler angles as rotation parametrization, thus we kept using them also for the error definition). As another example, in the controllers, in order to avoid to use any parametrization (neither axis/angle), we use an approach similar to [2] (Lemma 5.11.2, page 180) to compute a desired angular velocity given a rotation error.

[1]: Metrics for 3D Rotations: Comparison and Analysis [2]: Nonlinear control of underactuated mechanical systems with application to robotics and aerospace vehicles

[francesco-romano]

[1] suggests to use the norm of the imaginary part of the quaternion representing the orientation error

[1] Siciliano et al. "Robotics. Modelling, planning and control"

[ahoarau]

So.. can we add one of these functions ?

[traversaro]

@prashanthr05 do you have any opinion on this? I think you added some similar in one of your repo, so if there is something that could be added in Rotation it would be great.

[prashanthr05]

@S-Dafarra had already ported a function to iDynTree to compute the geodesic distance between two rotations. https://github.com/robotology/idyntree/blob/e52d72a6fb3c0872bd9bcf830325776caf909db5/src/core/src/SO3Utils.cpp#L29

It implements (R1^(-1) * R2).log().

Calling something similar for iDynTree Transforms object should give SE(3) distance (which is different from SO(3) x R^3 mentioned in https://github.com/robotology/idyntree/issues/383#issuecomment-336179151) due to the differences in the definitions of exponential/logarithm map. I suppose this could also be used as a suitable Cartesian error ?

P.S. we had modified the Transform object to reflect SE(3) instead of SO(3) x R^3 recently with this PR https://github.com/robotology/idyntree/pull/562 according to this issue https://github.com/robotology/idyntree/issues/561.

[ahoarau]

Hello all ! Any news on that ?