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 <chainjnttojacdotsolver.hpp>
Public Member Functions | |
ChainJntToJacDotSolver (const Chain &chain) | |
virtual int | JntToJacDot (const KDL::JntArrayVel &q_in, KDL::Twist &jac_dot_q_dot, int seg_nr=-1) |
Computes . | |
virtual int | JntToJacDot (const KDL::JntArrayVel &q_in, KDL::Jacobian &jdot, int seg_nr=-1) |
Computes . | |
void | setBodyFixedRepresentation () |
JntToJacDot() will compute in the Body-fixed representation (ref Frame: end-effector, ref Point: end-effector) | |
void | setHybridRepresentation () |
JntToJacDot() will compute in the Hybrid representation (ref Frame: base, ref Point: end-effector) | |
void | setInertialRepresentation () |
JntToJacDot() will compute in the Inertial representation (ref Frame: base, ref Point: base) | |
int | setLockedJoints (const std::vector< bool > &locked_joints) |
void | setRepresentation (const int &representation) |
Sets the internal variable for the representation (with a check on the value) | |
virtual const char * | strError (const int error) const |
virtual void | updateInternalDataStructures () |
virtual | ~ChainJntToJacDotSolver () |
Static Public Attributes | |
static const int | BODYFIXED = 1 |
static const int | E_FKSOLVERPOS_FAILED = -102 |
static const int | E_JAC_DOT_FAILED = -100 |
static const int | E_JACSOLVER_FAILED = -101 |
static const int | HYBRID = 0 |
static const int | INERTIAL = 2 |
Protected Member Functions | |
const Twist & | getPartialDerivative (const Jacobian &J, const unsigned int &joint_idx, const unsigned int &column_idx, const int &representation) |
Computes . | |
const Twist & | getPartialDerivativeBodyFixed (const Jacobian &ee_J_ee, const unsigned int &joint_idx, const unsigned int &column_idx) |
Computes . | |
const Twist & | getPartialDerivativeHybrid (const Jacobian &bs_J_ee, const unsigned int &joint_idx, const unsigned int &column_idx) |
Computes . | |
const Twist & | getPartialDerivativeInertial (const Jacobian &bs_J_bs, const unsigned int &joint_idx, const unsigned int &column_idx) |
Computes . | |
Private Attributes | |
const Chain & | chain |
Frame | F_bs_ee_ |
ChainFkSolverPos_recursive | fk_solver_ |
Jacobian | jac_ |
Jacobian | jac_dot_ |
Twist | jac_dot_k_ |
Twist | jac_i_ |
Twist | jac_j_ |
ChainJntToJacSolver | jac_solver_ |
std::vector< bool > | locked_joints_ |
unsigned int | nr_of_unlocked_joints_ |
int | representation_ |
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
Definition at line 48 of file chainjnttojacdotsolver.hpp.
KDL::ChainJntToJacDotSolver::ChainJntToJacDotSolver | ( | const Chain & | chain | ) | [explicit] |
Definition at line 26 of file chainjnttojacdotsolver.cpp.
KDL::ChainJntToJacDotSolver::~ChainJntToJacDotSolver | ( | ) | [virtual] |
Definition at line 241 of file chainjnttojacdotsolver.cpp.
const Twist & KDL::ChainJntToJacDotSolver::getPartialDerivative | ( | const Jacobian & | J, |
const unsigned int & | joint_idx, | ||
const unsigned int & | column_idx, | ||
const int & | representation | ||
) | [protected] |
Computes .
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 |
Definition at line 117 of file chainjnttojacdotsolver.cpp.
const Twist & KDL::ChainJntToJacDotSolver::getPartialDerivativeBodyFixed | ( | const Jacobian & | ee_J_ee, |
const unsigned int & | joint_idx, | ||
const unsigned int & | column_idx | ||
) | [protected] |
Computes .
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) |
Definition at line 167 of file chainjnttojacdotsolver.cpp.
const Twist & KDL::ChainJntToJacDotSolver::getPartialDerivativeHybrid | ( | const Jacobian & | bs_J_ee, |
const unsigned int & | joint_idx, | ||
const unsigned int & | column_idx | ||
) | [protected] |
Computes .
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) |
Definition at line 136 of file chainjnttojacdotsolver.cpp.
const Twist & KDL::ChainJntToJacDotSolver::getPartialDerivativeInertial | ( | const Jacobian & | bs_J_bs, |
const unsigned int & | joint_idx, | ||
const unsigned int & | column_idx | ||
) | [protected] |
Computes .
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) |
Definition at line 189 of file chainjnttojacdotsolver.cpp.
int KDL::ChainJntToJacDotSolver::JntToJacDot | ( | const KDL::JntArrayVel & | q_in, |
KDL::Twist & | jac_dot_q_dot, | ||
int | seg_nr = -1 |
||
) | [virtual] |
Computes .
q_in | Current joint positions and velocities |
jac_dot_q_dot | The twist representing Jdot*qdot |
seg_nr | The final segment to compute |
Definition at line 47 of file chainjnttojacdotsolver.cpp.
int KDL::ChainJntToJacDotSolver::JntToJacDot | ( | const KDL::JntArrayVel & | q_in, |
KDL::Jacobian & | jdot, | ||
int | seg_nr = -1 |
||
) | [virtual] |
Computes .
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 |
Definition at line 57 of file chainjnttojacdotsolver.cpp.
void KDL::ChainJntToJacDotSolver::setBodyFixedRepresentation | ( | ) | [inline] |
JntToJacDot() will compute in the Body-fixed representation (ref Frame: end-effector, ref Point: end-effector)
Definition at line 97 of file chainjnttojacdotsolver.hpp.
void KDL::ChainJntToJacDotSolver::setHybridRepresentation | ( | ) | [inline] |
JntToJacDot() will compute in the Hybrid representation (ref Frame: base, ref Point: end-effector)
Definition at line 91 of file chainjnttojacdotsolver.hpp.
void KDL::ChainJntToJacDotSolver::setInertialRepresentation | ( | ) | [inline] |
JntToJacDot() will compute in the Inertial representation (ref Frame: base, ref Point: base)
Definition at line 103 of file chainjnttojacdotsolver.hpp.
int KDL::ChainJntToJacDotSolver::setLockedJoints | ( | const std::vector< bool > & | locked_joints | ) |
Definition at line 219 of file chainjnttojacdotsolver.cpp.
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 INERTIAL |
Definition at line 210 of file chainjnttojacdotsolver.cpp.
const char * KDL::ChainJntToJacDotSolver::strError | ( | const int | error | ) | const [virtual] |
Return a description of the latest error
Reimplemented from KDL::SolverI.
Definition at line 233 of file chainjnttojacdotsolver.cpp.
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.
Definition at line 38 of file chainjnttojacdotsolver.cpp.
const int KDL::ChainJntToJacDotSolver::BODYFIXED = 1 [static] |
Definition at line 58 of file chainjnttojacdotsolver.hpp.
const Chain& KDL::ChainJntToJacDotSolver::chain [private] |
Definition at line 168 of file chainjnttojacdotsolver.hpp.
const int KDL::ChainJntToJacDotSolver::E_FKSOLVERPOS_FAILED = -102 [static] |
Definition at line 53 of file chainjnttojacdotsolver.hpp.
const int KDL::ChainJntToJacDotSolver::E_JAC_DOT_FAILED = -100 [static] |
Definition at line 51 of file chainjnttojacdotsolver.hpp.
const int KDL::ChainJntToJacDotSolver::E_JACSOLVER_FAILED = -101 [static] |
Definition at line 52 of file chainjnttojacdotsolver.hpp.
Frame KDL::ChainJntToJacDotSolver::F_bs_ee_ [private] |
Definition at line 176 of file chainjnttojacdotsolver.hpp.
Definition at line 175 of file chainjnttojacdotsolver.hpp.
const int KDL::ChainJntToJacDotSolver::HYBRID = 0 [static] |
Definition at line 56 of file chainjnttojacdotsolver.hpp.
const int KDL::ChainJntToJacDotSolver::INERTIAL = 2 [static] |
Definition at line 60 of file chainjnttojacdotsolver.hpp.
Jacobian KDL::ChainJntToJacDotSolver::jac_ [private] |
Definition at line 172 of file chainjnttojacdotsolver.hpp.
Definition at line 173 of file chainjnttojacdotsolver.hpp.
Twist KDL::ChainJntToJacDotSolver::jac_dot_k_ [private] |
Definition at line 177 of file chainjnttojacdotsolver.hpp.
Twist KDL::ChainJntToJacDotSolver::jac_i_ [private] |
Definition at line 178 of file chainjnttojacdotsolver.hpp.
Twist KDL::ChainJntToJacDotSolver::jac_j_ [private] |
Definition at line 178 of file chainjnttojacdotsolver.hpp.
Definition at line 171 of file chainjnttojacdotsolver.hpp.
std::vector<bool> KDL::ChainJntToJacDotSolver::locked_joints_ [private] |
Definition at line 169 of file chainjnttojacdotsolver.hpp.
unsigned int KDL::ChainJntToJacDotSolver::nr_of_unlocked_joints_ [private] |
Definition at line 170 of file chainjnttojacdotsolver.hpp.
int KDL::ChainJntToJacDotSolver::representation_ [private] |
Definition at line 174 of file chainjnttojacdotsolver.hpp.
Twist KDL::ChainJntToJacDotSolver::t_djdq_ [private] |
Definition at line 179 of file chainjnttojacdotsolver.hpp.