Skip to content

Commit a2bbe91

Browse files
MatthijsBurghMatthijsBurgh
authored
External wrench estimator: first-order momentum observer (#311)
External wrench estimator: first-order momentum observer
2 parents 6ed7034 + 2bd28d8 commit a2bbe91

File tree

4 files changed

+622
-0
lines changed

4 files changed

+622
-0
lines changed

‎orocos_kdl/src/chainexternalwrenchestimator.cpp‎

Lines changed: 214 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,214 @@
1+
// Copyright (C) 2021 Djordje Vukcevic <djordje dot vukcevic at h-brs dot de>
2+
3+
// Version: 1.0
4+
// Author: Djordje Vukcevic <djordje dot vukcevic at h-brs dot de>
5+
// URL: http://www.orocos.org/kdl
6+
7+
// This library is free software; you can redistribute it and/or
8+
// modify it under the terms of the GNU Lesser General Public
9+
// License as published by the Free Software Foundation; either
10+
// version 2.1 of the License, or (at your option) any later version.
11+
12+
// This library is distributed in the hope that it will be useful,
13+
// but WITHOUT ANY WARRANTY; without even the implied warranty of
14+
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
15+
// Lesser General Public License for more details.
16+
17+
// You should have received a copy of the GNU Lesser General Public
18+
// License along with this library; if not, write to the Free Software
19+
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
20+
21+
#include "chainexternalwrenchestimator.hpp"
22+
23+
namespace KDL {
24+
25+
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) :
26+
CHAIN(chain),
27+
DT_SEC(1.0 / sample_frequency), FILTER_CONST(filter_constant),
28+
svd_eps(eps),
29+
svd_maxiter(maxiter),
30+
nj(CHAIN.getNrOfJoints()), ns(CHAIN.getNrOfSegments()),
31+
jnt_mass_matrix(nj), previous_jnt_mass_matrix(nj), jnt_mass_matrix_dot(nj),
32+
initial_jnt_momentum(nj), estimated_momentum_integral(nj), filtered_estimated_ext_torque(nj),
33+
gravity_torque(nj), coriolis_torque(nj), total_torque(nj), estimated_ext_torque(nj),
34+
jacobian_end_eff(nj),
35+
jacobian_end_eff_transpose(Eigen::MatrixXd::Zero(nj, 6)), jacobian_end_eff_transpose_inv(Eigen::MatrixXd::Zero(6, nj)),
36+
U(Eigen::MatrixXd::Zero(nj, 6)), V(Eigen::MatrixXd::Zero(6, 6)),
37+
S(Eigen::VectorXd::Zero(6)), S_inv(Eigen::VectorXd::Zero(6)), tmp(Eigen::VectorXd::Zero(6)),
38+
ESTIMATION_GAIN(Eigen::VectorXd::Constant(nj, estimation_gain)),
39+
dynparam_solver(CHAIN, gravity),
40+
jacobian_solver(CHAIN),
41+
fk_pos_solver(CHAIN)
42+
{
43+
}
44+
45+
void ChainExternalWrenchEstimator::updateInternalDataStructures()
46+
{
47+
nj = CHAIN.getNrOfJoints();
48+
ns = CHAIN.getNrOfSegments();
49+
jnt_mass_matrix.resize(nj);
50+
previous_jnt_mass_matrix.resize(nj);
51+
jnt_mass_matrix_dot.resize(nj);
52+
initial_jnt_momentum.resize(nj);
53+
estimated_momentum_integral.resize(nj);
54+
filtered_estimated_ext_torque.resize(nj);
55+
gravity_torque.resize(nj);
56+
coriolis_torque.resize(nj);
57+
total_torque.resize(nj);
58+
estimated_ext_torque.resize(nj);
59+
jacobian_end_eff.resize(nj);
60+
jacobian_end_eff_transpose.conservativeResizeLike(MatrixXd::Zero(nj, 6));
61+
jacobian_end_eff_transpose_inv.conservativeResizeLike(MatrixXd::Zero(6, nj));
62+
U.conservativeResizeLike(MatrixXd::Zero(nj, 6));
63+
V.conservativeResizeLike(MatrixXd::Zero(6, 6));
64+
S.conservativeResizeLike(VectorXd::Zero(6));
65+
S_inv.conservativeResizeLike(VectorXd::Zero(6));
66+
tmp.conservativeResizeLike(VectorXd::Zero(6));
67+
ESTIMATION_GAIN.conservativeResizeLike(Eigen::VectorXd::Constant(nj, ESTIMATION_GAIN(0)));
68+
dynparam_solver.updateInternalDataStructures();
69+
jacobian_solver.updateInternalDataStructures();
70+
fk_pos_solver.updateInternalDataStructures();
71+
}
72+
73+
// 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.
74+
int ChainExternalWrenchEstimator::setInitialMomentum(const JntArray &joint_position, const JntArray &joint_velocity)
75+
{
76+
// Check sizes first
77+
if (joint_position.rows() != nj || joint_velocity.rows() != nj)
78+
return (error = E_SIZE_MISMATCH);
79+
80+
// Calculate robot's inertia and momentum in the joint space
81+
if (E_NOERROR != dynparam_solver.JntToMass(joint_position, jnt_mass_matrix))
82+
return (error = E_DYNPARAMSOLVERMASS_FAILED);
83+
84+
initial_jnt_momentum.data = jnt_mass_matrix.data * joint_velocity.data;
85+
86+
// Reset data because of the new momentum offset
87+
SetToZero(estimated_momentum_integral);
88+
SetToZero(filtered_estimated_ext_torque);
89+
90+
return (error = E_NOERROR);
91+
}
92+
93+
// Sets singular-value eps parameter for the SVD calculation
94+
void ChainExternalWrenchEstimator::setSVDEps(const double eps_in)
95+
{
96+
svd_eps = eps_in;
97+
}
98+
99+
// Sets maximum iteration parameter for the SVD calculation
100+
void ChainExternalWrenchEstimator::setSVDMaxIter(const int maxiter_in)
101+
{
102+
svd_maxiter = maxiter_in;
103+
}
104+
105+
// This method calculates the external wrench that is applied on the robot's end-effector.
106+
int ChainExternalWrenchEstimator::JntToExtWrench(const JntArray &joint_position, const JntArray &joint_velocity, const JntArray &joint_torque, Wrench &external_wrench)
107+
{
108+
/**
109+
* ==========================================================================
110+
* First-order momentum observer, an implementation based on:
111+
* S. Haddadin, A. De Luca and A. Albu-Schäffer,
112+
* "Robot Collisions: A Survey on Detection, Isolation, and Identification,"
113+
* in IEEE Transactions on Robotics, vol. 33(6), pp. 1292-1312, 2017.
114+
* ==========================================================================
115+
*/
116+
117+
// Check sizes first
118+
if (nj != CHAIN.getNrOfJoints() || ns != CHAIN.getNrOfSegments())
119+
return (error = E_NOT_UP_TO_DATE);
120+
if (joint_position.rows() != nj || joint_velocity.rows() != nj || joint_torque.rows() != nj)
121+
return (error = E_SIZE_MISMATCH);
122+
123+
/**
124+
* ================================================================================================================
125+
* Part I: Estimation of the torques felt in joints as a result of the external wrench being applied on the robot
126+
* ================================================================================================================
127+
*/
128+
129+
// Calculate decomposed robot's dynamics
130+
if (E_NOERROR != dynparam_solver.JntToMass(joint_position, jnt_mass_matrix))
131+
return (error = E_DYNPARAMSOLVERMASS_FAILED);
132+
133+
if (E_NOERROR != dynparam_solver.JntToCoriolis(joint_position, joint_velocity, coriolis_torque))
134+
return (error = E_DYNPARAMSOLVERCORIOLIS_FAILED);
135+
136+
if (E_NOERROR != dynparam_solver.JntToGravity(joint_position, gravity_torque))
137+
return (error = E_DYNPARAMSOLVERGRAVITY_FAILED);
138+
139+
// Calculate the change of robot's inertia in the joint space
140+
jnt_mass_matrix_dot.data = (jnt_mass_matrix.data - previous_jnt_mass_matrix.data) / DT_SEC;
141+
142+
// Save data for the next iteration
143+
previous_jnt_mass_matrix.data = jnt_mass_matrix.data;
144+
145+
// Calculate total torque exerted on the joint
146+
total_torque.data = joint_torque.data - gravity_torque.data - coriolis_torque.data + jnt_mass_matrix_dot.data * joint_velocity.data;
147+
148+
// Accumulate main integral
149+
estimated_momentum_integral.data += (total_torque.data + filtered_estimated_ext_torque.data) * DT_SEC;
150+
151+
// Estimate external joint torque
152+
estimated_ext_torque.data = ESTIMATION_GAIN.asDiagonal() * (jnt_mass_matrix.data * joint_velocity.data - estimated_momentum_integral.data - initial_jnt_momentum.data);
153+
154+
// First order low-pass filter: filter out the noise from the estimated signal
155+
// This filter can be turned off by setting FILTER_CONST value to 0
156+
filtered_estimated_ext_torque.data = FILTER_CONST * filtered_estimated_ext_torque.data + (1.0 - FILTER_CONST) * estimated_ext_torque.data;
157+
158+
/**
159+
* ==================================================================================================================
160+
* Part II: Propagate above-estimated joint torques to Cartesian wrench using a pseudo-inverse of Jacobian-Transpose
161+
* ==================================================================================================================
162+
*/
163+
164+
// Compute robot's end-effector frame, expressed in the base frame
165+
Frame end_eff_frame;
166+
if (E_NOERROR != fk_pos_solver.JntToCart(joint_position, end_eff_frame))
167+
return (error = E_FKSOLVERPOS_FAILED);
168+
169+
// Compute robot's jacobian for the end-effector frame, expressed in the base frame
170+
if (E_NOERROR != jacobian_solver.JntToJac(joint_position, jacobian_end_eff))
171+
return (error = E_JACSOLVER_FAILED);
172+
173+
// Transform the jacobian from the base frame to the end-effector frame.
174+
// This part can be commented out if the user wants estimated wrench to be expressed w.r.t. base frame
175+
jacobian_end_eff.changeBase(end_eff_frame.M.Inverse()); // Jacobian is now expressed w.r.t. end-effector frame
176+
177+
// SVD of "Jac^T" with maximum iterations "maxiter": Jac^T = U * S^-1 * V^T
178+
jacobian_end_eff_transpose = jacobian_end_eff.data.transpose();
179+
if (E_NOERROR != svd_eigen_HH(jacobian_end_eff_transpose, U, S, V, tmp, svd_maxiter))
180+
return (error = E_SVD_FAILED);
181+
182+
// Invert singular values: S^-1
183+
for (int i = 0; i < S.size(); ++i)
184+
S_inv(i) = (std::fabs(S(i)) < svd_eps) ? 0.0 : 1.0 / S(i);
185+
186+
// Compose the inverse: (Jac^T)^-1 = V * S^-1 * U^T
187+
jacobian_end_eff_transpose_inv = V * S_inv.asDiagonal() * U.adjoint();
188+
189+
// Compute end-effector's Cartesian wrench from the estimated joint torques: (Jac^T)^-1 * ext_tau
190+
Vector6d estimated_wrench = jacobian_end_eff_transpose_inv * filtered_estimated_ext_torque.data;
191+
for (int i = 0; i < 6; i++)
192+
external_wrench(i) = estimated_wrench(i);
193+
194+
return (error = E_NOERROR);
195+
}
196+
197+
// Getter for the torques felt in the robot's joints due to the external wrench being applied on the robot
198+
void ChainExternalWrenchEstimator::getEstimatedJntTorque(JntArray &external_joint_torque)
199+
{
200+
assert(external_joint_torque.rows() == filtered_estimated_ext_torque.rows());
201+
external_joint_torque = filtered_estimated_ext_torque;
202+
}
203+
204+
const char* ChainExternalWrenchEstimator::strError(const int error) const
205+
{
206+
if (E_FKSOLVERPOS_FAILED == error) return "Internally-used Forward Position Kinematics (Recursive) solver failed";
207+
else if (E_JACSOLVER_FAILED == error) return "Internally-used Jacobian solver failed";
208+
else if (E_DYNPARAMSOLVERMASS_FAILED == error) return "Internally-used Dynamics Parameters (Mass) solver failed";
209+
else if (E_DYNPARAMSOLVERCORIOLIS_FAILED == error) return "Internally-used Dynamics Parameters (Coriolis) solver failed";
210+
else if (E_DYNPARAMSOLVERGRAVITY_FAILED == error) return "Internally-used Dynamics Parameters (Gravity) solver failed";
211+
else return SolverI::strError(error);
212+
}
213+
214+
} // namespace

‎orocos_kdl/src/chainexternalwrenchestimator.hpp‎

Lines changed: 128 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,128 @@
1+
// Copyright (C) 2021 Djordje Vukcevic <djordje dot vukcevic at h-brs dot de>
2+
3+
// Version: 1.0
4+
// Author: Djordje Vukcevic <djordje dot vukcevic at h-brs dot de>
5+
// URL: http://www.orocos.org/kdl
6+
7+
// This library is free software; you can redistribute it and/or
8+
// modify it under the terms of the GNU Lesser General Public
9+
// License as published by the Free Software Foundation; either
10+
// version 2.1 of the License, or (at your option) any later version.
11+
12+
// This library is distributed in the hope that it will be useful,
13+
// but WITHOUT ANY WARRANTY; without even the implied warranty of
14+
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
15+
// Lesser General Public License for more details.
16+
17+
// You should have received a copy of the GNU Lesser General Public
18+
// License along with this library; if not, write to the Free Software
19+
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
20+
21+
#ifndef KDL_CHAIN_EXTERNAL_WRENCH_ESTIMATOR_HPP
22+
#define KDL_CHAIN_EXTERNAL_WRENCH_ESTIMATOR_HPP
23+
24+
#include <Eigen/Core>
25+
#include "utilities/svd_eigen_HH.hpp"
26+
#include "chaindynparam.hpp"
27+
#include "chainjnttojacsolver.hpp"
28+
#include "chainfksolverpos_recursive.hpp"
29+
#include <iostream>
30+
31+
namespace KDL {
32+
33+
/**
34+
* \brief First-order momentum observer for the estimation of external wrenches applied on the robot's end-effector.
35+
*
36+
* Implementation based on:
37+
* S. Haddadin, A. De Luca and A. Albu-Schäffer,
38+
* "Robot Collisions: A Survey on Detection, Isolation, and Identification,"
39+
* in IEEE Transactions on Robotics, vol. 33(6), pp. 1292-1312, 2017.
40+
*
41+
* Note: This component assumes that the external wrench is applied on the end-effector (last) link of the robot's chain.
42+
*/
43+
class ChainExternalWrenchEstimator : public SolverI
44+
{
45+
typedef Eigen::Matrix<double, 6, 1 > Vector6d;
46+
47+
public:
48+
49+
static const int E_FKSOLVERPOS_FAILED = -100; //! Internally-used Forward Position Kinematics (Recursive) solver failed
50+
static const int E_JACSOLVER_FAILED = -101; //! Internally-used Jacobian solver failed
51+
static const int E_DYNPARAMSOLVERMASS_FAILED = -102; //! Internally-used Dynamics Parameters (Mass) solver failed
52+
static const int E_DYNPARAMSOLVERCORIOLIS_FAILED = -103; //! Internally-used Dynamics Parameters (Coriolis) solver failed
53+
static const int E_DYNPARAMSOLVERGRAVITY_FAILED = -104; //! Internally-used Dynamics Parameters (Gravity) solver failed
54+
55+
/**
56+
* Constructor for the estimator, it will allocate all the necessary memory
57+
* \param chain The kinematic chain of the robot, an internal copy will be made.
58+
* \param gravity The gravity-acceleration vector to use during the calculation.
59+
* \param sample_frequency Frequency at which users updates it estimation loop (in Hz).
60+
* \param estimation_gain Parameter used to control the estimator's convergence
61+
* \param filter_constant Parameter defining how much the estimated signal should be filtered by the low-pass filter.
62+
* This input value should be between 0 and 1. Higher the number means more noise needs to be filtered-out.
63+
* The filter can be turned off by setting this value to 0.
64+
* \param eps If a SVD-singular value is below this value, its inverse is set to zero. Default: 0.00001
65+
* \param maxiter Maximum iterations for the SVD computations. Default: 150.
66+
*/
67+
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);
68+
~ChainExternalWrenchEstimator(){};
69+
70+
/**
71+
* Calculates robot's initial momentum in the joint space.
72+
* Bassically, sets the offset for future estimation (momentum calculation).
73+
* If this method is not called by the user, zero values will be taken for the initial momentum.
74+
*/
75+
int setInitialMomentum(const JntArray &joint_position, const JntArray &joint_velocity);
76+
77+
// Sets singular-value eps parameter for the SVD calculation
78+
void setSVDEps(const double eps_in);
79+
80+
// Sets maximum iteration parameter for the SVD calculation
81+
void setSVDMaxIter(const int maxiter_in);
82+
83+
/**
84+
* This method calculates the external wrench that is applied on the robot's end-effector.
85+
* Input parameters:
86+
* \param joint_position The current (measured) joint positions.
87+
* \param joint_velocity The current (measured) joint velocities.
88+
* \param joint_torque The joint space torques.
89+
* Depending on the user's choice, this array can represent commanded or measured joint torques.
90+
* A particular choice depends on the available sensors in robot's joint.
91+
* For more details see the above-referenced article.
92+
*
93+
* Output parameters:
94+
* \param external_wrench The estimated external wrench applied on the robot's end-effector.
95+
* The wrench will be expressed w.r.t. end-effector's frame.
96+
*
97+
* @return error/success code
98+
*/
99+
int JntToExtWrench(const JntArray &joint_position, const JntArray &joint_velocity, const JntArray &joint_torque, Wrench &external_wrench);
100+
101+
// Returns the torques felt in the robot's joints as a result of the external wrench being applied on the robot.
102+
void getEstimatedJntTorque(JntArray &external_joint_torque);
103+
104+
/// @copydoc KDL::SolverI::updateInternalDataStructures()
105+
virtual void updateInternalDataStructures();
106+
107+
/// @copydoc KDL::SolverI::strError()
108+
virtual const char* strError(const int error) const;
109+
110+
private:
111+
const Chain &CHAIN;
112+
const double DT_SEC, FILTER_CONST;
113+
double svd_eps;
114+
int svd_maxiter;
115+
unsigned int nj, ns;
116+
JntSpaceInertiaMatrix jnt_mass_matrix, previous_jnt_mass_matrix, jnt_mass_matrix_dot;
117+
JntArray initial_jnt_momentum, estimated_momentum_integral, filtered_estimated_ext_torque,
118+
gravity_torque, coriolis_torque, total_torque, estimated_ext_torque;
119+
Jacobian jacobian_end_eff;
120+
Eigen::MatrixXd jacobian_end_eff_transpose, jacobian_end_eff_transpose_inv, U, V;
121+
Eigen::VectorXd S, S_inv, tmp, ESTIMATION_GAIN;
122+
ChainDynParam dynparam_solver;
123+
ChainJntToJacSolver jacobian_solver;
124+
ChainFkSolverPos_recursive fk_pos_solver;
125+
};
126+
}
127+
128+
#endif

0 commit comments

Comments
 (0)