KDL 1.5.1
|
#include <src/chainidsolver_vereshchagin.hpp>
Public Member Functions | |
ChainIdSolver_Vereshchagin (const Chain &chain, const Twist &root_acc, const unsigned int nc) | |
int | CartToJnt (const JntArray &q, const JntArray &q_dot, JntArray &q_dotdot, const Jacobian &alfa, const JntArray &beta, const Wrenches &f_ext, const JntArray &ff_torques, JntArray &constraint_torques) |
This method calculates joint space constraint torques and accelerations. More... | |
virtual void | updateInternalDataStructures () |
Update the internal data structures. More... | |
void | getTransformedLinkAcceleration (Twists &x_dotdot) |
void | getTotalTorque (JntArray &total_tau) |
void | getContraintForceMagnitude (Eigen::VectorXd &nu_) |
Private Types | |
typedef std::vector< Twist > | Twists |
typedef std::vector< Frame > | Frames |
typedef Eigen::Matrix< double, 6, 1 > | Vector6d |
typedef Eigen::Matrix< double, 6, 6 > | Matrix6d |
typedef Eigen::Matrix< double, 6, Eigen::Dynamic > | Matrix6Xd |
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 } |
Private Member Functions | |
void | initial_upwards_sweep (const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const Wrenches &f_ext) |
This method calculates all cartesian space poses, twists, bias accelerations. More... | |
void | downwards_sweep (const Jacobian &alfa, const JntArray &ff_torques) |
This method is a force balance sweep. More... | |
void | constraint_calculation (const JntArray &beta) |
This method calculates constraint force magnitudes. More... | |
void | final_upwards_sweep (JntArray &q_dotdot, JntArray &constraint_torques) |
This method puts all acceleration contributions (constraint, bias, nullspace and parent accelerations) together. More... | |
virtual int | getError () const |
Return the latest error. More... | |
virtual const char * | strError (const int error) const |
Return a description of the latest error. More... | |
Private Attributes | |
const Chain & | chain |
unsigned int | nj |
unsigned int | ns |
unsigned int | nc |
Twist | acc_root |
Jacobian | alfa_N |
Jacobian | alfa_N2 |
Eigen::MatrixXd | M_0_inverse |
Eigen::MatrixXd | Um |
Eigen::MatrixXd | Vm |
JntArray | beta_N |
Eigen::VectorXd | nu |
Eigen::VectorXd | nu_sum |
Eigen::VectorXd | Sm |
Eigen::VectorXd | tmpm |
Eigen::VectorXd | total_torques |
Wrench | qdotdot_sum |
Frame | F_total |
std::vector< segment_info, Eigen::aligned_allocator< segment_info > > | results |
int | error |
Latest error, initialized to E_NOERROR in constructor. More... | |
|
privateinherited |
|
privateinherited |
|
privateinherited |
|
privateinherited |
|
privateinherited |
|
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::ChainIdSolver_Vereshchagin::ChainIdSolver_Vereshchagin | ( | const Chain & | chain, |
const Twist & | root_acc, | ||
const unsigned int | nc | ||
) |
|
inherited |
This method calculates joint space constraint torques and accelerations.
It returns 0 when it succeeds, otherwise -1 or -2 for nonmatching matrix and array sizes. Input parameters:
q | The current joint positions |
q_dot | The current joint velocities |
alpha | The active constraint directions (unit constraint forces expressed w.r.t. robot's base frame) |
beta | The acceleration energy setpoints (expressed w.r.t. above-defined unit constraint forces) |
f_ext | The external forces (no gravity, it is given in root acceleration) on the segments |
ff_torques | The feed-forward joint space torques |
Output parameters:
q_dotdot | The resulting joint accelerations |
constraint_torques | The resulting joint constraint torques (what each joint feels due to the constraint forces acting on the end-effector) |
References KDL::ChainHdSolver_Vereshchagin::chain, KDL::Jacobian::columns(), KDL::ChainHdSolver_Vereshchagin::constraint_calculation(), KDL::ChainHdSolver_Vereshchagin::downwards_sweep(), KDL::SolverI::E_NOERROR, KDL::SolverI::E_NOT_UP_TO_DATE, KDL::SolverI::E_SIZE_MISMATCH, KDL::SolverI::error, KDL::ChainHdSolver_Vereshchagin::final_upwards_sweep(), KDL::Chain::getNrOfJoints(), KDL::Chain::getNrOfSegments(), KDL::ChainHdSolver_Vereshchagin::initial_upwards_sweep(), KDL::ChainHdSolver_Vereshchagin::nc, KDL::ChainHdSolver_Vereshchagin::nj, KDL::ChainHdSolver_Vereshchagin::ns, and KDL::JntArray::rows().
|
privateinherited |
This method calculates constraint force magnitudes.
References KDL::ChainHdSolver_Vereshchagin::acc_root, KDL::Vector::data, KDL::JntArray::data, KDL::ChainHdSolver_Vereshchagin::M_0_inverse, KDL::ChainHdSolver_Vereshchagin::nc, KDL::ChainHdSolver_Vereshchagin::nu, KDL::ChainHdSolver_Vereshchagin::nu_sum, KDL::ChainHdSolver_Vereshchagin::results, KDL::Twist::rot, KDL::ChainHdSolver_Vereshchagin::Sm, KDL::ChainHdSolver_Vereshchagin::tmpm, KDL::ChainHdSolver_Vereshchagin::Um, KDL::Twist::vel, and KDL::ChainHdSolver_Vereshchagin::Vm.
Referenced by KDL::ChainHdSolver_Vereshchagin::CartToJnt().
|
privateinherited |
This method is a force balance sweep.
It calculates articulated body inertias and bias forces. Additionally, acceleration energies generated by bias forces and unit forces are calculated here.
Additionally adding joint inertia to s.D, see:
References KDL::ChainHdSolver_Vereshchagin::segment_info::C, KDL::ChainHdSolver_Vereshchagin::chain, KDL::ChainHdSolver_Vereshchagin::segment_info::D, KDL::Vector::data, dot(), KDL::ChainHdSolver_Vereshchagin::segment_info::E, KDL::ChainHdSolver_Vereshchagin::segment_info::E_tilde, KDL::ChainHdSolver_Vereshchagin::segment_info::EZ, KDL::ChainHdSolver_Vereshchagin::segment_info::F, KDL::ChainHdSolver_Vereshchagin::F_total, KDL::Joint::Fixed, KDL::Wrench::force, KDL::ChainHdSolver_Vereshchagin::segment_info::G, KDL::Joint::getInertia(), KDL::Segment::getJoint(), KDL::Chain::getSegment(), KDL::Joint::getType(), KDL::ChainHdSolver_Vereshchagin::segment_info::H, KDL::Rotation::Inverse(), KDL::ChainHdSolver_Vereshchagin::segment_info::M, KDL::Frame::M, KDL::ChainHdSolver_Vereshchagin::nc, KDL::ChainHdSolver_Vereshchagin::nj, KDL::ChainHdSolver_Vereshchagin::ns, KDL::ChainHdSolver_Vereshchagin::segment_info::P, KDL::ChainHdSolver_Vereshchagin::segment_info::P_tilde, KDL::ChainHdSolver_Vereshchagin::segment_info::PC, KDL::ChainHdSolver_Vereshchagin::segment_info::PZ, KDL::ChainHdSolver_Vereshchagin::segment_info::R, KDL::ChainHdSolver_Vereshchagin::segment_info::R_tilde, KDL::ChainHdSolver_Vereshchagin::results, KDL::Twist::rot, KDL::Wrench::torque, KDL::ChainHdSolver_Vereshchagin::segment_info::totalBias, KDL::ChainHdSolver_Vereshchagin::segment_info::U, KDL::ChainHdSolver_Vereshchagin::segment_info::u, KDL::Twist::vel, and KDL::ChainHdSolver_Vereshchagin::segment_info::Z.
Referenced by KDL::ChainHdSolver_Vereshchagin::CartToJnt().
|
privateinherited |
This method puts all acceleration contributions (constraint, bias, nullspace and parent accelerations) together.
References KDL::ChainHdSolver_Vereshchagin::segment_info::acc, KDL::ChainHdSolver_Vereshchagin::acc_root, KDL::ChainHdSolver_Vereshchagin::segment_info::C, KDL::ChainHdSolver_Vereshchagin::chain, KDL::ChainHdSolver_Vereshchagin::segment_info::constAccComp, KDL::ChainHdSolver_Vereshchagin::segment_info::D, dot(), KDL::ChainHdSolver_Vereshchagin::segment_info::E, KDL::ChainHdSolver_Vereshchagin::segment_info::F, KDL::Joint::Fixed, KDL::Segment::getJoint(), KDL::Chain::getSegment(), KDL::Joint::getType(), KDL::Frame::Inverse(), KDL::ChainHdSolver_Vereshchagin::ns, KDL::ChainHdSolver_Vereshchagin::nu, KDL::ChainHdSolver_Vereshchagin::segment_info::nullspaceAccComp, KDL::ChainHdSolver_Vereshchagin::segment_info::P, KDL::ChainHdSolver_Vereshchagin::results, KDL::ChainHdSolver_Vereshchagin::total_torques, KDL::ChainHdSolver_Vereshchagin::segment_info::u, and KDL::ChainHdSolver_Vereshchagin::segment_info::Z.
Referenced by KDL::ChainHdSolver_Vereshchagin::CartToJnt().
|
inherited |
References KDL::ChainHdSolver_Vereshchagin::nu.
|
inlinevirtualinherited |
Return the latest error.
References KDL::SolverI::error.
|
inherited |
References KDL::JntArray::data, and KDL::ChainHdSolver_Vereshchagin::total_torques.
|
inherited |
|
privateinherited |
This method calculates all cartesian space poses, twists, bias accelerations.
External forces are also taken into account in this outward sweep.
References KDL::ChainHdSolver_Vereshchagin::segment_info::A, KDL::ChainHdSolver_Vereshchagin::acc_root, KDL::ChainHdSolver_Vereshchagin::segment_info::C, KDL::ChainHdSolver_Vereshchagin::chain, KDL::ChainHdSolver_Vereshchagin::segment_info::F, KDL::ChainHdSolver_Vereshchagin::segment_info::F_base, KDL::ChainHdSolver_Vereshchagin::F_total, KDL::Joint::Fixed, KDL::Segment::getInertia(), KDL::Segment::getJoint(), KDL::Chain::getSegment(), KDL::Joint::getType(), KDL::ChainHdSolver_Vereshchagin::segment_info::H, KDL::Frame::Identity(), KDL::Rotation::Inverse(), KDL::Frame::Inverse(), KDL::Frame::M, KDL::ChainHdSolver_Vereshchagin::ns, KDL::Segment::pose(), KDL::ChainHdSolver_Vereshchagin::results, KDL::Segment::twist(), KDL::ChainHdSolver_Vereshchagin::segment_info::U, KDL::ChainHdSolver_Vereshchagin::segment_info::v, and KDL::ChainHdSolver_Vereshchagin::segment_info::Z.
Referenced by KDL::ChainHdSolver_Vereshchagin::CartToJnt().
|
inlinevirtualinherited |
Return a description of the latest error.
Reimplemented in KDL::ChainExternalWrenchEstimator, KDL::ChainIkSolverPos_LMA, KDL::ChainIkSolverPos_NR, KDL::ChainIkSolverPos_NR_JL, KDL::ChainIkSolverVel_pinv, KDL::ChainIkSolverVel_wdls, and KDL::ChainJntToJacDotSolver.
References KDL::SolverI::E_DEGRADED, KDL::SolverI::E_MAX_ITERATIONS_EXCEEDED, KDL::SolverI::E_NO_CONVERGE, KDL::SolverI::E_NOERROR, KDL::SolverI::E_NOT_IMPLEMENTED, KDL::SolverI::E_NOT_UP_TO_DATE, KDL::SolverI::E_OUT_OF_RANGE, KDL::SolverI::E_SIZE_MISMATCH, KDL::SolverI::E_SVD_FAILED, KDL::SolverI::E_UNDEFINED, and KDL::SolverI::error.
Referenced by KDL::ChainExternalWrenchEstimator::strError(), KDL::ChainIkSolverPos_LMA::strError(), KDL::ChainIkSolverPos_NR::strError(), KDL::ChainIkSolverPos_NR_JL::strError(), KDL::ChainIkSolverVel_pinv::strError(), KDL::ChainIkSolverVel_wdls::strError(), and KDL::ChainJntToJacDotSolver::strError().
|
virtualinherited |
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 KDL::ChainHdSolver_Vereshchagin::chain, KDL::Chain::getNrOfJoints(), KDL::Chain::getNrOfSegments(), KDL::ChainHdSolver_Vereshchagin::nc, KDL::ChainHdSolver_Vereshchagin::nj, KDL::ChainHdSolver_Vereshchagin::ns, KDL::ChainHdSolver_Vereshchagin::results, and KDL::ChainHdSolver_Vereshchagin::total_torques.
|
privateinherited |
Referenced by KDL::ChainHdSolver_Vereshchagin::ChainHdSolver_Vereshchagin(), KDL::ChainHdSolver_Vereshchagin::constraint_calculation(), KDL::ChainHdSolver_Vereshchagin::final_upwards_sweep(), KDL::ChainHdSolver_Vereshchagin::getTransformedLinkAcceleration(), and KDL::ChainHdSolver_Vereshchagin::initial_upwards_sweep().
|
privateinherited |
|
privateinherited |
|
privateinherited |
|
privateinherited |
|
protectedinherited |
Latest error, initialized to E_NOERROR in constructor.
Referenced by KDL::ChainIdSolver_RNE::CartToJnt(), KDL::TreeIdSolver_RNE::CartToJnt(), KDL::ChainFdSolver_RNE::CartToJnt(), KDL::ChainHdSolver_Vereshchagin::CartToJnt(), KDL::ChainIkSolverVel_pinv::CartToJnt(), KDL::ChainIkSolverVel_pinv_givens::CartToJnt(), KDL::ChainIkSolverVel_pinv_nso::CartToJnt(), KDL::ChainIkSolverVel_wdls::CartToJnt(), KDL::ChainIkSolverPos_NR::CartToJnt(), KDL::ChainIkSolverPos_NR_JL::CartToJnt(), KDL::ChainIkSolverPos_LMA::CartToJnt(), KDL::SolverI::getError(), KDL::ChainIkSolverVel_wdls::getSigma(), KDL::ChainFkSolverPos_recursive::JntToCart(), KDL::ChainFkSolverVel_recursive::JntToCart(), KDL::ChainExternalWrenchEstimator::JntToExtWrench(), KDL::ChainJntToJacSolver::JntToJac(), KDL::ChainJntToJacDotSolver::JntToJacDot(), KDL::ChainDynParam::JntToMass(), KDL::ChainExternalWrenchEstimator::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(), KDL::ChainExternalWrenchEstimator::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().
|
privateinherited |
|
privateinherited |
|
privateinherited |
Referenced by KDL::ChainHdSolver_Vereshchagin::CartToJnt(), KDL::ChainHdSolver_Vereshchagin::ChainHdSolver_Vereshchagin(), KDL::ChainHdSolver_Vereshchagin::constraint_calculation(), KDL::ChainHdSolver_Vereshchagin::downwards_sweep(), KDL::ChainHdSolver_Vereshchagin::segment_info::segment_info(), and KDL::ChainHdSolver_Vereshchagin::updateInternalDataStructures().
|
privateinherited |
|
privateinherited |
Referenced by KDL::ChainHdSolver_Vereshchagin::CartToJnt(), KDL::ChainHdSolver_Vereshchagin::downwards_sweep(), KDL::ChainHdSolver_Vereshchagin::final_upwards_sweep(), KDL::ChainHdSolver_Vereshchagin::getTransformedLinkAcceleration(), KDL::ChainHdSolver_Vereshchagin::initial_upwards_sweep(), and KDL::ChainHdSolver_Vereshchagin::updateInternalDataStructures().
|
privateinherited |
|
privateinherited |
|
privateinherited |
|
privateinherited |
Referenced by KDL::ChainHdSolver_Vereshchagin::constraint_calculation(), KDL::ChainHdSolver_Vereshchagin::downwards_sweep(), KDL::ChainHdSolver_Vereshchagin::final_upwards_sweep(), KDL::ChainHdSolver_Vereshchagin::getTransformedLinkAcceleration(), KDL::ChainHdSolver_Vereshchagin::initial_upwards_sweep(), and KDL::ChainHdSolver_Vereshchagin::updateInternalDataStructures().
|
privateinherited |
|
privateinherited |
|
privateinherited |
|
privateinherited |
|
privateinherited |