29 tree(tree_), nj(tree.getNrOfJoints()), ns(tree.getNrOfSegments())
43 for(SegmentMap::const_iterator seg = segments.begin(); seg != segments.end(); seg++) {
66 catch(
const std::out_of_range&) {
80 const std::string& segname = segment->first;
84 double q_, qdot_, qdotdot_;
89 qdotdot_ = q_dotdot(j);
92 q_ = qdot_ = qdotdot_ = 0.0;
97 X.at(segname) = seg.
pose(q_);
100 Twist vj =
X.at(segname).M.Inverse( seg.
twist(q_,qdot_) );
101 S.at(segname) =
X.at(segname).M.Inverse( seg.
twist(q_,1.0) );
106 a.at(segname) =
X.at(segname).Inverse(
ag) +
S.at(segname)*qdotdot_+
v.at(segname)*vj;
109 v.at(segname) =
X.at(segname).Inverse(
v.at(parname)) + vj;
110 a.at(segname) =
X.at(segname).Inverse(
a.at(parname)) +
S.at(segname)*qdotdot_ +
v.at(segname)*vj;
116 f.at(segname) = I*
a.at(segname) +
v.at(segname)*(I*
v.at(segname));
117 if(f_ext.find(segname) != f_ext.end())
118 f.at(segname) =
f.at(segname) - f_ext.at(segname);
121 SegmentMap::const_iterator child;
124 rne_step(child, q, q_dot, q_dotdot, f_ext, torques);
131 torques(j) =
dot(
S.at(segname),
f.at(segname));
137 f.at(parname) =
f.at(parname) +
X.at(segname)*
f.at(segname);
#define GetTreeElementSegment(tree_element)
std::map< std::string, Twist > S
int CartToJnt(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const WrenchMap &f_ext, JntArray &torques)
This class encapsulates a simple segment, that is a "rigid body" (i.e., a frame and a rigid body...
unsigned int rows() const
#define GetTreeElementQNr(tree_element)
std::map< std::string, Frame > X
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.
std::map< std::string, TreeElement > SegmentMap
std::map< std::string, Twist > a
doubleAcc dot(const VectorAcc &lhs, const VectorAcc &rhs)
#define GetTreeElementChildren(tree_element)
#define GetTreeElementParent(tree_element)
Frame pose(const double &q) const
std::map< std::string, Wrench > WrenchMap
Twist twist(const double &q, const double &qdot) const
represents both translational and rotational velocities.
A concrete implementation of a 3 dimensional vector class.
std::map< std::string, Twist > v
const Joint & getJoint() const
TreeIdSolver_RNE(const Tree &tree, Vector grav)
void initAuxVariables()
Helper function to initialize private members X, S, v, a, f.
const SegmentMap & getSegments() const
void rne_step(SegmentMap::const_iterator segment, const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const WrenchMap &f_ext, JntArray &torques)
One recursion step.
SegmentMap::const_iterator getRootSegment() const
virtual void updateInternalDataStructures()
std::map< std::string, Wrench > f
unsigned int getNrOfSegments() const
represents a frame transformation in 3D space (rotation + translation)
const double & getInertia() const
represents both translational and rotational acceleration.
int error
Latest error, initialized to E_NOERROR in constructor.
unsigned int getNrOfJoints() const
This class encapsulates a tree kinematic interconnection structure. It is built out of segments...
const RigidBodyInertia & getInertia() const
const JointType & getType() const