void test_qtvector()
{
// some non vectorizable fixed sizes
CALL_SUBTEST(check_qtvector_matrix(Vector2f()));
CALL_SUBTEST(check_qtvector_matrix(Matrix3f()));
CALL_SUBTEST(check_qtvector_matrix(Matrix3d()));
// some vectorizable fixed sizes
CALL_SUBTEST(check_qtvector_matrix(Matrix2f()));
CALL_SUBTEST(check_qtvector_matrix(Vector4f()));
CALL_SUBTEST(check_qtvector_matrix(Matrix4f()));
CALL_SUBTEST(check_qtvector_matrix(Matrix4d()));
// some dynamic sizes
CALL_SUBTEST(check_qtvector_matrix(MatrixXd(1,1)));
CALL_SUBTEST(check_qtvector_matrix(VectorXd(20)));
CALL_SUBTEST(check_qtvector_matrix(RowVectorXf(20)));
CALL_SUBTEST(check_qtvector_matrix(MatrixXcf(10,10)));
// some Transform
CALL_SUBTEST(check_qtvector_transform(Transform2f()));
CALL_SUBTEST(check_qtvector_transform(Transform3f()));
CALL_SUBTEST(check_qtvector_transform(Transform3d()));
//CALL_SUBTEST(check_qtvector_transform(Transform4d()));
// some Quaternion
CALL_SUBTEST(check_qtvector_quaternion(Quaternionf()));
CALL_SUBTEST(check_qtvector_quaternion(Quaternionf()));
}
void relativeTransform2(const Transform3f& tf1, const Transform3f& tf2,
Transform3f& tf)
{
const Quaternion3f& q1inv = fcl::conj(tf1.getQuatRotation());
const Quaternion3f& q2_q1inv = tf2.getQuatRotation() * q1inv;
tf = Transform3f(q2_q1inv, tf2.getTranslation() - q2_q1inv.transform(tf1.getTranslation()));
}
void cmuk::cacheTransforms(const KState& q, XformCache* c) const {
c->absXforms[0] = c->relXforms[0] = Transform3f(q.body_rot, q.body_pos);
const bool& cfik = _centeredFootIK;
size_t fidx = 0;
for (int leg=0; leg<NUM_LEGS; ++leg) {
++fidx;
c->relXforms[fidx] = Transform3f::rx(q.leg_rot[leg][0],
jo(_kc, leg, HIP_RX_OFFSET, cfik));
c->absXforms[fidx] = c->absXforms[FRAME_TRUNK] * c->relXforms[fidx];
++fidx;
c->relXforms[fidx] = Transform3f::ry(q.leg_rot[leg][1],
jo(_kc, leg, HIP_RY_OFFSET, cfik));
c->absXforms[fidx] = c->absXforms[fidx-1] * c->relXforms[fidx];
++fidx;
c->relXforms[fidx] = Transform3f::ry(q.leg_rot[leg][2],
jo(_kc, leg, KNEE_RY_OFFSET, cfik));
c->absXforms[fidx] = c->absXforms[fidx-1] * c->relXforms[fidx];
++fidx;
c->relXforms[fidx] = Transform3f(quatf(0.0f, 0.0f, 0.0f, 1.0f),
jo(_kc, leg, FOOT_OFFSET, cfik));
c->absXforms[fidx] = c->absXforms[fidx-1] * c->relXforms[fidx];
}
}
CMUK_ERROR_CODE cmuk::getAbsTransform( const cmuk::XformCache& cache,
FrameIndex frame,
Transform3f* xform ) const {
if (!xform) { return CMUK_INSUFFICIENT_ARGUMENTS; }
if (frame == FRAME_GROUND) {
*xform = Transform3f();
} else {
if ((int)frame >= NUM_FRAMES) {
return CMUK_BAD_FRAME_INDEX;
}
*xform = cache.absXforms[frame];
}
return CMUK_OKAY;
}
CMUK_ERROR_CODE cmuk::computeLegTransforms( LegIndex leg,
const vec3f& q,
Transform3f t[4] ) const {
if ((int)leg < 0 || (int)leg >= NUM_LEGS) {
return CMUK_BAD_LEG_INDEX;
}
if (!t) {
return CMUK_INSUFFICIENT_ARGUMENTS;
}
t[0] = Transform3f::rx(q[0], jo(_kc, leg, HIP_RX_OFFSET, _centeredFootIK));
t[1] = t[0] * Transform3f::ry(q[1], jo(_kc, leg, HIP_RY_OFFSET, _centeredFootIK));
t[2] = t[1] * Transform3f::ry(q[2], jo(_kc, leg, KNEE_RY_OFFSET, _centeredFootIK));
t[3] = t[2] * Transform3f(quatf(0.0f, 0.0f, 0.0f, 1.0f),
jo(_kc, leg, FOOT_OFFSET, _centeredFootIK));
return CMUK_OKAY;
}
CMUK_ERROR_CODE cmuk::getRelTransform( const cmuk::XformCache& cache,
FrameIndex f0,
FrameIndex f1,
Transform3f* xform ) const {
int if0 = int(f0);
int if1 = int(f1);
if (if0 != FRAME_GROUND && (if0 < 0 || if0 >= NUM_FRAMES)) {
return CMUK_BAD_FRAME_INDEX;
}
if (if1 !=FRAME_GROUND && (if1 < 0 || if1 >= NUM_FRAMES)) {
return CMUK_BAD_FRAME_INDEX;
}
if (!xform) {
return CMUK_INSUFFICIENT_ARGUMENTS;
}
int leg0 = -1;
int leg1 = -1;
if (if0 > int(FRAME_TRUNK)) { leg0 = (if0 - 1) / 4; }
if (if1 > int(FRAME_TRUNK)) { leg1 = (if0 - 1) / 4; }
if (leg0 >= 0 && leg1 >= 0 && leg0 != leg1) {
Transform3f t1, t2;
CMUK_ERROR_CODE status;
status = getRelTransform(cache, f0, FRAME_TRUNK, &t1);
if (status != CMUK_OKAY) { return status; }
status = getRelTransform(cache, FRAME_TRUNK, f1, &t2);
if (status != CMUK_OKAY) { return status; }
*xform = t1 * t2;
return CMUK_OKAY;
}
if (if0 == if1) {
*xform = Transform3f();
return CMUK_OKAY;
}
// if1 should be smaller than if0
bool swap = if0 < if1;
if (swap) {
int tmp = if0;
if0 = if1;
if1 = tmp;
}
Transform3f rval = cache.relXforms[if0];
if0 = parentFrame(if0);
// now we are going to decrement if0
while (if1 < if0 && if0 > 0) {
rval = rval * cache.relXforms[if0];
if0 = parentFrame(if0);
}
if (swap) {
rval = rval.inverse();
}
*xform = rval;
return CMUK_OKAY;
}
void relativeTransform(const Transform3f& tf1, const Transform3f& tf2,
Transform3f& tf)
{
const Quaternion3f& q1_inv = fcl::conj(tf1.getQuatRotation());
tf = Transform3f(q1_inv * tf2.getQuatRotation(), q1_inv.transform(tf2.getTranslation() - tf1.getTranslation()));
}
void Sphere::computeLocalAABB()
{
computeBV<AABB>(*this, Transform3f(), aabb_local);
aabb_center = aabb_local.center();
aabb_radius = radius;
}
void TriangleP::computeLocalAABB()
{
computeBV<AABB>(*this, Transform3f(), aabb_local);
aabb_center = aabb_local.center();
aabb_radius = (aabb_local.min_ - aabb_center).length();
}
Transform3f BallEulerJoint::getLocalTransform(const JointConfig& cfg) const
{
Matrix3f rot;
rot.setEulerYPR(cfg[0], cfg[1], cfg[2]);
return transform_to_parent_ * Transform3f(rot);
}
Transform3f RevoluteJoint::getLocalTransform(const JointConfig& cfg) const
{
Quaternion3f quat;
quat.fromAxisAngle(axis_, cfg[0]);
return Transform3f(transform_to_parent_.getQuatRotation() * quat, transform_to_parent_.getTranslation());
}
Transform3f PrismaticJoint::getLocalTransform(const JointConfig& cfg) const
{
const Quaternion3f& quat = transform_to_parent_.getQuatRotation();
const Vec3f& transl = transform_to_parent_.getTranslation();
return Transform3f(quat, quat.transform(axis_ * cfg[0]) + transl);
}
Transform3f BallEulerJoint::getLocalTransform() const
{
Matrix3f rot;
rot.setEulerYPR((*joint_cfg_)[0], (*joint_cfg_)[1], (*joint_cfg_)[2]);
return transform_to_parent_ * Transform3f(rot);
}
Darwin::Darwin(){
float t_array[16];
mat4f t;
Links[0].NAME = "DBODY_LINK";
Links[0].MASS = 0.975599;
Links[0].COM = vec3f(-0.003116,-0.039444,-0.019663);
Links[0].NEXT = 1;
Links[0].PREVIOUS = -1;
Links[0].AXIS = vec3f(-0.707106,0.707108,0.000000);
t_array[0] = 1.000000;
t_array[1] = 0.000000;
t_array[2] = 0.000000;
t_array[3] = 0.000000;
t_array[4] = 0.000000;
t_array[5] = 1.000000;
t_array[6] = 0.000000;
t_array[7] = 0.000000;
t_array[8] = 0.000000;
t_array[9] = 0.000000;
t_array[10] = 1.000000;
t_array[11] = 0.000000;
t_array[12] = 0.000000;
t_array[13] = 0.000000;
t_array[14] = 0.000000;
t_array[15] = 0.000000;
t = mat4f(t_array);
Links[0].T_PREV2NEXT = Transform3f(t);
Links[1].NAME = "DNECK_LINK";
Links[1].MASS = 0.024358;
Links[1].COM = vec3f(0.001424,-0.016568,-0.000713);
Links[1].NEXT = 2;
Links[1].PREVIOUS = 0;
Links[1].AXIS = vec3f(0.000000,1.000000,0.000000);
t_array[0] = 1.000000;
t_array[1] = -0.000000;
t_array[2] = -0.000000;
t_array[3] = 0.000000;
t_array[4] = -0.000000;
t_array[5] = 0.707106;
t_array[6] = -0.707108;
t_array[7] = 0.000000;
t_array[8] = 0.000000;
t_array[9] = 0.707108;
t_array[10] = 0.707106;
t_array[11] = 0.000000;
t_array[12] = 0.000000;
t_array[13] = 0.000000;
t_array[14] = 0.000000;
t_array[15] = 1.000000;
t = mat4f(t_array);
Links[1].T_PREV2NEXT = Transform3f(t);
Links[2].NAME = "DHEAD_LINK";
Links[2].MASS = 0.158042;
Links[2].COM = vec3f(0.000064,0.018565,0.076666);
Links[2].NEXT = -1;
Links[2].PREVIOUS = 1;
Links[2].AXIS = vec3f(-1.000000,0.000000,0.000000);
t_array[0] = 1.000000;
t_array[1] = 0.000000;
t_array[2] = 0.000000;
t_array[3] = 0.000000;
t_array[4] = 0.000000;
t_array[5] = 1.000000;
t_array[6] = 0.000000;
t_array[7] = 0.000000;
t_array[8] = 0.000000;
t_array[9] = 0.000000;
t_array[10] = 1.000000;
t_array[11] = 0.000000;
t_array[12] = 0.000000;
t_array[13] = 0.000000;
t_array[14] = 0.000000;
t_array[15] = 0.000000;
t = mat4f(t_array);
Links[2].T_PREV2NEXT = Transform3f(t);
Links[3].NAME = "DPELVYL_LINK";
Links[3].MASS = 0.027069;
Links[3].COM = vec3f(0.000000,0.018437,0.000480);
Links[3].NEXT = 4;
Links[3].PREVIOUS = 0;
Links[3].AXIS = vec3f(-0.000000,-1.000000,0.000000);
t_array[0] = 1.000000;
t_array[1] = 0.000000;
t_array[2] = -0.000000;
t_array[3] = 0.000000;
t_array[4] = -0.000000;
t_array[5] = 1.000000;
t_array[6] = 0.000000;
t_array[7] = 0.000000;
t_array[8] = 0.000000;
t_array[9] = -0.000000;
t_array[10] = 1.000000;
t_array[11] = 0.000000;
t_array[12] = 0.000000;
t_array[13] = 0.000000;
t_array[14] = 0.000000;
t_array[15] = 1.000000;
t = mat4f(t_array);
Links[3].T_PREV2NEXT = Transform3f(t);
Links[4].NAME = "DPELVL_LINK";
Links[4].MASS = 0.167108;
Links[4].COM = vec3f(0.000080,-0.013873,-0.018242);
Links[4].NEXT = 5;
Links[4].PREVIOUS = 3;
Links[4].AXIS = vec3f(-0.000000,-0.000000,-1.000000);
t_array[0] = 1.000000;
t_array[1] = 0.000000;
t_array[2] = 0.000000;
t_array[3] = 0.000000;
t_array[4] = 0.000000;
t_array[5] = 1.000000;
t_array[6] = 0.000000;
t_array[7] = 0.000000;
t_array[8] = 0.000000;
t_array[9] = 0.000000;
t_array[10] = 1.000000;
t_array[11] = 0.000000;
t_array[12] = 0.000000;
t_array[13] = 0.000000;
t_array[14] = 0.000000;
t_array[15] = 1.000000;
t = mat4f(t_array);
Links[4].T_PREV2NEXT = Transform3f(t);
Links[5].NAME = "DLEGUPPERL_LINK";
Links[5].MASS = 0.119043;
Links[5].COM = vec3f(-0.000323,-0.062966,0.000692);
Links[5].NEXT = 6;
Links[5].PREVIOUS = 4;
Links[5].AXIS = vec3f(-1.000000,-0.000000,0.000000);
t_array[0] = 1.000000;
t_array[1] = 0.000000;
t_array[2] = 0.000000;
t_array[3] = -0.000000;
t_array[4] = 0.000000;
t_array[5] = 1.000000;
t_array[6] = 0.000000;
t_array[7] = -0.093000;
t_array[8] = 0.000000;
t_array[9] = 0.000000;
t_array[10] = 1.000000;
t_array[11] = -0.000000;
t_array[12] = 0.000000;
t_array[13] = 0.000000;
t_array[14] = 0.000000;
t_array[15] = 1.000000;
t = mat4f(t_array);
Links[5].T_PREV2NEXT = Transform3f(t);
Links[6].NAME = "DLEGLOWERL_LINK";
Links[6].MASS = 0.07031;
Links[6].COM = vec3f(-0.000592,0.053955,0.006547);
Links[6].NEXT = 7;
Links[6].PREVIOUS = 5;
Links[6].AXIS = vec3f(-1.000000,-0.000000,0.000000);
t_array[0] = 1.000000;
t_array[1] = 0.000000;
t_array[2] = 0.000000;
t_array[3] = -0.000000;
t_array[4] = 0.000000;
t_array[5] = 1.000000;
t_array[6] = 0.000000;
t_array[7] = -0.093000;
t_array[8] = 0.000000;
t_array[9] = 0.000000;
t_array[10] = 1.000000;
t_array[11] = -0.000000;
t_array[12] = 0.000000;
t_array[13] = 0.000000;
t_array[14] = 0.000000;
t_array[15] = 1.000000;
t = mat4f(t_array);
Links[6].T_PREV2NEXT = Transform3f(t);
Links[7].NAME = "DANKLEL_LINK";
Links[7].MASS = 0.167108;
Links[7].COM = vec3f(-0.000214,0.013873,-0.018536);
Links[7].NEXT = 8;
Links[7].PREVIOUS = 6;
Links[7].AXIS = vec3f(1.000000,0.000000,0.000000);
t_array[0] = 1.000000;
t_array[1] = -0.000000;
t_array[2] = 0.000000;
t_array[3] = 0.000000;
t_array[4] = 0.000000;
t_array[5] = 1.000000;
t_array[6] = -0.000000;
t_array[7] = 0.000000;
t_array[8] = -0.000000;
t_array[9] = 0.000000;
t_array[10] = 1.000000;
t_array[11] = 0.000000;
t_array[12] = 0.000000;
t_array[13] = 0.000000;
t_array[14] = 0.000000;
t_array[15] = 1.000000;
t = mat4f(t_array);
Links[7].T_PREV2NEXT = Transform3f(t);
Links[8].NAME = "DFOOTL_LINK";
Links[8].MASS = 0.079446;
Links[8].COM = vec3f(0.009506,-0.025995,-0.000503);
Links[8].NEXT = -1;
Links[8].PREVIOUS = 7;
Links[8].AXIS = vec3f(-0.000000,-0.000000,1.000000);
t_array[0] = 1.000000;
t_array[1] = 0.000000;
t_array[2] = 0.000000;
t_array[3] = 0.000000;
t_array[4] = 0.000000;
t_array[5] = 1.000000;
t_array[6] = 0.000000;
t_array[7] = 0.000000;
t_array[8] = 0.000000;
t_array[9] = 0.000000;
t_array[10] = 1.000000;
t_array[11] = 0.000000;
t_array[12] = 0.000000;
t_array[13] = 0.000000;
t_array[14] = 0.000000;
t_array[15] = 0.000000;
t = mat4f(t_array);
Links[8].T_PREV2NEXT = Transform3f(t);
Links[9].NAME = "DPELVYR_LINK";
Links[9].MASS = 0.027069;
Links[9].COM = vec3f(-0.000000,0.018437,0.000480);
Links[9].NEXT = 10;
Links[9].PREVIOUS = 0;
Links[9].AXIS = vec3f(-0.000000,-1.000000,0.000000);
t_array[0] = 1.000000;
t_array[1] = 0.000000;
t_array[2] = -0.000000;
t_array[3] = 0.000000;
t_array[4] = -0.000000;
t_array[5] = 1.000000;
t_array[6] = 0.000000;
t_array[7] = 0.000000;
t_array[8] = 0.000000;
t_array[9] = -0.000000;
t_array[10] = 1.000000;
t_array[11] = 0.000000;
t_array[12] = 0.000000;
t_array[13] = 0.000000;
t_array[14] = 0.000000;
t_array[15] = 1.000000;
t = mat4f(t_array);
Links[9].T_PREV2NEXT = Transform3f(t);
Links[10].NAME = "DPELVR_LINK";
Links[10].MASS = 0.167108;
Links[10].COM = vec3f(-0.000080,-0.013873,-0.018242);
Links[10].NEXT = 11;
Links[10].PREVIOUS = 9;
Links[10].AXIS = vec3f(-0.000000,-0.000000,-1.000000);
t_array[0] = 1.000000;
t_array[1] = 0.000000;
t_array[2] = 0.000000;
t_array[3] = 0.000000;
t_array[4] = 0.000000;
t_array[5] = 1.000000;
t_array[6] = 0.000000;
t_array[7] = 0.000000;
t_array[8] = 0.000000;
t_array[9] = 0.000000;
t_array[10] = 1.000000;
t_array[11] = 0.000000;
t_array[12] = 0.000000;
t_array[13] = 0.000000;
t_array[14] = 0.000000;
t_array[15] = 1.000000;
t = mat4f(t_array);
Links[10].T_PREV2NEXT = Transform3f(t);
Links[11].NAME = "DLEGUPPERR_LINK";
Links[11].MASS = 0.119043;
Links[11].COM = vec3f(0.000323,-0.062966,0.000692);
Links[11].NEXT = 12;
Links[11].PREVIOUS = 10;
Links[11].AXIS = vec3f(1.000000,0.000000,0.000000);
t_array[0] = 1.000000;
t_array[1] = 0.000000;
t_array[2] = 0.000000;
t_array[3] = -0.000000;
t_array[4] = 0.000000;
t_array[5] = 1.000000;
t_array[6] = 0.000000;
t_array[7] = -0.093000;
t_array[8] = 0.000000;
t_array[9] = 0.000000;
t_array[10] = 1.000000;
t_array[11] = -0.000000;
t_array[12] = 0.000000;
t_array[13] = 0.000000;
t_array[14] = 0.000000;
t_array[15] = 1.000000;
t = mat4f(t_array);
Links[11].T_PREV2NEXT = Transform3f(t);
Links[12].NAME = "DLEGLOWERR_LINK";
Links[12].MASS = 0.07031;
Links[12].COM = vec3f(0.000592,0.053955,0.006547);
Links[12].NEXT = 13;
Links[12].PREVIOUS = 11;
Links[12].AXIS = vec3f(1.000000,0.000000,0.000000);
t_array[0] = 1.000000;
t_array[1] = 0.000000;
t_array[2] = 0.000000;
t_array[3] = -0.000000;
t_array[4] = 0.000000;
t_array[5] = 1.000000;
t_array[6] = 0.000000;
t_array[7] = -0.093000;
t_array[8] = 0.000000;
t_array[9] = 0.000000;
t_array[10] = 1.000000;
t_array[11] = -0.000000;
t_array[12] = 0.000000;
t_array[13] = 0.000000;
t_array[14] = 0.000000;
t_array[15] = 1.000000;
t = mat4f(t_array);
Links[12].T_PREV2NEXT = Transform3f(t);
Links[13].NAME = "DANKLER_LINK";
Links[13].MASS = 0.167108;
Links[13].COM = vec3f(0.000214,0.013873,-0.018536);
Links[13].NEXT = 14;
Links[13].PREVIOUS = 12;
Links[13].AXIS = vec3f(-1.000000,-0.000000,0.000000);
t_array[0] = 1.000000;
t_array[1] = -0.000000;
t_array[2] = 0.000000;
t_array[3] = 0.000000;
t_array[4] = 0.000000;
t_array[5] = 1.000000;
t_array[6] = -0.000000;
t_array[7] = 0.000000;
t_array[8] = -0.000000;
t_array[9] = 0.000000;
t_array[10] = 1.000000;
t_array[11] = 0.000000;
t_array[12] = 0.000000;
t_array[13] = 0.000000;
t_array[14] = 0.000000;
t_array[15] = 1.000000;
t = mat4f(t_array);
Links[13].T_PREV2NEXT = Transform3f(t);
Links[14].NAME = "DFOOTR_LINK";
Links[14].MASS = 0.079446;
Links[14].COM = vec3f(-0.009506,-0.025995,-0.000503);
Links[14].NEXT = -1;
Links[14].PREVIOUS = 13;
Links[14].AXIS = vec3f(-0.000000,-0.000000,1.000000);
t_array[0] = 1.000000;
t_array[1] = 0.000000;
t_array[2] = 0.000000;
t_array[3] = 0.000000;
t_array[4] = 0.000000;
t_array[5] = 1.000000;
t_array[6] = 0.000000;
t_array[7] = 0.000000;
t_array[8] = 0.000000;
t_array[9] = 0.000000;
t_array[10] = 1.000000;
t_array[11] = 0.000000;
t_array[12] = 0.000000;
t_array[13] = 0.000000;
t_array[14] = 0.000000;
t_array[15] = 0.000000;
t = mat4f(t_array);
Links[14].T_PREV2NEXT = Transform3f(t);
Links[15].NAME = "DSHOULDERL_LINK";
Links[15].MASS = 0.025913;
Links[15].COM = vec3f(-0.013523,0.010264,0.001394);
Links[15].NEXT = 16;
Links[15].PREVIOUS = 0;
Links[15].AXIS = vec3f(1.000000,-0.000000,0.000000);
t_array[0] = 0.707106;
t_array[1] = -0.707108;
t_array[2] = -0.000000;
t_array[3] = -0.000000;
t_array[4] = 0.707108;
t_array[5] = 0.707106;
t_array[6] = 0.000000;
t_array[7] = -0.016000;
t_array[8] = -0.000000;
t_array[9] = -0.000000;
t_array[10] = 1.000000;
t_array[11] = -0.000000;
t_array[12] = 0.000000;
t_array[13] = 0.000000;
t_array[14] = 0.000000;
t_array[15] = 1.000000;
t = mat4f(t_array);
Links[15].T_PREV2NEXT = Transform3f(t);
Links[16].NAME = "DARMUPPERL_LINK";
Links[16].MASS = 0.168377;
Links[16].COM = vec3f(0.000660,-0.036239,0.000734);
Links[16].NEXT = 17;
Links[16].PREVIOUS = 15;
Links[16].AXIS = vec3f(0.000000,0.000000,-1.000000);
t_array[0] = 1.000000;
t_array[1] = 0.000000;
t_array[2] = -0.000000;
t_array[3] = 0.000000;
t_array[4] = 0.000000;
t_array[5] = 0.000000;
t_array[6] = 1.000000;
t_array[7] = -0.060000;
t_array[8] = -0.000000;
t_array[9] = -1.000000;
t_array[10] = 0.000000;
t_array[11] = 0.016000;
t_array[12] = 0.000000;
t_array[13] = 0.000000;
t_array[14] = 0.000000;
t_array[15] = 1.000000;
t = mat4f(t_array);
Links[16].T_PREV2NEXT = Transform3f(t);
Links[17].NAME = "DARMLOWERL_LINK";
Links[17].MASS = 0.059288;
Links[17].COM = vec3f(0.006666,-0.045838,-0.013490);
Links[17].NEXT = -1;
Links[17].PREVIOUS = 16;
Links[17].AXIS = vec3f(1.000000,-0.000000,-0.000000);
t_array[0] = 1.000000;
t_array[1] = 0.000000;
t_array[2] = 0.000000;
t_array[3] = 0.000000;
t_array[4] = 0.000000;
t_array[5] = 1.000000;
t_array[6] = 0.000000;
t_array[7] = 0.000000;
t_array[8] = 0.000000;
t_array[9] = 0.000000;
t_array[10] = 1.000000;
t_array[11] = 0.000000;
t_array[12] = 0.000000;
t_array[13] = 0.000000;
t_array[14] = 0.000000;
t_array[15] = 0.000000;
t = mat4f(t_array);
Links[17].T_PREV2NEXT = Transform3f(t);
Links[18].NAME = "DSHOULDERR_LINK";
Links[18].MASS = 0.025913;
Links[18].COM = vec3f(0.013523,0.010264,0.001394);
Links[18].NEXT = 19;
Links[18].PREVIOUS = 0;
Links[18].AXIS = vec3f(-1.000000,0.000000,0.000000);
t_array[0] = 0.707106;
t_array[1] = 0.707108;
t_array[2] = -0.000000;
t_array[3] = -0.000000;
t_array[4] = -0.707108;
t_array[5] = 0.707106;
t_array[6] = -0.000000;
t_array[7] = -0.016000;
t_array[8] = 0.000000;
t_array[9] = -0.000000;
t_array[10] = 1.000000;
t_array[11] = 0.000000;
t_array[12] = 0.000000;
t_array[13] = 0.000000;
t_array[14] = 0.000000;
t_array[15] = 1.000000;
t = mat4f(t_array);
Links[18].T_PREV2NEXT = Transform3f(t);
Links[19].NAME = "DARMUPPERR_LINK";
Links[19].MASS = 0.168377;
Links[19].COM = vec3f(-0.000660,-0.036239,0.000734);
Links[19].NEXT = 20;
Links[19].PREVIOUS = 18;
Links[19].AXIS = vec3f(-0.000000,0.000000,-1.000000);
t_array[0] = 1.000000;
t_array[1] = -0.000000;
t_array[2] = 0.000000;
t_array[3] = -0.000000;
t_array[4] = -0.000000;
t_array[5] = 0.000000;
t_array[6] = 1.000000;
t_array[7] = -0.060000;
t_array[8] = -0.000000;
t_array[9] = -1.000000;
t_array[10] = 0.000000;
t_array[11] = 0.016000;
t_array[12] = 0.000000;
t_array[13] = 0.000000;
t_array[14] = 0.000000;
t_array[15] = 1.000000;
t = mat4f(t_array);
Links[19].T_PREV2NEXT = Transform3f(t);
Links[20].NAME = "DARMLOWERR_LINK";
Links[20].MASS = 0.059288;
Links[20].COM = vec3f(-0.006666,-0.045838,-0.013490);
Links[20].NEXT = -1;
Links[20].PREVIOUS = 19;
Links[20].AXIS = vec3f(-1.000000,0.000000,-0.000000);
t_array[0] = 1.000000;
t_array[1] = 0.000000;
t_array[2] = 0.000000;
t_array[3] = 0.000000;
t_array[4] = 0.000000;
t_array[5] = 1.000000;
t_array[6] = 0.000000;
t_array[7] = 0.000000;
t_array[8] = 0.000000;
t_array[9] = 0.000000;
t_array[10] = 1.000000;
t_array[11] = 0.000000;
t_array[12] = 0.000000;
t_array[13] = 0.000000;
t_array[14] = 0.000000;
t_array[15] = 0.000000;
t = mat4f(t_array);
Links[20].T_PREV2NEXT = Transform3f(t);
Chains[0].FIRST = 1;
t_array[0] = 1.000000;
t_array[1] = -0.000000;
t_array[2] = -0.000000;
t_array[3] = 0.000000;
t_array[4] = -0.000000;
t_array[5] = 1.000000;
t_array[6] = -0.000000;
t_array[7] = 0.051000;
t_array[8] = -0.000000;
t_array[9] = -0.000000;
t_array[10] = 1.000000;
t_array[11] = -0.000000;
t_array[12] = 0.000000;
t_array[13] = 0.000000;
t_array[14] = 0.000000;
t_array[15] = 1.000000;
t = mat4f(t_array);
Chains[0].T_FROM_BODY = Transform3f(t);
Chains[1].FIRST = 3;
t_array[0] = 1.000000;
t_array[1] = -0.000000;
t_array[2] = -0.000000;
t_array[3] = 0.037000;
t_array[4] = -0.000000;
t_array[5] = 1.000000;
t_array[6] = -0.000000;
t_array[7] = -0.122200;
t_array[8] = -0.000000;
t_array[9] = -0.000000;
t_array[10] = 1.000000;
t_array[11] = -0.005000;
t_array[12] = 0.000000;
t_array[13] = 0.000000;
t_array[14] = 0.000000;
t_array[15] = 1.000000;
t = mat4f(t_array);
Chains[1].T_FROM_BODY = Transform3f(t);
Chains[2].FIRST = 9;
t_array[0] = 1.000000;
t_array[1] = -0.000000;
t_array[2] = -0.000000;
t_array[3] = -0.037000;
t_array[4] = -0.000000;
t_array[5] = 1.000000;
t_array[6] = -0.000000;
t_array[7] = -0.122200;
t_array[8] = -0.000000;
t_array[9] = -0.000000;
t_array[10] = 1.000000;
t_array[11] = -0.005000;
t_array[12] = 0.000000;
t_array[13] = 0.000000;
t_array[14] = 0.000000;
t_array[15] = 1.000000;
t = mat4f(t_array);
Chains[2].T_FROM_BODY = Transform3f(t);
Chains[3].FIRST = 15;
t_array[0] = 1.000000;
t_array[1] = -0.000000;
t_array[2] = -0.000000;
t_array[3] = 0.082000;
t_array[4] = -0.000000;
t_array[5] = 1.000000;
t_array[6] = -0.000000;
t_array[7] = 0.000000;
t_array[8] = -0.000000;
t_array[9] = -0.000000;
t_array[10] = 1.000000;
t_array[11] = 0.000000;
t_array[12] = 0.000000;
t_array[13] = 0.000000;
t_array[14] = 0.000000;
t_array[15] = 1.000000;
t = mat4f(t_array);
Chains[3].T_FROM_BODY = Transform3f(t);
Chains[4].FIRST = 18;
t_array[0] = 1.000000;
t_array[1] = -0.000000;
t_array[2] = -0.000000;
t_array[3] = -0.082000;
t_array[4] = -0.000000;
t_array[5] = 1.000000;
t_array[6] = -0.000000;
t_array[7] = -0.000000;
t_array[8] = -0.000000;
t_array[9] = -0.000000;
t_array[10] = 1.000000;
t_array[11] = -0.000000;
t_array[12] = 0.000000;
t_array[13] = 0.000000;
t_array[14] = 0.000000;
t_array[15] = 1.000000;
t = mat4f(t_array);
Chains[4].T_FROM_BODY = Transform3f(t);
}