예제 #1
0
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()));
}
예제 #2
0
파일: transform.cpp 프로젝트: orthez/fcl
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()));
}
예제 #3
0
파일: cmuk.cpp 프로젝트: swatbotics/darwin
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];
    
  }
  
}
예제 #4
0
파일: cmuk.cpp 프로젝트: swatbotics/darwin
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;

}
예제 #5
0
파일: cmuk.cpp 프로젝트: swatbotics/darwin
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;

}
예제 #6
0
파일: cmuk.cpp 프로젝트: swatbotics/darwin
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;

}
예제 #7
0
파일: transform.cpp 프로젝트: orthez/fcl
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()));
}
예제 #8
0
void Sphere::computeLocalAABB()
{
  computeBV<AABB>(*this, Transform3f(), aabb_local);
  aabb_center = aabb_local.center();
  aabb_radius = radius;
}
예제 #9
0
void TriangleP::computeLocalAABB()
{
  computeBV<AABB>(*this, Transform3f(), aabb_local);
  aabb_center = aabb_local.center();
  aabb_radius = (aabb_local.min_ - aabb_center).length();
}
예제 #10
0
Transform3f BallEulerJoint::getLocalTransform(const JointConfig& cfg) const
{
  Matrix3f rot;
  rot.setEulerYPR(cfg[0], cfg[1], cfg[2]);
  return transform_to_parent_ * Transform3f(rot);
}
예제 #11
0
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());
}
예제 #12
0
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);
}
예제 #13
0
파일: joint.cpp 프로젝트: Sosi/fcl
Transform3f BallEulerJoint::getLocalTransform() const
{
  Matrix3f rot;
  rot.setEulerYPR((*joint_cfg_)[0], (*joint_cfg_)[1], (*joint_cfg_)[2]);
  return transform_to_parent_ * Transform3f(rot);
}
예제 #14
0
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);

}