KDL
1.4.0
|
Computes the Jacobian time derivative (Jdot) by calculating the partial derivatives regarding to a joint angle, in the Hybrid, Body-fixed or Inertial representation. More...
#include <src/chainjnttojacdotsolver.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 | |
ChainJntToJacDotSolver (const Chain &chain) | |
virtual | ~ChainJntToJacDotSolver () |
virtual int | JntToJacDot (const KDL::JntArrayVel &q_in, KDL::Twist &jac_dot_q_dot, int seg_nr=-1) |
Computes \( {}_{bs}\dot{J}^{ee}.\dot{q} \). More... | |
virtual int | JntToJacDot (const KDL::JntArrayVel &q_in, KDL::Jacobian &jdot, int seg_nr=-1) |
Computes \( {}_{bs}\dot{J}^{ee} \). More... | |
int | setLockedJoints (const std::vector< bool > &locked_joints) |
void | setHybridRepresentation () |
JntToJacDot() will compute in the Hybrid representation (ref Frame: base, ref Point: end-effector) More... | |
void | setBodyFixedRepresentation () |
JntToJacDot() will compute in the Body-fixed representation (ref Frame: end-effector, ref Point: end-effector) More... | |
void | setInternialRepresentation () |
JntToJacDot() will compute in the Inertial representation (ref Frame: base, ref Point: base) More... | |
void | setRepresentation (const int &representation) |
Sets the internal variable for the representation (with a check on the value) More... | |
virtual void | updateInternalDataStructures () |
Update the internal data structures. More... | |
virtual const char * | strError (const int error) const |
Return a description of the latest error. More... | |
virtual int | getError () const |
Return the latest error. More... | |
Static Public Attributes | |
static const int | E_JAC_DOT_FAILED = -100 |
static const int | E_JACSOLVER_FAILED = -101 |
static const int | E_FKSOLVERPOS_FAILED = -102 |
static const int | HYBRID = 0 |
static const int | BODYFIXED = 1 |
static const int | INTERTIAL = 2 |
Protected Member Functions | |
const Twist & | getPartialDerivativeHybrid (const Jacobian &bs_J_ee, const unsigned int &joint_idx, const unsigned int &column_idx) |
Computes \( \frac{\partial {}_{bs}J^{i,ee}}{\partial q^{j}}.\dot{q}^{j} \). More... | |
const Twist & | getPartialDerivativeBodyFixed (const Jacobian &ee_J_ee, const unsigned int &joint_idx, const unsigned int &column_idx) |
Computes \( \frac{\partial {}_{ee}J^{i,ee}}{\partial q^{j}}.\dot{q}^{j} \). More... | |
const Twist & | getPartialDerivativeInertial (const Jacobian &bs_J_bs, const unsigned int &joint_idx, const unsigned int &column_idx) |
Computes \( \frac{\partial {}_{bs}J^{i,bs}}{\partial q^{j}}.\dot{q}^{j} \). More... | |
const Twist & | getPartialDerivative (const Jacobian &J, const unsigned int &joint_idx, const unsigned int &column_idx, const int &representation) |
Computes \( \frac{\partial J^{i,ee}}{\partial q^{j}}.\dot{q}^{j} \). More... | |
Protected Attributes | |
int | error |
Latest error, initialized to E_NOERROR in constructor. More... | |
Private Attributes | |
const Chain & | chain |
std::vector< bool > | locked_joints_ |
unsigned int | nr_of_unlocked_joints_ |
ChainJntToJacSolver | jac_solver_ |
Jacobian | jac_ |
Jacobian | jac_dot_ |
int | representation_ |
ChainFkSolverPos_recursive | fk_solver_ |
Frame | F_bs_ee_ |
Twist | jac_dot_k_ |
Twist | jac_j_ |
Twist | jac_i_ |
Twist | t_djdq_ |
Computes the Jacobian time derivative (Jdot) by calculating the partial derivatives regarding to a joint angle, in the Hybrid, Body-fixed or Inertial representation.
This work is based on : Symbolic differentiation of the velocity mapping for a serial kinematic chain H. Bruyninckx, J. De Schutter doi:10.1016/0094-114X(95)00069-B
url : http://www.sciencedirect.com/science/article/pii/0094114X9500069B
|
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. |
|
explicit |
|
virtual |
|
inlinevirtualinherited |
Return the latest error.
References KDL::SolverI::error.
|
protected |
Computes \( \frac{\partial J^{i,ee}}{\partial q^{j}}.\dot{q}^{j} \).
bs_J_bs | The Jacobian expressed in the base frame with the end effector as the reference point |
joint_idx | The indice of the current joint (j in the formula) |
column_idx | The indice of the current column (i in the formula) |
representation | The representation (Hybrid,Body-fixed,Inertial) in which you want to get dJ/dqj .qdotj |
References BODYFIXED, getPartialDerivativeBodyFixed(), getPartialDerivativeHybrid(), getPartialDerivativeInertial(), HYBRID, INTERTIAL, KDL::SetToZero(), and t_djdq_.
Referenced by JntToJacDot(), and setInternialRepresentation().
|
protected |
Computes \( \frac{\partial {}_{ee}J^{i,ee}}{\partial q^{j}}.\dot{q}^{j} \).
bs_J_ee | The Jacobian expressed in the end effector frame with the end effector as the reference point |
joint_idx | The indice of the current joint (j in the formula) |
column_idx | The indice of the current column (i in the formula) |
References KDL::Jacobian::getColumn(), jac_i_, jac_j_, KDL::Twist::rot, KDL::SetToZero(), t_djdq_, and KDL::Twist::vel.
Referenced by getPartialDerivative(), and setInternialRepresentation().
|
protected |
Computes \( \frac{\partial {}_{bs}J^{i,ee}}{\partial q^{j}}.\dot{q}^{j} \).
bs_J_ee | The Jacobian expressed in the base frame with the end effector as the reference point (default in KDL Jacobian Solver) |
joint_idx | The index of the current joint (j in the formula) |
column_idx | The index of the current column (i in the formula) |
References KDL::Jacobian::getColumn(), jac_i_, jac_j_, KDL::Twist::rot, KDL::SetToZero(), t_djdq_, and KDL::Twist::vel.
Referenced by getPartialDerivative(), and setInternialRepresentation().
|
protected |
Computes \( \frac{\partial {}_{bs}J^{i,bs}}{\partial q^{j}}.\dot{q}^{j} \).
ee_J_ee | The Jacobian expressed in the base frame with the base as the reference point |
joint_idx | The indice of the current joint (j in the formula) |
column_idx | The indice of the current column (i in the formula) |
References KDL::Jacobian::getColumn(), jac_i_, jac_j_, KDL::Twist::rot, KDL::SetToZero(), t_djdq_, and KDL::Twist::vel.
Referenced by getPartialDerivative(), and setInternialRepresentation().
|
virtual |
Computes \( {}_{bs}\dot{J}^{ee}.\dot{q} \).
q_in | Current joint positions and velocities |
jac_dot_q_dot | The twist representing Jdot*qdot |
seg_nr | The final segment to compute |
References KDL::SolverI::E_NOERROR, KDL::SolverI::error, jac_dot_, KDL::MultiplyJacobian(), and KDL::JntArrayVel::qdot.
|
virtual |
Computes \( {}_{bs}\dot{J}^{ee} \).
q_in | Current joint positions and velocities |
jdot | The jacobian time derivative in the configured representation (HYBRID, BODYFIXED or INERTIAL) |
seg_nr | The final segment to compute |
References BODYFIXED, chain, KDL::Jacobian::changeBase, KDL::Jacobian::changeRefPoint, KDL::Jacobian::columns(), E_FKSOLVERPOS_FAILED, E_JAC_DOT_FAILED, E_JACSOLVER_FAILED, KDL::SolverI::E_NOERROR, KDL::SolverI::E_NOT_UP_TO_DATE, KDL::SolverI::E_OUT_OF_RANGE, KDL::SolverI::E_SIZE_MISMATCH, KDL::SolverI::error, F_bs_ee_, fk_solver_, KDL::Segment::getJoint(), KDL::Chain::getNrOfJoints(), KDL::Chain::getNrOfSegments(), getPartialDerivative(), KDL::Chain::getSegment(), KDL::Joint::getType(), HYBRID, INTERTIAL, KDL::Rotation::Inverse(), jac_, jac_dot_k_, jac_solver_, KDL::ChainFkSolverPos_recursive::JntToCart(), KDL::ChainJntToJacSolver::JntToJac(), locked_joints_, KDL::Frame::M, KDL::Joint::None, nr_of_unlocked_joints_, KDL::Frame::p, KDL::JntArrayVel::q, KDL::JntArrayVel::qdot, representation_, KDL::JntArray::rows(), KDL::Jacobian::setColumn(), and KDL::SetToZero().
|
inline |
JntToJacDot() will compute in the Body-fixed representation (ref Frame: end-effector, ref Point: end-effector)
References setRepresentation().
|
inline |
JntToJacDot() will compute in the Hybrid representation (ref Frame: base, ref Point: end-effector)
References setRepresentation().
|
inline |
JntToJacDot() will compute in the Inertial representation (ref Frame: base, ref Point: base)
References KDL::SolverI::error, getPartialDerivative(), getPartialDerivativeBodyFixed(), getPartialDerivativeHybrid(), getPartialDerivativeInertial(), setRepresentation(), strError(), and updateInternalDataStructures().
int KDL::ChainJntToJacDotSolver::setLockedJoints | ( | const std::vector< bool > & | locked_joints | ) |
References KDL::SolverI::E_NOERROR, KDL::SolverI::E_SIZE_MISMATCH, KDL::SolverI::error, locked_joints_, and nr_of_unlocked_joints_.
Referenced by updateInternalDataStructures().
void KDL::ChainJntToJacDotSolver::setRepresentation | ( | const int & | representation | ) |
Sets the internal variable for the representation (with a check on the value)
representation | The representation for Jdot : HYBRID,BODYFIXED or INTERTIAL |
References BODYFIXED, HYBRID, INTERTIAL, and representation_.
Referenced by setBodyFixedRepresentation(), setHybridRepresentation(), and setInternialRepresentation().
|
virtual |
Return a description of the latest error.
Reimplemented from KDL::SolverI.
References E_FKSOLVERPOS_FAILED, E_JAC_DOT_FAILED, E_JACSOLVER_FAILED, and KDL::SolverI::strError().
Referenced by setInternialRepresentation().
|
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, fk_solver_, KDL::Chain::getNrOfJoints(), jac_, jac_dot_, jac_solver_, locked_joints_, KDL::Jacobian::resize(), setLockedJoints(), KDL::ChainFkSolverPos_recursive::updateInternalDataStructures(), and KDL::ChainJntToJacSolver::updateInternalDataStructures().
Referenced by setInternialRepresentation().
|
static |
Referenced by getPartialDerivative(), JntToJacDot(), and setRepresentation().
|
private |
Referenced by JntToJacDot(), and updateInternalDataStructures().
|
static |
Referenced by JntToJacDot(), and strError().
|
static |
Referenced by JntToJacDot(), and strError().
|
static |
Referenced by JntToJacDot(), and strError().
|
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(), KDL::ChainIdSolver_Vereshchagin::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(), JntToJacDot(), KDL::ChainDynParam::JntToMass(), setInternialRepresentation(), KDL::ChainIkSolverPos_NR_JL::setJointLimits(), KDL::ChainJntToJacSolver::setLockedJoints(), setLockedJoints(), KDL::ChainIkSolverVel_pinv_nso::setOptPos(), KDL::ChainIkSolverVel_wdls::setWeightJS(), KDL::ChainIkSolverVel_pinv_nso::setWeights(), and KDL::ChainIkSolverVel_wdls::setWeightTS().
|
private |
Referenced by JntToJacDot().
|
private |
Referenced by JntToJacDot(), and updateInternalDataStructures().
|
static |
Referenced by getPartialDerivative(), JntToJacDot(), and setRepresentation().
|
static |
Referenced by getPartialDerivative(), JntToJacDot(), and setRepresentation().
|
private |
Referenced by JntToJacDot(), and updateInternalDataStructures().
|
private |
Referenced by JntToJacDot(), and updateInternalDataStructures().
|
private |
Referenced by JntToJacDot().
|
private |
Referenced by getPartialDerivativeBodyFixed(), getPartialDerivativeHybrid(), and getPartialDerivativeInertial().
|
private |
Referenced by getPartialDerivativeBodyFixed(), getPartialDerivativeHybrid(), and getPartialDerivativeInertial().
|
private |
Referenced by JntToJacDot(), and updateInternalDataStructures().
|
private |
Referenced by JntToJacDot(), setLockedJoints(), and updateInternalDataStructures().
|
private |
Referenced by JntToJacDot(), and setLockedJoints().
|
private |
Referenced by JntToJacDot(), and setRepresentation().
|
private |