30 using namespace Eigen;
33 chain(chain_), nj(chain.getNrOfJoints()), ns(chain.getNrOfSegments()), nc(nc_),
41 Um = MatrixXd::Identity(
nc,
nc);
42 Vm = MatrixXd::Identity(
nc,
nc);
43 Sm = VectorXd::Ones(
nc);
85 for (
unsigned int i = 0; i <
ns; i++)
94 s.
F = segment.
pose(q(j));
132 s.
U = s.
v * (s.
H * s.
v) - FextLocal;
142 for (
int i =
ns; i >= 0; i--)
161 for (
unsigned int r = 0; r < 3; r++)
162 for (
unsigned int c = 0; c <
nc; c++)
165 s.
E_tilde(r, c) = alfa(r + 3, c);
166 s.
E_tilde(r + 3, c) = alfa(r, c);
171 for (
unsigned int c = 0; c <
nc; c++)
175 col = base_to_end*col;
176 s.
E_tilde.col(c) << Vector3d::Map(col.torque.data), Vector3d::Map(col.force.data);
188 PZDPZt.noalias() = vPZ * vPZ.transpose();
201 s.
E_tilde.noalias() -= (vPZ * child.
EZ.transpose()) / child.
D;
206 s.
M.noalias() -= (child.
EZ * child.
EZ.transpose()) / child.
D;
210 Twist CiZDu = child.
C + (child.
Z / child.
D) * child.
u;
212 vCiZDu << Vector3d::Map(CiZDu.rot.data), Vector3d::Map(CiZDu.vel.data);
213 s.
G.noalias() += child.
E.transpose() * vCiZDu;
223 for (
unsigned int c = 0; c <
nc; c++)
257 s.
EZ.noalias() = s.
E.transpose() * vZ;
279 for (
unsigned int i = 0; i <
nc; i++)
304 for (
unsigned int i = 1; i <=
ns; i++)
324 Vector(tmp(0), tmp(1), tmp(2)));
329 double parent_forceProjection = -
dot(s.
Z, parent_force);
330 double parentAccComp = parent_forceProjection / s.
D;
333 constraint_torques(j) = -
dot(s.
Z, constraint_force);
337 total_torques(j) = s.
u + parent_forceProjection + constraint_torques(j);
355 assert(x_dotdot.size() ==
ns + 1);
357 for (
unsigned int i = 1; i <
ns + 1; i++)
371 assert(nu_.size() ==
nu.size());
void final_upwards_sweep(JntArray &q_dotdot, JntArray &constraint_torques)
represents rotations in 3 dimensional space.
Eigen::Matrix< double, 6, 6 > Matrix6d
const Segment & getSegment(unsigned int nr) const
std::vector< Twist > Twists
This class encapsulates a simple segment, that is a "rigid body" (i.e., a frame and a rigid body...
unsigned int rows() const
void getTotalTorque(JntArray &total_tau)
Rotation Inverse() const
Gives back the inverse rotation matrix of *this.
This class encapsulates a serial kinematic interconnection structure. It is built out of segments...
Vector vel
The velocity of that point.
virtual void updateInternalDataStructures()
unsigned int getNrOfSegments() const
This class represents an fixed size array containing joint values of a KDL::Chain.
void initial_upwards_sweep(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const Wrenches &f_ext)
Input size does not match internal state.
Vector torque
Torque that is applied at the origin of the current ref frame.
doubleAcc dot(const VectorAcc &lhs, const VectorAcc &rhs)
unsigned int columns() const
int svd_eigen_HH(const MatrixXd &A, MatrixXd &U, VectorXd &S, MatrixXd &V, VectorXd &tmp, int maxiter, double epsilon)
Rotation M
Orientation of the Frame.
Frame pose(const double &q) const
Twist twist(const double &q, const double &qdot) const
represents both translational and rotational velocities.
6D Inertia of a articulated body
A concrete implementation of a 3 dimensional vector class.
Vector rot
The rotational velocity of that point.
void downwards_sweep(const Jacobian &alfa, const JntArray &ff_torques)
void getTransformedLinkAcceleration(Twists &x_dotdot)
const Joint & getJoint() const
unsigned int getNrOfJoints() const
Frame Inverse() const
Gives back inverse transformation of a Frame.
void getContraintForceMagnitude(Eigen::VectorXd &nu_)
ArticulatedBodyInertia P_tilde
Vector force
Force that is applied at the origin of the current ref frame.
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)
std::vector< Wrench > Wrenches
Eigen::MatrixXd M_0_inverse
Eigen::Matrix< double, 6, 1 > Vector6d
void constraint_calculation(const JntArray &beta)
ChainHdSolver_Vereshchagin(const Chain &chain, const Twist &root_acc, const unsigned int nc)
const double & getInertia() const
represents both translational and rotational acceleration.
Eigen::VectorXd total_torques
int error
Latest error, initialized to E_NOERROR in constructor.
const RigidBodyInertia & getInertia() const
std::vector< segment_info, Eigen::aligned_allocator< segment_info > > results
const JointType & getType() const