Skip to content

Commit

Permalink
External wrench estimator: first-order momentum observer (#311)
Browse files Browse the repository at this point in the history 
External wrench estimator: first-order momentum observer
  • Loading branch information
@MatthijsBurgh
MatthijsBurgh authored May 23, 2021
2 parents 6ed7034 + 2bd28d8 commit a2bbe91
Show file tree
Hide file tree
Showing 4 changed files with 622 additions and 0 deletions.
214 changes: 214 additions & 0 deletions orocos_kdl/src/chainexternalwrenchestimator.cpp
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,214 @@
// Copyright (C) 2021 Djordje Vukcevic <djordje dot vukcevic at h-brs dot de>

// Version: 1.0
// Author: Djordje Vukcevic <djordje dot vukcevic at h-brs dot de>
// URL: http://www.orocos.org/kdl

// This library is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 2.1 of the License, or (at your option) any later version.

// This library is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// Lesser General Public License for more details.

// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA

#include "chainexternalwrenchestimator.hpp"

namespace KDL {

ChainExternalWrenchEstimator::ChainExternalWrenchEstimator(const Chain &chain, const Vector &gravity, const double sample_frequency, const double estimation_gain, const double filter_constant, const double eps, const int maxiter) :
CHAIN(chain),
DT_SEC(1.0 / sample_frequency), FILTER_CONST(filter_constant),
svd_eps(eps),
svd_maxiter(maxiter),
nj(CHAIN.getNrOfJoints()), ns(CHAIN.getNrOfSegments()),
jnt_mass_matrix(nj), previous_jnt_mass_matrix(nj), jnt_mass_matrix_dot(nj),
initial_jnt_momentum(nj), estimated_momentum_integral(nj), filtered_estimated_ext_torque(nj),
gravity_torque(nj), coriolis_torque(nj), total_torque(nj), estimated_ext_torque(nj),
jacobian_end_eff(nj),
jacobian_end_eff_transpose(Eigen::MatrixXd::Zero(nj, 6)), jacobian_end_eff_transpose_inv(Eigen::MatrixXd::Zero(6, nj)),
U(Eigen::MatrixXd::Zero(nj, 6)), V(Eigen::MatrixXd::Zero(6, 6)),
S(Eigen::VectorXd::Zero(6)), S_inv(Eigen::VectorXd::Zero(6)), tmp(Eigen::VectorXd::Zero(6)),
ESTIMATION_GAIN(Eigen::VectorXd::Constant(nj, estimation_gain)),
dynparam_solver(CHAIN, gravity),
jacobian_solver(CHAIN),
fk_pos_solver(CHAIN)
{
}

void ChainExternalWrenchEstimator::updateInternalDataStructures()
{
nj = CHAIN.getNrOfJoints();
ns = CHAIN.getNrOfSegments();
jnt_mass_matrix.resize(nj);
previous_jnt_mass_matrix.resize(nj);
jnt_mass_matrix_dot.resize(nj);
initial_jnt_momentum.resize(nj);
estimated_momentum_integral.resize(nj);
filtered_estimated_ext_torque.resize(nj);
gravity_torque.resize(nj);
coriolis_torque.resize(nj);
total_torque.resize(nj);
estimated_ext_torque.resize(nj);
jacobian_end_eff.resize(nj);
jacobian_end_eff_transpose.conservativeResizeLike(MatrixXd::Zero(nj, 6));
jacobian_end_eff_transpose_inv.conservativeResizeLike(MatrixXd::Zero(6, nj));
U.conservativeResizeLike(MatrixXd::Zero(nj, 6));
V.conservativeResizeLike(MatrixXd::Zero(6, 6));
S.conservativeResizeLike(VectorXd::Zero(6));
S_inv.conservativeResizeLike(VectorXd::Zero(6));
tmp.conservativeResizeLike(VectorXd::Zero(6));
ESTIMATION_GAIN.conservativeResizeLike(Eigen::VectorXd::Constant(nj, ESTIMATION_GAIN(0)));
dynparam_solver.updateInternalDataStructures();
jacobian_solver.updateInternalDataStructures();
fk_pos_solver.updateInternalDataStructures();
}

// Calculates robot's initial momentum in the joint space. If this method is not called by the user, zero values will be taken for the initial momentum.
int ChainExternalWrenchEstimator::setInitialMomentum(const JntArray &joint_position, const JntArray &joint_velocity)
{
// Check sizes first
if (joint_position.rows() != nj || joint_velocity.rows() != nj)
return (error = E_SIZE_MISMATCH);

// Calculate robot's inertia and momentum in the joint space
if (E_NOERROR != dynparam_solver.JntToMass(joint_position, jnt_mass_matrix))
return (error = E_DYNPARAMSOLVERMASS_FAILED);

initial_jnt_momentum.data = jnt_mass_matrix.data * joint_velocity.data;

// Reset data because of the new momentum offset
SetToZero(estimated_momentum_integral);
SetToZero(filtered_estimated_ext_torque);

return (error = E_NOERROR);
}

// Sets singular-value eps parameter for the SVD calculation
void ChainExternalWrenchEstimator::setSVDEps(const double eps_in)
{
svd_eps = eps_in;
}

// Sets maximum iteration parameter for the SVD calculation
void ChainExternalWrenchEstimator::setSVDMaxIter(const int maxiter_in)
{
svd_maxiter = maxiter_in;
}

// This method calculates the external wrench that is applied on the robot's end-effector.
int ChainExternalWrenchEstimator::JntToExtWrench(const JntArray &joint_position, const JntArray &joint_velocity, const JntArray &joint_torque, Wrench &external_wrench)
{
/**
* ==========================================================================
* First-order 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.
* ==========================================================================
*/

// Check sizes first
if (nj != CHAIN.getNrOfJoints() || ns != CHAIN.getNrOfSegments())
return (error = E_NOT_UP_TO_DATE);
if (joint_position.rows() != nj || joint_velocity.rows() != nj || joint_torque.rows() != nj)
return (error = E_SIZE_MISMATCH);

/**
* ================================================================================================================
* Part I: Estimation of the torques felt in joints as a result of the external wrench being applied on the robot
* ================================================================================================================
*/

// Calculate decomposed robot's dynamics
if (E_NOERROR != dynparam_solver.JntToMass(joint_position, jnt_mass_matrix))
return (error = E_DYNPARAMSOLVERMASS_FAILED);

if (E_NOERROR != dynparam_solver.JntToCoriolis(joint_position, joint_velocity, coriolis_torque))
return (error = E_DYNPARAMSOLVERCORIOLIS_FAILED);

if (E_NOERROR != dynparam_solver.JntToGravity(joint_position, gravity_torque))
return (error = E_DYNPARAMSOLVERGRAVITY_FAILED);

// Calculate the change of robot's inertia in the joint space
jnt_mass_matrix_dot.data = (jnt_mass_matrix.data - previous_jnt_mass_matrix.data) / DT_SEC;

// Save data for the next iteration
previous_jnt_mass_matrix.data = jnt_mass_matrix.data;

// Calculate total torque exerted on the joint
total_torque.data = joint_torque.data - gravity_torque.data - coriolis_torque.data + jnt_mass_matrix_dot.data * joint_velocity.data;

// Accumulate main integral
estimated_momentum_integral.data += (total_torque.data + filtered_estimated_ext_torque.data) * DT_SEC;

// Estimate external joint torque
estimated_ext_torque.data = ESTIMATION_GAIN.asDiagonal() * (jnt_mass_matrix.data * joint_velocity.data - estimated_momentum_integral.data - initial_jnt_momentum.data);

// First order low-pass filter: filter out the noise from the estimated signal
// This filter can be turned off by setting FILTER_CONST value to 0
filtered_estimated_ext_torque.data = FILTER_CONST * filtered_estimated_ext_torque.data + (1.0 - FILTER_CONST) * estimated_ext_torque.data;

/**
* ==================================================================================================================
* Part II: Propagate above-estimated joint torques to Cartesian wrench using a pseudo-inverse of Jacobian-Transpose
* ==================================================================================================================
*/

// Compute robot's end-effector frame, expressed in the base frame
Frame end_eff_frame;
if (E_NOERROR != fk_pos_solver.JntToCart(joint_position, end_eff_frame))
return (error = E_FKSOLVERPOS_FAILED);

// Compute robot's jacobian for the end-effector frame, expressed in the base frame
if (E_NOERROR != jacobian_solver.JntToJac(joint_position, jacobian_end_eff))
return (error = E_JACSOLVER_FAILED);

// Transform the jacobian from the base frame to the end-effector frame.
// This part can be commented out if the user wants estimated wrench to be expressed w.r.t. base frame
jacobian_end_eff.changeBase(end_eff_frame.M.Inverse()); // Jacobian is now expressed w.r.t. end-effector frame

// SVD of "Jac^T" with maximum iterations "maxiter": Jac^T = U * S^-1 * V^T
jacobian_end_eff_transpose = jacobian_end_eff.data.transpose();
if (E_NOERROR != svd_eigen_HH(jacobian_end_eff_transpose, U, S, V, tmp, svd_maxiter))
return (error = E_SVD_FAILED);

// Invert singular values: S^-1
for (int i = 0; i < S.size(); ++i)
S_inv(i) = (std::fabs(S(i)) < svd_eps) ? 0.0 : 1.0 / S(i);

// Compose the inverse: (Jac^T)^-1 = V * S^-1 * U^T
jacobian_end_eff_transpose_inv = V * S_inv.asDiagonal() * U.adjoint();

// Compute end-effector's Cartesian wrench from the estimated joint torques: (Jac^T)^-1 * ext_tau
Vector6d estimated_wrench = jacobian_end_eff_transpose_inv * filtered_estimated_ext_torque.data;
for (int i = 0; i < 6; i++)
external_wrench(i) = estimated_wrench(i);

return (error = E_NOERROR);
}

// Getter for the torques felt in the robot's joints due to the external wrench being applied on the robot
void ChainExternalWrenchEstimator::getEstimatedJntTorque(JntArray &external_joint_torque)
{
assert(external_joint_torque.rows() == filtered_estimated_ext_torque.rows());
external_joint_torque = filtered_estimated_ext_torque;
}

const char* ChainExternalWrenchEstimator::strError(const int error) const
{
if (E_FKSOLVERPOS_FAILED == error) return "Internally-used Forward Position Kinematics (Recursive) solver failed";
else if (E_JACSOLVER_FAILED == error) return "Internally-used Jacobian solver failed";
else if (E_DYNPARAMSOLVERMASS_FAILED == error) return "Internally-used Dynamics Parameters (Mass) solver failed";
else if (E_DYNPARAMSOLVERCORIOLIS_FAILED == error) return "Internally-used Dynamics Parameters (Coriolis) solver failed";
else if (E_DYNPARAMSOLVERGRAVITY_FAILED == error) return "Internally-used Dynamics Parameters (Gravity) solver failed";
else return SolverI::strError(error);
}

} // namespace
128 changes: 128 additions & 0 deletions orocos_kdl/src/chainexternalwrenchestimator.hpp
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,128 @@
// Copyright (C) 2021 Djordje Vukcevic <djordje dot vukcevic at h-brs dot de>

// Version: 1.0
// Author: Djordje Vukcevic <djordje dot vukcevic at h-brs dot de>
// URL: http://www.orocos.org/kdl

// This library is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 2.1 of the License, or (at your option) any later version.

// This library is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// Lesser General Public License for more details.

// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA

#ifndef KDL_CHAIN_EXTERNAL_WRENCH_ESTIMATOR_HPP
#define KDL_CHAIN_EXTERNAL_WRENCH_ESTIMATOR_HPP

#include <Eigen/Core>
#include "utilities/svd_eigen_HH.hpp"
#include "chaindynparam.hpp"
#include "chainjnttojacsolver.hpp"
#include "chainfksolverpos_recursive.hpp"
#include <iostream>

namespace KDL {

/**
* \brief First-order momentum observer for the estimation of external wrenches applied on the robot's end-effector.
*
* 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.
*
* Note: This component assumes that the external wrench is applied on the end-effector (last) link of the robot's chain.
*/
class ChainExternalWrenchEstimator : public SolverI
{
typedef Eigen::Matrix<double, 6, 1 > Vector6d;

public:

static const int E_FKSOLVERPOS_FAILED = -100; //! Internally-used Forward Position Kinematics (Recursive) solver failed
static const int E_JACSOLVER_FAILED = -101; //! Internally-used Jacobian solver failed
static const int E_DYNPARAMSOLVERMASS_FAILED = -102; //! Internally-used Dynamics Parameters (Mass) solver failed
static const int E_DYNPARAMSOLVERCORIOLIS_FAILED = -103; //! Internally-used Dynamics Parameters (Coriolis) solver failed
static const int E_DYNPARAMSOLVERGRAVITY_FAILED = -104; //! Internally-used Dynamics Parameters (Gravity) solver failed

/**
* Constructor for the estimator, it will allocate all the necessary memory
* \param chain The kinematic chain of the robot, an internal copy will be made.
* \param gravity The gravity-acceleration vector to use during the calculation.
* \param sample_frequency Frequency at which users updates it estimation loop (in Hz).
* \param estimation_gain Parameter used to control the estimator's convergence
* \param filter_constant Parameter defining how much the estimated signal should be filtered by the low-pass filter.
* This input value should be between 0 and 1. Higher the number means more noise needs to be filtered-out.
* The filter can be turned off by setting this value to 0.
* \param eps If a SVD-singular value is below this value, its inverse is set to zero. Default: 0.00001
* \param maxiter Maximum iterations for the SVD computations. Default: 150.
*/
ChainExternalWrenchEstimator(const Chain &chain, const Vector &gravity, const double sample_frequency, const double estimation_gain, const double filter_constant, const double eps = 0.00001, const int maxiter = 150);
~ChainExternalWrenchEstimator(){};

/**
* Calculates robot's initial momentum in the joint space.
* Bassically, sets the offset for future estimation (momentum calculation).
* If this method is not called by the user, zero values will be taken for the initial momentum.
*/
int setInitialMomentum(const JntArray &joint_position, const JntArray &joint_velocity);

// Sets singular-value eps parameter for the SVD calculation
void setSVDEps(const double eps_in);

// Sets maximum iteration parameter for the SVD calculation
void setSVDMaxIter(const int maxiter_in);

/**
* This method calculates the external wrench that is applied on the robot's end-effector.
* Input parameters:
* \param joint_position The current (measured) joint positions.
* \param joint_velocity The current (measured) joint velocities.
* \param joint_torque The joint space torques.
* Depending on the user's choice, this array can represent commanded or measured joint torques.
* A particular choice depends on the available sensors in robot's joint.
* For more details see the above-referenced article.
*
* Output parameters:
* \param external_wrench The estimated external wrench applied on the robot's end-effector.
* The wrench will be expressed w.r.t. end-effector's frame.
*
* @return error/success code
*/
int JntToExtWrench(const JntArray &joint_position, const JntArray &joint_velocity, const JntArray &joint_torque, Wrench &external_wrench);

// Returns the torques felt in the robot's joints as a result of the external wrench being applied on the robot.
void getEstimatedJntTorque(JntArray &external_joint_torque);

/// @copydoc KDL::SolverI::updateInternalDataStructures()
virtual void updateInternalDataStructures();

/// @copydoc KDL::SolverI::strError()
virtual const char* strError(const int error) const;

private:
const Chain &CHAIN;
const double DT_SEC, FILTER_CONST;
double svd_eps;
int svd_maxiter;
unsigned int nj, ns;
JntSpaceInertiaMatrix jnt_mass_matrix, previous_jnt_mass_matrix, jnt_mass_matrix_dot;
JntArray initial_jnt_momentum, estimated_momentum_integral, filtered_estimated_ext_torque,
gravity_torque, coriolis_torque, total_torque, estimated_ext_torque;
Jacobian jacobian_end_eff;
Eigen::MatrixXd jacobian_end_eff_transpose, jacobian_end_eff_transpose_inv, U, V;
Eigen::VectorXd S, S_inv, tmp, ESTIMATION_GAIN;
ChainDynParam dynparam_solver;
ChainJntToJacSolver jacobian_solver;
ChainFkSolverPos_recursive fk_pos_solver;
};
}

#endif
Loading

0 comments on commit a2bbe91

Please sign in to comment.