Go to the documentation of this file.
29 tree(tree_), nj(tree.getNrOfJoints()), ns(tree.getNrOfSegments())
43 for(SegmentMap::const_iterator seg = segments.
begin(); seg != segments.
end(); seg++) {
84 double q_, qdot_, qdotdot_;
89 qdotdot_ = q_dotdot(j);
92 q_ = qdot_ = qdotdot_ = 0.0;
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);
unsigned int getNrOfJoints() const
SegmentMap::const_iterator getRootSegment() const
const Joint & getJoint() 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.
std::map< std::string, Twist > S
TreeIdSolver_RNE(const Tree &tree, Vector grav)
@ E_NOT_UP_TO_DATE
Chain size changed.
std::map< std::string, Wrench > f
6D Inertia of a rigid body
#define GetTreeElementParent(tree_element)
#define GetTreeElementChildren(tree_element)
A concrete implementation of a 3 dimensional vector class.
std::map< std::string, Twist > v
std::map< std::string, Frame > X
represents both translational and rotational velocities.
const SegmentMap & getSegments() const
unsigned int getNrOfSegments() const
unsigned int rows() const
doubleAcc dot(const VectorAcc &lhs, const VectorAcc &rhs)
@ E_SIZE_MISMATCH
Input size does not match internal state.
represents both translational and rotational acceleration.
Frame pose(const double &q) const
represents a frame transformation in 3D space (rotation + translation)
void initAuxVariables()
Helper function to initialize private members X, S, v, a, f.
const double & getInertia() const
Twist twist(const double &q, const double &qdot) const
This class encapsulates a tree kinematic interconnection structure. It is built out of segments.
This class encapsulates a simple segment, that is a "rigid body" (i.e., a frame and a rigid body ine...
const JointType & getType() const
virtual void updateInternalDataStructures()
#define GetTreeElementSegment(tree_element)
std::map< std::string, Twist > a
const RigidBodyInertia & getInertia() const
int CartToJnt(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const WrenchMap &f_ext, JntArray &torques)
int error
Latest error, initialized to E_NOERROR in constructor.
#define GetTreeElementQNr(tree_element)