Go to the documentation of this file.
32 chain(chain_), nj(chain.getNrOfJoints()), ns(chain.getNrOfSegments()), nc(nc_),
40 Um = Eigen::MatrixXd::Identity(
nc,
nc);
41 Vm = Eigen::MatrixXd::Identity(
nc,
nc);
42 Sm = Eigen::VectorXd::Ones(
nc);
43 tmpm = Eigen::VectorXd::Ones(
nc);
84 for (
unsigned int i = 0; i <
ns; i++)
93 s.
F = segment.
pose(q(j));
131 s.
U = s.
v * (s.
H * s.
v) - FextLocal;
141 for (
int i =
ns; i >= 0; i--)
160 for (
unsigned int r = 0; r < 3; r++)
161 for (
unsigned int c = 0; c <
nc; c++)
164 s.
E_tilde(r, c) = alfa(r + 3, c);
165 s.
E_tilde(r + 3, c) = alfa(r, c);
170 for (
unsigned int c = 0; c <
nc; c++)
174 col = base_to_end*col;
175 s.
E_tilde.col(c) << Eigen::Vector3d::Map(col.torque.data), Eigen::Vector3d::Map(col.force.data);
187 PZDPZt.noalias() = vPZ * vPZ.transpose();
200 s.
E_tilde.noalias() -= (vPZ * child.
EZ.transpose()) / child.
D;
205 s.
M.noalias() -= (child.
EZ * child.
EZ.transpose()) / child.
D;
209 Twist CiZDu = child.
C + (child.
Z / child.
D) * child.
u;
211 vCiZDu << Eigen::Vector3d::Map(CiZDu.rot.data), Eigen::Vector3d::Map(CiZDu.vel.data);
212 s.
G.noalias() += child.
E.transpose() * vCiZDu;
222 for (
unsigned int c = 0; c <
nc; c++)
255 vZ << Eigen::Vector3d::Map(s.
Z.
rot.
data), Eigen::Vector3d::Map(s.
Z.
vel.
data);
256 s.
EZ.noalias() = s.
E.transpose() * vZ;
278 for (
unsigned int i = 0; i <
nc; i++)
303 for (
unsigned int i = 1; i <=
ns; i++)
323 Vector(tmp(0), tmp(1), tmp(2)));
328 double parent_forceProjection = -
dot(s.
Z, parent_force);
329 double parentAccComp = parent_forceProjection / s.
D;
332 constraint_torques(j) = -
dot(s.
Z, constraint_force);
336 total_torques(j) = s.
u + parent_forceProjection + constraint_torques(j);
354 assert(x_dotdot.
size() ==
ns + 1);
356 for (
unsigned int i = 1; i <
ns + 1; i++)
370 assert(nu_.size() ==
nu.size());
unsigned int columns() const
Vector rot
The rotational velocity of that point.
Frame Inverse() const
Gives back inverse transformation of a Frame.
void downwards_sweep(const Jacobian &alfa, const JntArray &ff_torques)
ChainHdSolver_Vereshchagin(const Chain &chain, const Twist &root_acc, const unsigned int nc)
void initial_upwards_sweep(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const Wrenches &f_ext)
Rotation Inverse() const
Gives back the inverse rotation matrix of *this.
const Joint & getJoint() const
int svd_eigen_HH(const Eigen::MatrixXd &A, Eigen::MatrixXd &U, Eigen::VectorXd &S, Eigen::MatrixXd &V, Eigen::VectorXd &tmp, int maxiter, double epsilon)
6D Inertia of a articulated body
@ E_NOT_UP_TO_DATE
Chain size changed.
Eigen::VectorXd total_torques
void getTotalTorque(JntArray &total_tau)
unsigned int getNrOfSegments() const
A concrete implementation of a 3 dimensional vector class.
virtual void updateInternalDataStructures()
represents both translational and rotational velocities.
Eigen::MatrixXd M_0_inverse
unsigned int rows() const
Vector torque
Torque that is applied at the origin of the current ref frame.
doubleAcc dot(const VectorAcc &lhs, const VectorAcc &rhs)
const Segment & getSegment(unsigned int nr) const
@ E_SIZE_MISMATCH
Input size does not match internal state.
void getContraintForceMagnitude(Eigen::VectorXd &nu_)
represents both translational and rotational acceleration.
void constraint_calculation(const JntArray &beta)
Frame pose(const double &q) const
Eigen::Matrix< double, 6, 6 > Matrix6d
const double & getInertia() const
Twist twist(const double &q, const double &qdot) const
Vector vel
The velocity of that point.
This class encapsulates a simple segment, that is a "rigid body" (i.e., a frame and a rigid body ine...
std::vector< segment_info, Eigen::aligned_allocator< segment_info > > results
Rotation M
Orientation of the Frame.
void final_upwards_sweep(JntArray &q_dotdot, JntArray &constraint_torques)
ArticulatedBodyInertia P_tilde
const JointType & getType() const
Eigen::Matrix< double, 6, 1 > Vector6d
Vector force
Force that is applied at the origin of the current ref frame.
This class encapsulates a serial kinematic interconnection structure. It is built out of segments.
const RigidBodyInertia & getInertia() const
int CartToJnt(const JntArray &q, const JntArray &q_dot, JntArray &q_dotdot, const Jacobian &alfa, const JntArray &beta, const Wrenches &f_ext, const JntArray &ff_torques, JntArray &constraint_torques)
unsigned int getNrOfJoints() const
int error
Latest error, initialized to E_NOERROR in constructor.
void getTransformedLinkAcceleration(Twists &x_dotdot)
represents rotations in 3 dimensional space.