27 chain(_chain),locked_joints_(chain.getNrOfJoints(),false)
52 unsigned int segmentNr;
71 for (
unsigned int i=0;i<segmentNr;i++) {
const Segment & getSegment(unsigned int nr) const
unsigned int rows() const
This class encapsulates a serial kinematic interconnection structure. It is built out of segments...
unsigned int getNrOfSegments() const
This class represents an fixed size array containing joint values of a KDL::Chain.
Input size does not match internal state.
bool changeRefPoint(const Jacobian &src1, const Vector &base_AB, Jacobian &dest)
std::vector< bool > locked_joints_
unsigned int columns() const
Rotation M
Orientation of the Frame.
Frame pose(const double &q) const
Twist twist(const double &q, const double &qdot) const
IMETHOD void SetToZero(Vector &v)
int setLockedJoints(const std::vector< bool > locked_joints)
virtual void updateInternalDataStructures()
Requested index out of range.
const Joint & getJoint() const
void setColumn(unsigned int i, const Twist &t)
Vector p
origine of the Frame
unsigned int getNrOfJoints() const
virtual ~ChainJntToJacSolver()
represents a frame transformation in 3D space (rotation + translation)
ChainJntToJacSolver(const Chain &chain)
virtual int JntToJac(const JntArray &q_in, Jacobian &jac, int seg_nr=-1)
int error
Latest error, initialized to E_NOERROR in constructor.
const JointType & getType() const