KDL 1.5.1
|
First-order momentum observer for the estimation of external wrenches applied on the robot's end-effector. More...
#include <src/chainexternalwrenchestimator.hpp>
Public Types | |
enum | { E_DEGRADED = +1 , E_NOERROR = 0 , E_NO_CONVERGE = -1 , E_UNDEFINED = -2 , E_NOT_UP_TO_DATE = -3 , E_SIZE_MISMATCH = -4 , E_MAX_ITERATIONS_EXCEEDED = -5 , E_OUT_OF_RANGE = -6 , E_NOT_IMPLEMENTED = -7 , E_SVD_FAILED = -8 } |
Public Member Functions | |
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) | |
Internally-used Dynamics Parameters (Gravity) solver failed. | |
~ChainExternalWrenchEstimator () | |
int | setInitialMomentum (const JntArray &joint_position, const JntArray &joint_velocity) |
Calculates robot's initial momentum in the joint space. | |
void | setSVDEps (const double eps_in) |
void | setSVDMaxIter (const int maxiter_in) |
int | JntToExtWrench (const JntArray &joint_position, const JntArray &joint_velocity, const JntArray &joint_torque, Wrench &external_wrench) |
This method calculates the external wrench that is applied on the robot's end-effector. | |
void | getEstimatedJntTorque (JntArray &external_joint_torque) |
virtual void | updateInternalDataStructures () |
Update the internal data structures. | |
virtual const char * | strError (const int error) const |
Return a description of the latest error. | |
virtual int | getError () const |
Return the latest error. | |
Static Public Attributes | |
static const int | E_FKSOLVERPOS_FAILED = -100 |
static const int | E_JACSOLVER_FAILED = -101 |
Internally-used Forward Position Kinematics (Recursive) solver failed. | |
static const int | E_DYNPARAMSOLVERMASS_FAILED = -102 |
Internally-used Jacobian solver failed. | |
static const int | E_DYNPARAMSOLVERCORIOLIS_FAILED = -103 |
Internally-used Dynamics Parameters (Mass) solver failed. | |
static const int | E_DYNPARAMSOLVERGRAVITY_FAILED = -104 |
Internally-used Dynamics Parameters (Coriolis) solver failed. | |
Protected Attributes | |
int | error |
Latest error, initialized to E_NOERROR in constructor. | |
Private Types | |
typedef Eigen::Matrix< double, 6, 1 > | Vector6d |
Private Attributes | |
const Chain & | CHAIN |
const double | DT_SEC |
const double | FILTER_CONST |
double | svd_eps |
int | svd_maxiter |
unsigned int | nj |
unsigned int | ns |
JntSpaceInertiaMatrix | jnt_mass_matrix |
JntSpaceInertiaMatrix | previous_jnt_mass_matrix |
JntSpaceInertiaMatrix | jnt_mass_matrix_dot |
JntArray | initial_jnt_momentum |
JntArray | estimated_momentum_integral |
JntArray | filtered_estimated_ext_torque |
JntArray | gravity_torque |
JntArray | coriolis_torque |
JntArray | total_torque |
JntArray | estimated_ext_torque |
Jacobian | jacobian_end_eff |
Eigen::MatrixXd | jacobian_end_eff_transpose |
Eigen::MatrixXd | jacobian_end_eff_transpose_inv |
Eigen::MatrixXd | U |
Eigen::MatrixXd | V |
Eigen::VectorXd | S |
Eigen::VectorXd | S_inv |
Eigen::VectorXd | tmp |
Eigen::VectorXd | ESTIMATION_GAIN |
ChainDynParam | dynparam_solver |
ChainJntToJacSolver | jacobian_solver |
ChainFkSolverPos_recursive | fk_pos_solver |
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.
|
private |
|
inherited |
Enumerator | |
---|---|
E_DEGRADED | Converged but degraded solution (e.g. WDLS with psuedo-inverse singular) |
E_NOERROR | No error. |
E_NO_CONVERGE | Failed to converge. |
E_UNDEFINED | Undefined value (e.g. computed a NAN, or tan(90 degrees) ) |
E_NOT_UP_TO_DATE | Chain size changed. |
E_SIZE_MISMATCH | Input size does not match internal state. |
E_MAX_ITERATIONS_EXCEEDED | Maximum number of iterations exceeded. |
E_OUT_OF_RANGE | Requested index out of range. |
E_NOT_IMPLEMENTED | Not yet implemented. |
E_SVD_FAILED | Internal svd calculation failed. |
KDL::ChainExternalWrenchEstimator::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 ) |
Internally-used Dynamics Parameters (Gravity) solver failed.
Constructor for the estimator, it will allocate all the necessary memory
chain | The kinematic chain of the robot, an internal copy will be made. |
gravity | The gravity-acceleration vector to use during the calculation. |
sample_frequency | Frequency at which users updates it estimation loop (in Hz). |
estimation_gain | Parameter used to control the estimator's convergence |
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. |
eps | If a SVD-singular value is below this value, its inverse is set to zero. Default: 0.00001 |
maxiter | Maximum iterations for the SVD computations. Default: 150. |
|
inline |
|
inlinevirtualinherited |
Return the latest error.
References KDL::SolverI::error.
void KDL::ChainExternalWrenchEstimator::getEstimatedJntTorque | ( | JntArray & | external_joint_torque | ) |
References filtered_estimated_ext_torque, and KDL::JntArray::rows().
int KDL::ChainExternalWrenchEstimator::JntToExtWrench | ( | const JntArray & | joint_position, |
const JntArray & | joint_velocity, | ||
const JntArray & | joint_torque, | ||
Wrench & | external_wrench ) |
This method calculates the external wrench that is applied on the robot's end-effector.
Input parameters:
joint_position | The current (measured) joint positions. |
joint_velocity | The current (measured) joint velocities. |
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:
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. |
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,"
References CHAIN, KDL::Jacobian::changeBase, coriolis_torque, KDL::Jacobian::data, KDL::JntArray::data, DT_SEC, dynparam_solver, E_DYNPARAMSOLVERCORIOLIS_FAILED, E_DYNPARAMSOLVERGRAVITY_FAILED, E_DYNPARAMSOLVERMASS_FAILED, E_FKSOLVERPOS_FAILED, E_JACSOLVER_FAILED, KDL::SolverI::E_NOERROR, KDL::SolverI::E_NOT_UP_TO_DATE, KDL::SolverI::E_SIZE_MISMATCH, KDL::SolverI::E_SVD_FAILED, KDL::SolverI::error, estimated_ext_torque, estimated_momentum_integral, ESTIMATION_GAIN, FILTER_CONST, filtered_estimated_ext_torque, fk_pos_solver, KDL::Chain::getNrOfJoints(), KDL::Chain::getNrOfSegments(), gravity_torque, initial_jnt_momentum, KDL::Rotation::Inverse(), jacobian_end_eff, jacobian_end_eff_transpose, jacobian_end_eff_transpose_inv, jacobian_solver, jnt_mass_matrix, jnt_mass_matrix_dot, KDL::ChainFkSolverPos_recursive::JntToCart(), KDL::ChainDynParam::JntToCoriolis(), KDL::ChainDynParam::JntToGravity(), KDL::ChainJntToJacSolver::JntToJac(), KDL::ChainDynParam::JntToMass(), KDL::Frame::M, nj, ns, previous_jnt_mass_matrix, KDL::JntArray::rows(), S, S_inv, svd_eps, svd_maxiter, tmp, total_torque, U, and V.
int KDL::ChainExternalWrenchEstimator::setInitialMomentum | ( | const JntArray & | joint_position, |
const JntArray & | joint_velocity ) |
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.
References KDL::JntArray::data, dynparam_solver, E_DYNPARAMSOLVERMASS_FAILED, KDL::SolverI::E_NOERROR, KDL::SolverI::E_SIZE_MISMATCH, KDL::SolverI::error, estimated_momentum_integral, filtered_estimated_ext_torque, initial_jnt_momentum, jnt_mass_matrix, KDL::ChainDynParam::JntToMass(), nj, KDL::JntArray::rows(), and KDL::SetToZero().
void KDL::ChainExternalWrenchEstimator::setSVDEps | ( | const double | eps_in | ) |
References svd_eps.
void KDL::ChainExternalWrenchEstimator::setSVDMaxIter | ( | const int | maxiter_in | ) |
References svd_maxiter.
|
virtual |
Return a description of the latest error.
Reimplemented from KDL::SolverI.
References E_DYNPARAMSOLVERCORIOLIS_FAILED, E_DYNPARAMSOLVERGRAVITY_FAILED, E_DYNPARAMSOLVERMASS_FAILED, E_FKSOLVERPOS_FAILED, E_JACSOLVER_FAILED, KDL::SolverI::error, and KDL::SolverI::strError().
|
virtual |
Update the internal data structures.
This is required if the number of segments or number of joints of a chain/tree have changed. This provides a single point of contact for solver memory allocations.
Implements KDL::SolverI.
References CHAIN, coriolis_torque, dynparam_solver, estimated_ext_torque, estimated_momentum_integral, ESTIMATION_GAIN, filtered_estimated_ext_torque, fk_pos_solver, KDL::Chain::getNrOfJoints(), KDL::Chain::getNrOfSegments(), gravity_torque, initial_jnt_momentum, jacobian_end_eff, jacobian_end_eff_transpose, jacobian_end_eff_transpose_inv, jacobian_solver, jnt_mass_matrix, jnt_mass_matrix_dot, nj, ns, previous_jnt_mass_matrix, KDL::Jacobian::resize(), KDL::JntArray::resize(), S, S_inv, tmp, total_torque, U, KDL::ChainDynParam::updateInternalDataStructures(), KDL::ChainFkSolverPos_recursive::updateInternalDataStructures(), KDL::ChainJntToJacSolver::updateInternalDataStructures(), and V.
|
private |
Referenced by JntToExtWrench(), and updateInternalDataStructures().
|
private |
Referenced by JntToExtWrench(), and updateInternalDataStructures().
|
private |
Referenced by JntToExtWrench().
|
private |
Referenced by JntToExtWrench(), setInitialMomentum(), and updateInternalDataStructures().
|
static |
Internally-used Dynamics Parameters (Mass) solver failed.
Referenced by JntToExtWrench(), and strError().
|
static |
Internally-used Dynamics Parameters (Coriolis) solver failed.
Referenced by JntToExtWrench(), and strError().
|
static |
Internally-used Jacobian solver failed.
Referenced by JntToExtWrench(), setInitialMomentum(), and strError().
|
static |
Referenced by JntToExtWrench(), and strError().
|
static |
Internally-used Forward Position Kinematics (Recursive) solver failed.
Referenced by JntToExtWrench(), and strError().
|
protectedinherited |
Latest error, initialized to E_NOERROR in constructor.
Referenced by KDL::ChainFdSolver_RNE::CartToJnt(), KDL::ChainHdSolver_Vereshchagin::CartToJnt(), KDL::ChainIdSolver_RNE::CartToJnt(), KDL::ChainIkSolverPos_LMA::CartToJnt(), KDL::ChainIkSolverPos_NR::CartToJnt(), KDL::ChainIkSolverPos_NR_JL::CartToJnt(), KDL::ChainIkSolverVel_pinv::CartToJnt(), KDL::ChainIkSolverVel_pinv_givens::CartToJnt(), KDL::ChainIkSolverVel_pinv_givens::CartToJnt(), KDL::ChainIkSolverVel_pinv_nso::CartToJnt(), KDL::ChainIkSolverVel_wdls::CartToJnt(), KDL::TreeIdSolver_RNE::CartToJnt(), KDL::SolverI::getError(), KDL::ChainIkSolverVel_wdls::getSigma(), KDL::ChainFkSolverPos_recursive::JntToCart(), KDL::ChainFkSolverVel_recursive::JntToCart(), JntToExtWrench(), KDL::ChainJntToJacSolver::JntToJac(), KDL::ChainJntToJacDotSolver::JntToJacDot(), KDL::ChainJntToJacDotSolver::JntToJacDot(), KDL::ChainDynParam::JntToMass(), setInitialMomentum(), KDL::ChainIkSolverPos_NR_JL::setJointLimits(), KDL::ChainJntToJacDotSolver::setLockedJoints(), KDL::ChainJntToJacSolver::setLockedJoints(), KDL::ChainIkSolverVel_pinv_nso::setOptPos(), KDL::ChainIkSolverVel_wdls::setWeightJS(), KDL::ChainIkSolverVel_pinv_nso::setWeights(), KDL::ChainIkSolverVel_wdls::setWeightTS(), strError(), KDL::ChainIkSolverPos_LMA::strError(), KDL::ChainIkSolverPos_NR::strError(), KDL::ChainIkSolverPos_NR_JL::strError(), KDL::ChainIkSolverVel_pinv::strError(), KDL::ChainIkSolverVel_wdls::strError(), KDL::ChainJntToJacDotSolver::strError(), and KDL::SolverI::strError().
|
private |
Referenced by JntToExtWrench(), and updateInternalDataStructures().
|
private |
Referenced by JntToExtWrench(), setInitialMomentum(), and updateInternalDataStructures().
|
private |
Referenced by JntToExtWrench(), and updateInternalDataStructures().
|
private |
Referenced by JntToExtWrench().
|
private |
Referenced by getEstimatedJntTorque(), JntToExtWrench(), setInitialMomentum(), and updateInternalDataStructures().
|
private |
Referenced by JntToExtWrench(), and updateInternalDataStructures().
|
private |
Referenced by JntToExtWrench(), and updateInternalDataStructures().
|
private |
Referenced by JntToExtWrench(), setInitialMomentum(), and updateInternalDataStructures().
|
private |
Referenced by JntToExtWrench(), and updateInternalDataStructures().
|
private |
Referenced by JntToExtWrench(), and updateInternalDataStructures().
|
private |
Referenced by JntToExtWrench(), and updateInternalDataStructures().
|
private |
Referenced by JntToExtWrench(), and updateInternalDataStructures().
|
private |
Referenced by JntToExtWrench(), setInitialMomentum(), and updateInternalDataStructures().
|
private |
Referenced by JntToExtWrench(), and updateInternalDataStructures().
|
private |
Referenced by JntToExtWrench(), setInitialMomentum(), and updateInternalDataStructures().
|
private |
Referenced by JntToExtWrench(), and updateInternalDataStructures().
|
private |
Referenced by JntToExtWrench(), and updateInternalDataStructures().
|
private |
Referenced by JntToExtWrench(), and updateInternalDataStructures().
|
private |
Referenced by JntToExtWrench(), and updateInternalDataStructures().
|
private |
Referenced by JntToExtWrench(), and setSVDEps().
|
private |
Referenced by JntToExtWrench(), and setSVDMaxIter().
|
private |
Referenced by JntToExtWrench(), and updateInternalDataStructures().
|
private |
Referenced by JntToExtWrench(), and updateInternalDataStructures().
|
private |
Referenced by JntToExtWrench(), and updateInternalDataStructures().
|
private |
Referenced by JntToExtWrench(), and updateInternalDataStructures().