estimate the external wrench given the actual feedback torque and joint pos, vel, acc?

[xibeisiber]

Hi all,

Is there any code to estimate the external wrench given the actual feedback torque and joint pos, vel, acc in KDL?

Thanks!

[MatthijsBurgh]

@smits @meyerj Do you have any suggestion?

[DjoleMNE]

Hi all,

I think it would be useful to have such a component in KDL. An already implemented external-wrench estimator would make a good contribution to the robotics community.

Some time ago, my colleagues and I have investigated state-of-the-art estimators to enable wrench feedback for our robot arms that are not equipped with a Force/Torque sensors. In the end, we opted for an approach that i) is relatively simple to implement; ii) provides accurate and DoF-decoupled results; and iii) requires (only) the typical feedback from the robot (joint position, velocity and torque), i.e. the momentum-observer presented in [1]. The publication in [1] is a survey that covers some other external-wrench estimation approaches as well and its results show that the momentum observer is the best candidate among all listed approaches. However, this momentum-based approach can also work with commanded instead of measured joint-torques, if a real joint-torque sensor does not exist in the robot's joint but, results, in that case, a bit noisier.

[1] Haddadin, Sami, Alessandro De Luca, and Alin Albu-Schäffer. "Robot collisions: A survey on detection, isolation, and identification." IEEE Transactions on Robotics 33.6 (2017): 1292-1312.

In our labs, we tested this approach on Kinova Gen3 7-DoF torque control arms. Both, i) experiment with loads for which we have ground-truth parameters and ii) experiment with contact-force estimation in a typical robotics use case showed good results from this estimator. For instance, in a polishing a surface use case that requires a force control (and thus, external-wrench feedback), this estimator enabled sufficiently well results:
https://drive.google.com/file/d/1LkXb8OhgQaqI_QLAWKiWELGTpKuuDBwt/view?usp=sharing
https://drive.google.com/file/d/1N6R1Lfys1FX4tGAjYv5I1Jc6Qx5zbH4W/view?usp=sharing

It should be noted that this estimator (similarly as all other approaches out there) requires a good dynamics model (mass-inertia parameters) of the robot to produce those results. If a robot is also equipped with real joint-torque sensors this improves estimation results because, usually in that case, the low-level torque-control loop in each joint tracks the torque-commands more accurately then the low-level loop that is only using joint-current information. In our case, we had a quite accurate dynamics model of our robot and the arms have real joint-torque sensors mounted on the output shaft of each joint.

Here, I will share a C++ method that I implemented for estimating the external wrench on the robot's tool-tip. The computations are made using the solvers from the KDL library.

// Momentum-observer for wrench estimation
int estimate_external_wrench(const KDL::JntArray &joint_position_measured,
                             const KDL::JntArray &joint_velocity_measured,
                             const KDL::JntArray &joint_torque_measured, 
                             KDL::Wrench &ext_force_torque)
{
    /**
    * ==========================================================================
    * Momentum observer, an implementation based on:
    * S. Haddadin, A. De Luca and A. Albu-Schäffer,
    * "Robot Collisions: A Survey on Detection, Isolation, and Identification,"
    * in IEEE Transactions on Robotics, vol. 33(6), pp. 1292-1312, 2017.
    * ==========================================================================
    */
    int solver_result = this->dynamic_parameter_solver_->JntToMass(joint_position_measured, jnt_mass_matrix_);
    if (solver_result != 0) return solver_result;
    solver_result = this->dynamic_parameter_solver_->JntToCoriolis(joint_position_measured, joint_velocity_measured, coriolis_torque_);
    if (solver_result != 0) return solver_result;
    solver_result = this->dynamic_parameter_solver_->JntToGravity(joint_position_measured, gravity_torque_);
    if (solver_result != 0) return solver_result;

    jnt_mass_matrix_dot_.data = (jnt_mass_matrix_.data - previous_jnt_mass_matrix_.data) / DT_SEC_;
    previous_jnt_mass_matrix_.data = jnt_mass_matrix_.data;

    total_torque_estimation_.data = robot_state_.control_torque.data - gravity_torque_.data - coriolis_torque_.data + jnt_mass_matrix_dot_.data * joint_velocity_measured.data;
    // total_torque_estimation_.data = -joint_torque_measured.data - gravity_torque_.data - coriolis_torque_.data + jnt_mass_matrix_dot_.data * joint_velocity_measured.data;

    estimated_momentum_integral_.data += (total_torque_estimation_.data + estimated_ext_torque_.data) * DT_SEC_;

    model_based_jnt_momentum_.data = jnt_mass_matrix_.data * joint_velocity_measured.data;
    estimated_ext_torque_.data = wrench_estimation_gain_.asDiagonal() * (model_based_jnt_momentum_.data -
                                                                         estimated_momentum_integral_.data -
                                                                         initial_jnt_momentum_.data);

    // First order low-pass filter
    double alpha = 0.5;
    for (int i = 0; i < NUM_OF_JOINTS_; i++)
        filtered_estimated_ext_torque_(i) = alpha * filtered_estimated_ext_torque_(i) + (1.0 - alpha) * estimated_ext_torque_(i);

    /**
    * ========================================================================================
    * Propagate joint torques to Cartesian wrench using a pseudo-inverse of Jacobian-Transpose
    * ========================================================================================
    */
    solver_result = jacobian_solver_.JntToJac(joint_position_measured, jacobian_end_eff_);
    if (solver_result != 0) return solver_result;

    solver_result = fk_pos_solver_->JntToCart(joint_position_measured, tool_tip_frame_full_model_);
    if (solver_result != 0) return solver_result;

    // Transform the jacobian from the base to the tool-tip frame
    jacobian_end_eff_.changeBase(tool_tip_frame_full_model_.M.Inverse());

    // Compute SVD of the jacobian using Eigen functions
    Eigen::JacobiSVD<Eigen::MatrixXd> svd(jacobian_end_eff_.data.transpose(), Eigen::ComputeThinU | Eigen::ComputeThinV);

    Eigen::VectorXd singular_inv(svd.singularValues());
    for (int j = 0; j < singular_inv.size(); ++j) singular_inv(j) = (singular_inv(j) < 1e-8) ? 0.0 : 1.0 / singular_inv(j);
    jacobian_end_eff_inv_.noalias() = svd.matrixV() * singular_inv.matrix().asDiagonal() * svd.matrixU().adjoint();

    // Compute End-Effector Cartesian forces from joint external torques
    Eigen::VectorXd wrench = jacobian_end_eff_inv_ * filtered_estimated_ext_torque_.data;
    for (int i = 0; i < NUM_OF_CONSTRAINTS_; i++) ext_force_torque(i) = wrench(i);

    return 0;
}

Here, for the last part that deals with propagating the estimated joint torques to the Cartesian wrench using a pseudo-inverse of a Jacobian, I used an SVD solver from the Eigen library. However, the code can be adjusted to use the stand-alone SVD solvers that are implemented in the KDL. For example, those that are used in:
https://github.com/orocos/orocos_kinematics_dynamics/blob/master/orocos_kdl/src/chainiksolvervel_wdls.cpp
https://github.com/orocos/orocos_kinematics_dynamics/blob/master/orocos_kdl/src/chainhdsolver_vereshchagin.cpp

There might be a good reason why @smits, Prof. H. Bruyninckx and their colleagues decided to go with a separate SVD solver, rather than use the one from Eigen.

I hope that this information and the code help!

Cheers,
Djordje

[DjoleMNE]

BTW, a novel method that uses measured acceleration data - published in [2] - might provide even better results than the above outlined momentum-based approach. However, for this technique to work, every robot's link has to be equipped with at least
one IMU. Nevertheless, even though there are no many robots out there with those sensing capabilities, this approach might also fit nicely in the KDL library.

[2] Birjandi, Seyed Ali Baradaran, Johannes Kühn, and Sami Haddadin. "Observer-Extended Direct Method for Collision Monitoring in Robot Manipulators Using Proprioception and IMU Sensing." IEEE Robotics and Automation Letters 5, no. 2 (2020): 954-961.

[xibeisiber]

Hi DjoleMNE,
Thanks very much for your detailed and helpful answer!
I've been looking for publications on this subject and found several methods. The first paper you mentioned helps me a lot in comparing them and presenting the momentum-based approach.
And also thanks for sharing the code!
Jiying

[smits]

@DjoleMNE , the main reason why we used/implemented our own SVD back in the days (before Eigen was stable), was to have a solver that would not do allocations on the heap, which might potentially break realtime usage.

[DjoleMNE]

@smits, thanks for the information!
Is that still the case with the current Eigen's SVD, or it can be now used for realtime computations?

And do you think that this type of external wrench estimator would be a good fit for KDL? I.e. should we go with implementing a class for it and make a pull request?

[smits]

Regarding Eigen's SVD, you either need to test it or dive into the source code to check if at-runtime allocations of dynamically sized objects are happening.

Regarding the estimator I see no reason why it would not be a good fit.

[DjoleMNE]

I see. Then I would say that it would be better to implement the pseudo-inverse-jacobian part of this estimator using a KDL's SVD.

Thanks for the feedback!