KDL  1.4.0
Public Types | Public Member Functions | Static Public Attributes | Protected Member Functions | Protected Attributes | Private Attributes | List of all members
KDL::ChainJntToJacDotSolver Class Reference

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>

Inheritance diagram for KDL::ChainJntToJacDotSolver:
Inheritance graph
[legend]
Collaboration diagram for KDL::ChainJntToJacDotSolver:
Collaboration graph
[legend]

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 TwistgetPartialDerivativeHybrid (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 TwistgetPartialDerivativeBodyFixed (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 TwistgetPartialDerivativeInertial (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 TwistgetPartialDerivative (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 Chainchain
 
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_
 

Detailed Description

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

Member Enumeration Documentation

§ anonymous enum

anonymous enum
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.

Constructor & Destructor Documentation

§ ChainJntToJacDotSolver()

KDL::ChainJntToJacDotSolver::ChainJntToJacDotSolver ( const Chain chain)
explicit

§ ~ChainJntToJacDotSolver()

KDL::ChainJntToJacDotSolver::~ChainJntToJacDotSolver ( )
virtual

Member Function Documentation

§ getError()

virtual int KDL::SolverI::getError ( ) const
inlinevirtualinherited

Return the latest error.

References KDL::SolverI::error.

§ getPartialDerivative()

const Twist & KDL::ChainJntToJacDotSolver::getPartialDerivative ( const Jacobian J,
const unsigned int &  joint_idx,
const unsigned int &  column_idx,
const int &  representation 
)
protected

Computes \( \frac{\partial J^{i,ee}}{\partial q^{j}}.\dot{q}^{j} \).

Parameters
bs_J_bsThe Jacobian expressed in the base frame with the end effector as the reference point
joint_idxThe indice of the current joint (j in the formula)
column_idxThe indice of the current column (i in the formula)
representationThe representation (Hybrid,Body-fixed,Inertial) in which you want to get dJ/dqj .qdotj
Returns
Twist The twist representing dJi/dqj .qdotj

References BODYFIXED, getPartialDerivativeBodyFixed(), getPartialDerivativeHybrid(), getPartialDerivativeInertial(), HYBRID, INTERTIAL, KDL::SetToZero(), and t_djdq_.

Referenced by JntToJacDot(), and setInternialRepresentation().

§ getPartialDerivativeBodyFixed()

const Twist & KDL::ChainJntToJacDotSolver::getPartialDerivativeBodyFixed ( const Jacobian ee_J_ee,
const unsigned int &  joint_idx,
const unsigned int &  column_idx 
)
protected

Computes \( \frac{\partial {}_{ee}J^{i,ee}}{\partial q^{j}}.\dot{q}^{j} \).

Parameters
bs_J_eeThe Jacobian expressed in the end effector frame with the end effector as the reference point
joint_idxThe indice of the current joint (j in the formula)
column_idxThe indice of the current column (i in the formula)
Returns
Twist The twist representing dJi/dqj .qdotj

References KDL::Jacobian::getColumn(), jac_i_, jac_j_, KDL::Twist::rot, KDL::SetToZero(), t_djdq_, and KDL::Twist::vel.

Referenced by getPartialDerivative(), and setInternialRepresentation().

§ getPartialDerivativeHybrid()

const Twist & KDL::ChainJntToJacDotSolver::getPartialDerivativeHybrid ( const Jacobian bs_J_ee,
const unsigned int &  joint_idx,
const unsigned int &  column_idx 
)
protected

Computes \( \frac{\partial {}_{bs}J^{i,ee}}{\partial q^{j}}.\dot{q}^{j} \).

Parameters
bs_J_eeThe Jacobian expressed in the base frame with the end effector as the reference point (default in KDL Jacobian Solver)
joint_idxThe index of the current joint (j in the formula)
column_idxThe index of the current column (i in the formula)
Returns
Twist The twist representing dJi/dqj .qdotj

References KDL::Jacobian::getColumn(), jac_i_, jac_j_, KDL::Twist::rot, KDL::SetToZero(), t_djdq_, and KDL::Twist::vel.

Referenced by getPartialDerivative(), and setInternialRepresentation().

§ getPartialDerivativeInertial()

const Twist & KDL::ChainJntToJacDotSolver::getPartialDerivativeInertial ( const Jacobian bs_J_bs,
const unsigned int &  joint_idx,
const unsigned int &  column_idx 
)
protected

Computes \( \frac{\partial {}_{bs}J^{i,bs}}{\partial q^{j}}.\dot{q}^{j} \).

Parameters
ee_J_eeThe Jacobian expressed in the base frame with the base as the reference point
joint_idxThe indice of the current joint (j in the formula)
column_idxThe indice of the current column (i in the formula)
Returns
Twist The twist representing dJi/dqj .qdotj

References KDL::Jacobian::getColumn(), jac_i_, jac_j_, KDL::Twist::rot, KDL::SetToZero(), t_djdq_, and KDL::Twist::vel.

Referenced by getPartialDerivative(), and setInternialRepresentation().

§ JntToJacDot() [1/2]

int KDL::ChainJntToJacDotSolver::JntToJacDot ( const KDL::JntArrayVel q_in,
KDL::Twist jac_dot_q_dot,
int  seg_nr = -1 
)
virtual

Computes \( {}_{bs}\dot{J}^{ee}.\dot{q} \).

Parameters
q_inCurrent joint positions and velocities
jac_dot_q_dotThe twist representing Jdot*qdot
seg_nrThe final segment to compute
Returns
int 0 if no errors happened

References KDL::SolverI::E_NOERROR, KDL::SolverI::error, jac_dot_, KDL::MultiplyJacobian(), and KDL::JntArrayVel::qdot.

§ JntToJacDot() [2/2]

int KDL::ChainJntToJacDotSolver::JntToJacDot ( const KDL::JntArrayVel q_in,
KDL::Jacobian jdot,
int  seg_nr = -1 
)
virtual

§ setBodyFixedRepresentation()

void KDL::ChainJntToJacDotSolver::setBodyFixedRepresentation ( )
inline

JntToJacDot() will compute in the Body-fixed representation (ref Frame: end-effector, ref Point: end-effector)

Returns
void

References setRepresentation().

§ setHybridRepresentation()

void KDL::ChainJntToJacDotSolver::setHybridRepresentation ( )
inline

JntToJacDot() will compute in the Hybrid representation (ref Frame: base, ref Point: end-effector)

Returns
void

References setRepresentation().

§ setInternialRepresentation()

void KDL::ChainJntToJacDotSolver::setInternialRepresentation ( )
inline

§ setLockedJoints()

int KDL::ChainJntToJacDotSolver::setLockedJoints ( const std::vector< bool > &  locked_joints)

§ setRepresentation()

void KDL::ChainJntToJacDotSolver::setRepresentation ( const int &  representation)

Sets the internal variable for the representation (with a check on the value)

Parameters
representationThe representation for Jdot : HYBRID,BODYFIXED or INTERTIAL
Returns
void

References BODYFIXED, HYBRID, INTERTIAL, and representation_.

Referenced by setBodyFixedRepresentation(), setHybridRepresentation(), and setInternialRepresentation().

§ strError()

const char * KDL::ChainJntToJacDotSolver::strError ( const int  error) const
virtual

Return a description of the latest error.

Returns
if error is known then a description of error, otherwise "UNKNOWN ERROR"

Reimplemented from KDL::SolverI.

References E_FKSOLVERPOS_FAILED, E_JAC_DOT_FAILED, E_JACSOLVER_FAILED, and KDL::SolverI::strError().

Referenced by setInternialRepresentation().

§ updateInternalDataStructures()

void KDL::ChainJntToJacDotSolver::updateInternalDataStructures ( )
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().

Member Data Documentation

§ BODYFIXED

const int KDL::ChainJntToJacDotSolver::BODYFIXED = 1
static

§ chain

const Chain& KDL::ChainJntToJacDotSolver::chain
private

§ E_FKSOLVERPOS_FAILED

const int KDL::ChainJntToJacDotSolver::E_FKSOLVERPOS_FAILED = -102
static

Referenced by JntToJacDot(), and strError().

§ E_JAC_DOT_FAILED

const int KDL::ChainJntToJacDotSolver::E_JAC_DOT_FAILED = -100
static

Referenced by JntToJacDot(), and strError().

§ E_JACSOLVER_FAILED

const int KDL::ChainJntToJacDotSolver::E_JACSOLVER_FAILED = -101
static

Referenced by JntToJacDot(), and strError().

§ error

int KDL::SolverI::error
protectedinherited

§ F_bs_ee_

Frame KDL::ChainJntToJacDotSolver::F_bs_ee_
private

Referenced by JntToJacDot().

§ fk_solver_

ChainFkSolverPos_recursive KDL::ChainJntToJacDotSolver::fk_solver_
private

§ HYBRID

const int KDL::ChainJntToJacDotSolver::HYBRID = 0
static

§ INTERTIAL

const int KDL::ChainJntToJacDotSolver::INTERTIAL = 2
static

§ jac_

Jacobian KDL::ChainJntToJacDotSolver::jac_
private

§ jac_dot_

Jacobian KDL::ChainJntToJacDotSolver::jac_dot_
private

§ jac_dot_k_

Twist KDL::ChainJntToJacDotSolver::jac_dot_k_
private

Referenced by JntToJacDot().

§ jac_i_

Twist KDL::ChainJntToJacDotSolver::jac_i_
private

§ jac_j_

Twist KDL::ChainJntToJacDotSolver::jac_j_
private

§ jac_solver_

ChainJntToJacSolver KDL::ChainJntToJacDotSolver::jac_solver_
private

§ locked_joints_

std::vector<bool> KDL::ChainJntToJacDotSolver::locked_joints_
private

§ nr_of_unlocked_joints_

unsigned int KDL::ChainJntToJacDotSolver::nr_of_unlocked_joints_
private

Referenced by JntToJacDot(), and setLockedJoints().

§ representation_

int KDL::ChainJntToJacDotSolver::representation_
private

Referenced by JntToJacDot(), and setRepresentation().

§ t_djdq_

Twist KDL::ChainJntToJacDotSolver::t_djdq_
private

The documentation for this class was generated from the following files: