21#ifndef KDL_CHAINJNTTOJACDOTSOLVER_HPP
22#define KDL_CHAINJNTTOJACDOTSOLVER_HPP
129 const unsigned int& joint_idx,
130 const unsigned int& column_idx);
140 const unsigned int& joint_idx,
141 const unsigned int& column_idx);
151 const unsigned int& joint_idx,
152 const unsigned int& column_idx);
163 const unsigned int& joint_idx,
164 const unsigned int& column_idx,
165 const int& representation);
Implementation of a recursive forward position kinematics algorithm to calculate the position transfo...
Definition: chainfksolverpos_recursive.hpp:37
Computes the Jacobian time derivative (Jdot) by calculating the partial derivatives regarding to a jo...
Definition: chainjnttojacdotsolver.hpp:49
Jacobian jac_
Definition: chainjnttojacdotsolver.hpp:172
Twist jac_dot_k_
Definition: chainjnttojacdotsolver.hpp:177
ChainJntToJacSolver jac_solver_
Definition: chainjnttojacdotsolver.hpp:171
static const int E_FKSOLVERPOS_FAILED
Definition: chainjnttojacdotsolver.hpp:53
const Twist & getPartialDerivative(const Jacobian &J, const unsigned int &joint_idx, const unsigned int &column_idx, const int &representation)
Computes .
Definition: chainjnttojacdotsolver.cpp:125
void setHybridRepresentation()
JntToJacDot() will compute in the Hybrid representation (ref Frame: base, ref Point: end-effector)
Definition: chainjnttojacdotsolver.hpp:91
ChainFkSolverPos_recursive fk_solver_
Definition: chainjnttojacdotsolver.hpp:175
virtual const char * strError(const int error) const
Return a description of the latest error.
Definition: chainjnttojacdotsolver.cpp:241
void setInertialRepresentation()
JntToJacDot() will compute in the Inertial representation (ref Frame: base, ref Point: base)
Definition: chainjnttojacdotsolver.hpp:103
std::vector< bool > locked_joints_
Definition: chainjnttojacdotsolver.hpp:169
ChainJntToJacDotSolver(const Chain &chain)
Definition: chainjnttojacdotsolver.cpp:34
static const int HYBRID
Definition: chainjnttojacdotsolver.hpp:56
static const int E_JAC_DOT_FAILED
Definition: chainjnttojacdotsolver.hpp:51
const Twist & getPartialDerivativeHybrid(const Jacobian &bs_J_ee, const unsigned int &joint_idx, const unsigned int &column_idx)
Computes .
Definition: chainjnttojacdotsolver.cpp:144
const Twist & getPartialDerivativeBodyFixed(const Jacobian &ee_J_ee, const unsigned int &joint_idx, const unsigned int &column_idx)
Computes .
Definition: chainjnttojacdotsolver.cpp:175
int representation_
Definition: chainjnttojacdotsolver.hpp:174
virtual ~ChainJntToJacDotSolver()
Definition: chainjnttojacdotsolver.cpp:249
void setBodyFixedRepresentation()
JntToJacDot() will compute in the Body-fixed representation (ref Frame: end-effector,...
Definition: chainjnttojacdotsolver.hpp:97
Jacobian jac_dot_
Definition: chainjnttojacdotsolver.hpp:173
Twist t_djdq_
Definition: chainjnttojacdotsolver.hpp:179
int setLockedJoints(const std::vector< bool > &locked_joints)
Definition: chainjnttojacdotsolver.cpp:227
const Chain & chain
Definition: chainjnttojacdotsolver.hpp:168
static const int BODYFIXED
Definition: chainjnttojacdotsolver.hpp:58
Frame F_bs_ee_
Definition: chainjnttojacdotsolver.hpp:176
virtual void updateInternalDataStructures()
Update the internal data structures.
Definition: chainjnttojacdotsolver.cpp:46
static const int E_JACSOLVER_FAILED
Definition: chainjnttojacdotsolver.hpp:52
Twist jac_i_
Definition: chainjnttojacdotsolver.hpp:178
static const int INERTIAL
Definition: chainjnttojacdotsolver.hpp:60
virtual int JntToJacDot(const KDL::JntArrayVel &q_in, KDL::Twist &jac_dot_q_dot, int seg_nr=-1)
Computes .
Definition: chainjnttojacdotsolver.cpp:55
void setRepresentation(const int &representation)
Sets the internal variable for the representation (with a check on the value)
Definition: chainjnttojacdotsolver.cpp:218
const Twist & getPartialDerivativeInertial(const Jacobian &bs_J_bs, const unsigned int &joint_idx, const unsigned int &column_idx)
Computes .
Definition: chainjnttojacdotsolver.cpp:197
unsigned int nr_of_unlocked_joints_
Definition: chainjnttojacdotsolver.hpp:170
Twist jac_j_
Definition: chainjnttojacdotsolver.hpp:178
Class to calculate the jacobian of a general KDL::Chain, it is used by other solvers.
Definition: chainjnttojacsolver.hpp:39
Definition: frames.hpp:570
Definition: jacobian.hpp:37
Definition: jntarrayvel.hpp:46
Solver interface supporting storage and description of the latest error.
Definition: solveri.hpp:85
int error
Latest error, initialized to E_NOERROR in constructor.
Definition: solveri.hpp:149
represents both translational and rotational velocities.
Definition: frames.hpp:720
Definition: articulatedbodyinertia.cpp:26