28 chain(chain_),nj(chain.getNrOfJoints()),ns(chain.getNrOfSegments()),
29 X(ns),S(ns),v(ns),a(ns),f(ns)
55 for(
unsigned int i=0;i<
ns;i++){
56 double q_,qdot_,qdotdot_;
63 q_=qdot_=qdotdot_=0.0;
76 a[i]=
X[i].Inverse(
ag)+
S[i]*qdotdot_+
v[i]*vj;
78 v[i]=
X[i].Inverse(
v[i-1])+vj;
79 a[i]=
X[i].Inverse(
a[i-1])+
S[i]*qdotdot_+
v[i]*vj;
84 f[i]=Ii*
a[i]+
v[i]*(Ii*
v[i])-f_ext[i];
89 for(
int i=ns-1;i>=0;i--){
91 torques(j)=
dot(
S[i],
f[i]);
96 f[i-1]=
f[i-1]+
X[i]*
f[i];
virtual void updateInternalDataStructures()
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...
ChainIdSolver_RNE(const Chain &chain, Vector grav)
unsigned int getNrOfSegments() const
This class represents an fixed size array containing joint values of a KDL::Chain.
6D Inertia of a rigid body
Input size does not match internal state.
doubleAcc dot(const VectorAcc &lhs, const VectorAcc &rhs)
Frame pose(const double &q) const
Twist twist(const double &q, const double &qdot) const
represents both translational and rotational velocities.
A concrete implementation of a 3 dimensional vector class.
const Joint & getJoint() const
int CartToJnt(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const Wrenches &f_ext, JntArray &torques)
unsigned int getNrOfJoints() const
std::vector< Wrench > Wrenches
const double & getInertia() const
int error
Latest error, initialized to E_NOERROR in constructor.
const RigidBodyInertia & getInertia() const
const JointType & getType() const