Class to calculate the jacobian of a general KDL::Chain, it is used by other solvers. More...
#include <chainjnttojacsolver.hpp>
Public Member Functions | |
ChainJntToJacSolver (const Chain &chain) | |
int | JntToJac (const JntArray &q_in, Jacobian &jac) |
Calculate the jacobian expressed in the base frame of the chain, with reference point at the end effector of the *chain. | |
int | setLockedJoints (const std::vector< bool > locked_joints) |
~ChainJntToJacSolver () |
Class to calculate the jacobian of a general KDL::Chain, it is used by other solvers.
It should not be used outside of KDL.
Definition at line 40 of file chainjnttojacsolver.hpp.
KDL::ChainJntToJacSolver::ChainJntToJacSolver | ( | const Chain & | chain | ) |
Definition at line 26 of file chainjnttojacsolver.cpp.
KDL::ChainJntToJacSolver::~ChainJntToJacSolver | ( | ) |
Definition at line 32 of file chainjnttojacsolver.cpp.
Calculate the jacobian expressed in the base frame of the chain, with reference point at the end effector of the *chain.
The alghoritm is similar to the one used in KDL::ChainFkSolverVel_recursive
q_in | input joint positions | |
jac | output jacobian |
Definition at line 48 of file chainjnttojacsolver.cpp.
References KDL::changeRefPoint(), KDL::Jacobian::columns(), KDL::Segment::getJoint(), KDL::Chain::getNrOfJoints(), KDL::Chain::getNrOfSegments(), KDL::Chain::getSegment(), KDL::Joint::getType(), KDL::Frame::Identity(), k, KDL::Frame::M, KDL::Joint::None, KDL::Frame::p, KDL::Segment::pose(), KDL::JntArray::rows(), KDL::Jacobian::setColumn(), KDL::SetToZero(), and KDL::Segment::twist().
Referenced by KDL::ChainIkSolverVel_wdls::CartToJnt(), KDL::ChainIkSolverVel_pinv_nso::CartToJnt(), KDL::ChainIkSolverVel_pinv_givens::CartToJnt(), and KDL::ChainIkSolverVel_pinv::CartToJnt().
int KDL::ChainJntToJacSolver::setLockedJoints | ( | const std::vector< bool > | locked_joints | ) |
Definition at line 36 of file chainjnttojacsolver.cpp.