KDL
1.4.0
|
Dynamics calculations by constraints based on Vereshchagin 1989. More...
#include <src/chainidsolver_vereshchagin.hpp>
Classes | |
struct | segment_info |
Public Member Functions | |
ChainIdSolver_Vereshchagin (const Chain &chain, Twist root_acc, unsigned int nc) | |
Constructor for the solver, it will allocate all the necessary memory. More... | |
~ChainIdSolver_Vereshchagin () | |
int | CartToJnt (const JntArray &q, const JntArray &q_dot, JntArray &q_dotdot, const Jacobian &alfa, const JntArray &beta, const Wrenches &f_ext, JntArray &torques) |
This method calculates joint space constraint torques and total joint space acceleration. More... | |
virtual void | updateInternalDataStructures () |
Update the internal data structures. More... | |
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 &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 &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 |
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... | |
Dynamics calculations by constraints based on Vereshchagin 1989.
for a chain. This class creates instance of hybrid dynamics solver. The solver calculates total joint space accelerations in a chain when a constraint force(s) is applied to the chain's end-effector (task space/cartesian space).
|
private |
|
private |
|
private |
|
private |
|
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::ChainIdSolver_Vereshchagin::ChainIdSolver_Vereshchagin | ( | const Chain & | chain, |
Twist | root_acc, | ||
unsigned int | nc | ||
) |
Constructor for the solver, it will allocate all the necessary memory.
chain | The kinematic chain to calculate the inverse dynamics for, an internal copy will be made. |
root_acc | The acceleration vector of the root to use during the calculation.(most likely contains gravity) |
References acc_root, M_0_inverse, nc, nu_sum, Sm, tmpm, Um, and Vm.
|
inline |
int KDL::ChainIdSolver_Vereshchagin::CartToJnt | ( | const JntArray & | q, |
const JntArray & | q_dot, | ||
JntArray & | q_dotdot, | ||
const Jacobian & | alfa, | ||
const JntArray & | beta, | ||
const Wrenches & | f_ext, | ||
JntArray & | torques | ||
) |
This method calculates joint space constraint torques and total joint space acceleration.
It returns 0 when it succeeds, otherwise -1 or -2 for unmatching matrix and array sizes. Input parameters;
q | The current joint positions |
q_dot | The current joint velocities |
f_ext | The external forces (no gravity, it is given in root acceleration) on the segments. Output parameters: |
q_dotdot | The joint accelerations |
torques | the resulting constraint torques for the joints |
References chain, KDL::Jacobian::columns(), constraint_calculation(), downwards_sweep(), KDL::SolverI::E_NOERROR, KDL::SolverI::E_NOT_UP_TO_DATE, KDL::SolverI::E_SIZE_MISMATCH, KDL::SolverI::error, final_upwards_sweep(), KDL::Chain::getNrOfJoints(), KDL::Chain::getNrOfSegments(), initial_upwards_sweep(), nc, nj, ns, and KDL::JntArray::rows().
Referenced by ~ChainIdSolver_Vereshchagin().
|
private |
This method calculates constraint force magnitudes.
References acc_root, KDL::JntArray::data, KDL::Vector::data, M_0_inverse, nc, nu, nu_sum, results, KDL::Twist::rot, Sm, tmpm, Um, KDL::Twist::vel, and Vm.
Referenced by CartToJnt(), and ~ChainIdSolver_Vereshchagin().
|
private |
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.
References KDL::ChainIdSolver_Vereshchagin::segment_info::C, chain, KDL::ChainIdSolver_Vereshchagin::segment_info::D, KDL::Vector::data, KDL::dot(), KDL::ChainIdSolver_Vereshchagin::segment_info::E, KDL::ChainIdSolver_Vereshchagin::segment_info::E_tilde, KDL::ChainIdSolver_Vereshchagin::segment_info::EZ, KDL::ChainIdSolver_Vereshchagin::segment_info::F, F_total, KDL::Wrench::force, KDL::ChainIdSolver_Vereshchagin::segment_info::G, KDL::Segment::getJoint(), KDL::Chain::getSegment(), KDL::Joint::getType(), KDL::ChainIdSolver_Vereshchagin::segment_info::H, KDL::Rotation::Inverse(), KDL::ChainIdSolver_Vereshchagin::segment_info::M, KDL::Frame::M, nc, nj, KDL::Joint::None, ns, KDL::ChainIdSolver_Vereshchagin::segment_info::P, KDL::ChainIdSolver_Vereshchagin::segment_info::P_tilde, KDL::ChainIdSolver_Vereshchagin::segment_info::PC, KDL::ChainIdSolver_Vereshchagin::segment_info::PZ, KDL::ChainIdSolver_Vereshchagin::segment_info::R, KDL::ChainIdSolver_Vereshchagin::segment_info::R_tilde, results, KDL::Twist::rot, KDL::Wrench::torque, KDL::ChainIdSolver_Vereshchagin::segment_info::totalBias, KDL::ChainIdSolver_Vereshchagin::segment_info::U, KDL::ChainIdSolver_Vereshchagin::segment_info::u, KDL::Twist::vel, and KDL::ChainIdSolver_Vereshchagin::segment_info::Z.
Referenced by CartToJnt(), and ~ChainIdSolver_Vereshchagin().
|
private |
This method puts all acceleration contributions (constraint, bias, nullspace and parent accelerations) together.
References KDL::ChainIdSolver_Vereshchagin::segment_info::acc, acc_root, KDL::ChainIdSolver_Vereshchagin::segment_info::C, chain, KDL::ChainIdSolver_Vereshchagin::segment_info::constAccComp, KDL::ChainIdSolver_Vereshchagin::segment_info::D, KDL::dot(), KDL::ChainIdSolver_Vereshchagin::segment_info::E, KDL::ChainIdSolver_Vereshchagin::segment_info::F, KDL::Segment::getJoint(), KDL::Chain::getSegment(), KDL::Joint::getType(), KDL::Frame::Inverse(), KDL::Joint::None, ns, nu, KDL::ChainIdSolver_Vereshchagin::segment_info::nullspaceAccComp, KDL::ChainIdSolver_Vereshchagin::segment_info::P, results, KDL::ChainIdSolver_Vereshchagin::segment_info::u, and KDL::ChainIdSolver_Vereshchagin::segment_info::Z.
Referenced by CartToJnt(), and ~ChainIdSolver_Vereshchagin().
|
inlinevirtualinherited |
Return the latest error.
References KDL::SolverI::error.
|
private |
This method calculates all cartesian space poses, twists, bias accelerations.
External forces are also taken into account in this outward sweep.
References KDL::ChainIdSolver_Vereshchagin::segment_info::A, acc_root, KDL::ChainIdSolver_Vereshchagin::segment_info::C, chain, KDL::ChainIdSolver_Vereshchagin::segment_info::F, KDL::ChainIdSolver_Vereshchagin::segment_info::F_base, F_total, KDL::Segment::getInertia(), KDL::Segment::getJoint(), KDL::Chain::getSegment(), KDL::Joint::getType(), KDL::ChainIdSolver_Vereshchagin::segment_info::H, KDL::Frame::Identity(), KDL::Rotation::Inverse(), KDL::Frame::Inverse(), KDL::Frame::M, KDL::Joint::None, ns, KDL::Segment::pose(), results, KDL::Segment::twist(), KDL::ChainIdSolver_Vereshchagin::segment_info::U, KDL::ChainIdSolver_Vereshchagin::segment_info::v, and KDL::ChainIdSolver_Vereshchagin::segment_info::Z.
Referenced by CartToJnt(), and ~ChainIdSolver_Vereshchagin().
|
inlinevirtualinherited |
Return a description of the latest error.
Reimplemented in KDL::ChainIkSolverVel_wdls, KDL::ChainIkSolverPos_LMA, KDL::ChainJntToJacDotSolver, KDL::ChainIkSolverPos_NR_JL, KDL::ChainIkSolverVel_pinv, and KDL::ChainIkSolverPos_NR.
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::updateInternalDataStructures().
Referenced by KDL::ChainIkSolverPos_NR::strError(), KDL::ChainIkSolverVel_pinv::strError(), KDL::ChainIkSolverPos_NR_JL::strError(), KDL::ChainJntToJacDotSolver::strError(), KDL::ChainIkSolverPos_LMA::strError(), and KDL::ChainIkSolverVel_wdls::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, KDL::Chain::getNrOfSegments(), nc, ns, and results.
Referenced by ~ChainIdSolver_Vereshchagin().
|
private |
Referenced by ChainIdSolver_Vereshchagin(), constraint_calculation(), final_upwards_sweep(), and initial_upwards_sweep().
|
private |
|
private |
|
private |
|
private |
Referenced by CartToJnt(), downwards_sweep(), final_upwards_sweep(), initial_upwards_sweep(), and updateInternalDataStructures().
|
protectedinherited |
Latest error, initialized to E_NOERROR in constructor.
Referenced by KDL::ChainIkSolverVel_pinv_givens::CartToJnt(), KDL::ChainIdSolver_RNE::CartToJnt(), KDL::ChainIkSolverVel_pinv_nso::CartToJnt(), CartToJnt(), KDL::ChainIkSolverPos_NR::CartToJnt(), KDL::ChainIkSolverVel_pinv::CartToJnt(), KDL::ChainIkSolverPos_NR_JL::CartToJnt(), KDL::ChainIkSolverVel_wdls::CartToJnt(), KDL::ChainIkSolverPos_LMA::CartToJnt(), KDL::SolverI::getError(), KDL::ChainIkSolverVel_wdls::getSigma(), KDL::ChainIkSolverVel_pinv::getSVDResult(), KDL::ChainIkSolverVel_wdls::getSVDResult(), KDL::ChainFkSolverPos_recursive::JntToCart(), KDL::ChainFkSolverVel_recursive::JntToCart(), KDL::ChainJntToJacSolver::JntToJac(), KDL::ChainJntToJacDotSolver::JntToJacDot(), KDL::ChainDynParam::JntToMass(), KDL::ChainJntToJacDotSolver::setInternialRepresentation(), KDL::ChainIkSolverPos_NR_JL::setJointLimits(), KDL::ChainJntToJacSolver::setLockedJoints(), KDL::ChainJntToJacDotSolver::setLockedJoints(), KDL::ChainIkSolverVel_pinv_nso::setOptPos(), KDL::ChainIkSolverVel_wdls::setWeightJS(), KDL::ChainIkSolverVel_pinv_nso::setWeights(), and KDL::ChainIkSolverVel_wdls::setWeightTS().
|
private |
Referenced by downwards_sweep(), and initial_upwards_sweep().
|
private |
Referenced by ChainIdSolver_Vereshchagin(), and constraint_calculation().
|
private |
|
private |
Referenced by CartToJnt(), and downwards_sweep().
|
private |
Referenced by CartToJnt(), downwards_sweep(), final_upwards_sweep(), initial_upwards_sweep(), and updateInternalDataStructures().
|
private |
Referenced by constraint_calculation(), and final_upwards_sweep().
|
private |
Referenced by ChainIdSolver_Vereshchagin(), and constraint_calculation().
|
private |
|
private |
|
private |
Referenced by ChainIdSolver_Vereshchagin(), and constraint_calculation().
|
private |
Referenced by ChainIdSolver_Vereshchagin(), and constraint_calculation().
|
private |
Referenced by ChainIdSolver_Vereshchagin(), and constraint_calculation().
|
private |
Referenced by ChainIdSolver_Vereshchagin(), and constraint_calculation().