Go to the documentation of this file.
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);
Frame F_bs_ee_
Definition: chainjnttojacdotsolver.hpp:176
unsigned int columns() const
Definition: jacobian.cpp:75
static const int E_FKSOLVERPOS_FAILED
Definition: chainjnttojacdotsolver.hpp:53
virtual const char * strError(const int error) const
Return a description of the latest error.
Definition: solveri.hpp:125
int representation_
Definition: chainjnttojacdotsolver.hpp:174
Vector rot
The rotational velocity of that point.
Definition: frames.hpp:723
int setLockedJoints(const std::vector< bool > &locked_joints)
Definition: chainjnttojacdotsolver.cpp:219
void setColumn(unsigned int i, const Twist &t)
Definition: jacobian.cpp:149
Twist jac_j_
Definition: chainjnttojacdotsolver.hpp:178
static const int E_JACSOLVER_FAILED
Definition: chainjnttojacdotsolver.hpp:52
friend bool changeBase(const Jacobian &src1, const Rotation &rot, Jacobian &dest)
Definition: jacobian.cpp:104
void setBodyFixedRepresentation()
JntToJacDot() will compute in the Body-fixed representation (ref Frame: end-effector,...
Definition: chainjnttojacdotsolver.hpp:97
static const int BODYFIXED
Definition: chainjnttojacdotsolver.hpp:58
Twist jac_i_
Definition: chainjnttojacdotsolver.hpp:178
Rotation Inverse() const
Gives back the inverse rotation matrix of *this.
Definition: frames.inl:632
const Joint & getJoint() const
Request the joint of the segment.
Definition: segment.hpp:118
Vector p
origine of the Frame
Definition: frames.hpp:572
@ E_NOT_UP_TO_DATE
Chain size changed.
Definition: solveri.hpp:97
ChainJntToJacDotSolver(const Chain &chain)
Definition: chainjnttojacdotsolver.cpp:26
virtual ~ChainJntToJacDotSolver()
Definition: chainjnttojacdotsolver.cpp:241
Definition: articulatedbodyinertia.cpp:28
unsigned int getNrOfSegments() const
Request the total number of segments in the chain.
Definition: chain.hpp:76
friend bool changeRefPoint(const Jacobian &src1, const Vector &base_AB, Jacobian &dest)
Definition: jacobian.cpp:90
void setHybridRepresentation()
JntToJacDot() will compute in the Hybrid representation (ref Frame: base, ref Point: end-effector)
Definition: chainjnttojacdotsolver.hpp:91
virtual const char * strError(const int error) const
Return a description of the latest error.
Definition: chainjnttojacdotsolver.cpp:233
unsigned int nr_of_unlocked_joints_
Definition: chainjnttojacdotsolver.hpp:170
void resize(unsigned int newNrOfColumns)
Allocates memory for new size (can break realtime behavior)
Definition: jacobian.cpp:55
represents both translational and rotational velocities.
Definition: frames.hpp:720
@ E_OUT_OF_RANGE
Requested index out of range.
Definition: solveri.hpp:103
unsigned int rows() const
Returns the number of rows (size) of the array.
Definition: jntarray.cpp:72
virtual int JntToJac(const JntArray &q_in, Jacobian &jac, int segmentNR=-1)
Calculate the jacobian expressed in the base frame of the chain, with reference point at the end effe...
Definition: chainjnttojacsolver.cpp:48
std::vector< bool > locked_joints_
Definition: chainjnttojacdotsolver.hpp:169
Computes the Jacobian time derivative (Jdot) by calculating the partial derivatives regarding to a jo...
Definition: chainjnttojacdotsolver.hpp:49
const Segment & getSegment(unsigned int nr) const
Request the nr'd segment of the chain.
Definition: chain.cpp:68
@ E_SIZE_MISMATCH
Input size does not match internal state.
Definition: solveri.hpp:99
ChainJntToJacSolver jac_solver_
Definition: chainjnttojacdotsolver.hpp:171
Definition: frames.hpp:570
virtual int JntToCart(const JntArray &q_in, Frame &p_out, int segmentNr=-1)
Calculate forward position kinematics for a KDL::Chain, from joint coordinates to cartesian pose.
Definition: chainfksolverpos_recursive.cpp:33
Jacobian jac_dot_
Definition: chainjnttojacdotsolver.hpp:173
static const int INTERTIAL
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:47
virtual void updateInternalDataStructures()
Update the internal data structures.
Definition: chainjnttojacdotsolver.cpp:38
@ None
Definition: joint.hpp:47
const Chain & chain
Definition: chainjnttojacdotsolver.hpp:168
static const int HYBRID
Definition: chainjnttojacdotsolver.hpp:56
virtual void updateInternalDataStructures()
Update the internal data structures.
Definition: chainjnttojacsolver.cpp:31
Vector vel
The velocity of that point.
Definition: frames.hpp:722
Definition: jntarrayvel.hpp:46
ChainFkSolverPos_recursive fk_solver_
Definition: chainjnttojacdotsolver.hpp:175
void setInternialRepresentation()
JntToJacDot() will compute in the Inertial representation (ref Frame: base, ref Point: base)
Definition: chainjnttojacdotsolver.hpp:103
Solver interface supporting storage and description of the latest error.
Definition: solveri.hpp:85
Twist jac_dot_k_
Definition: chainjnttojacdotsolver.hpp:177
const Twist & getPartialDerivative(const Jacobian &J, const unsigned int &joint_idx, const unsigned int &column_idx, const int &representation)
Computes .
Definition: chainjnttojacdotsolver.cpp:117
Rotation M
Orientation of the Frame.
Definition: frames.hpp:573
JntArray qdot
Definition: jntarrayvel.hpp:49
Jacobian jac_
Definition: chainjnttojacdotsolver.hpp:172
virtual void updateInternalDataStructures()
Update the internal data structures.
Definition: chainfksolverpos_recursive.hpp:45
const Twist & getPartialDerivativeInertial(const Jacobian &bs_J_bs, const unsigned int &joint_idx, const unsigned int &column_idx)
Computes .
Definition: chainjnttojacdotsolver.cpp:189
const JointType & getType() const
Request the type of the joint.
Definition: joint.hpp:159
const Twist & getPartialDerivativeBodyFixed(const Jacobian &ee_J_ee, const unsigned int &joint_idx, const unsigned int &column_idx)
Computes .
Definition: chainjnttojacdotsolver.cpp:167
Class to calculate the jacobian of a general KDL::Chain, it is used by other solvers.
Definition: chainjnttojacsolver.hpp:42
void SetToZero(Jacobian &jac)
Definition: jacobian.cpp:80
JntArray q
Definition: jntarrayvel.hpp:48
@ E_NOERROR
No error.
Definition: solveri.hpp:91
Implementation of a recursive forward position kinematics algorithm to calculate the position transfo...
Definition: chainfksolverpos_recursive.hpp:37
void MultiplyJacobian(const Jacobian &jac, const JntArray &src, Twist &dest)
Function to multiply a KDL::Jacobian with a KDL::JntArray to get a KDL::Twist, it should not be used ...
Definition: jntarray.cpp:102
Twist getColumn(unsigned int i) const
Definition: jacobian.cpp:145
Twist t_djdq_
Definition: chainjnttojacdotsolver.hpp:179
const Twist & getPartialDerivativeHybrid(const Jacobian &bs_J_ee, const unsigned int &joint_idx, const unsigned int &column_idx)
Computes .
Definition: chainjnttojacdotsolver.cpp:136
unsigned int getNrOfJoints() const
Request the total number of joints in the chain.
Definition: chain.hpp:71
static const int E_JAC_DOT_FAILED
Definition: chainjnttojacdotsolver.hpp:51
int error
Latest error, initialized to E_NOERROR in constructor.
Definition: solveri.hpp:149
void setRepresentation(const int &representation)
Sets the internal variable for the representation (with a check on the value)
Definition: chainjnttojacdotsolver.cpp:210
Definition: jacobian.hpp:37