#include <UnitTest++.h>
#include "rbdl/rbdl.h"
#include <cassert>

using namespace std;
using namespace RigidBodyDynamics;
using namespace RigidBodyDynamics::Math;

const double TEST_PREC = 1.0e-11;

// Reduce an angle to the (-pi, pi] range.
static double inRange(double angle) {
  while(angle > M_PI) {
    angle -= 2. * M_PI;
  }
  while(angle <= -M_PI) {
    angle += 2. * M_PI;
  }
  return angle;
}

struct FourBarLinkage {

  FourBarLinkage()
    : model()
    , cs()
    , q()
    , qd()
    , qdd()
    , tau()
    , l1(2.)
    , l2(2.)
    , m1(2.)
    , m2(2.)
    , idB1(0)
    , idB2(0)
    , idB3(0)
    , idB4(0)
    , idB5(0)
    , X_p(Xtrans(Vector3d(l2, 0., 0.)))
    , X_s(Xtrans(Vector3d(0., 0., 0.))) {
    
    Body link1 = Body(m1, Vector3d(0.5 * l1, 0., 0.)
      , Vector3d(0., 0., m1 * l1 * l1 / 3.));
    Body link2 = Body(m2, Vector3d(0.5 * l2, 0., 0.)
      , Vector3d(0., 0., m2 * l2 * l2 / 3.));
    Vector3d vector3d_zero = Vector3d::Zero();
    Body virtual_body(0., vector3d_zero, vector3d_zero);
    Joint joint_rev_z(JointTypeRevoluteZ);

    idB1 = model.AddBody(0, Xtrans(Vector3d(0., 0., 0.)), joint_rev_z, link1);
    idB2 = model.AddBody(idB1, Xtrans(Vector3d(l1, 0., 0.)), joint_rev_z, link2);
    idB3 = model.AddBody(0, Xtrans(Vector3d(0., 0., 0.)), joint_rev_z, link1);
    idB4 = model.AddBody(idB3, Xtrans(Vector3d(l1, 0., 0.)), joint_rev_z, link2);
    idB5 = model.AddBody(idB4, Xtrans(Vector3d(l2, 0., 0.)), joint_rev_z
      , virtual_body);

    cs.AddLoopConstraint(idB2, idB5, X_p, X_s, SpatialVector(0,0,0,1,0,0), 0.1);
    cs.AddLoopConstraint(idB2, idB5, X_p, X_s, SpatialVector(0,0,0,0,1,0), 0.1);
    cs.AddLoopConstraint(idB2, idB5, X_p, X_s, SpatialVector(0,0,1,0,0,0), 0.1);

    cs.Bind(model);

    q = VectorNd::Zero(model.dof_count);
    qd = VectorNd::Zero(model.dof_count);
    qdd = VectorNd::Zero(model.dof_count);
    tau = VectorNd::Zero(model.dof_count);

  }

  Model model;
  ConstraintSet cs;

  VectorNd q;
  VectorNd qd;
  VectorNd qdd;
  VectorNd tau;

  double l1;
  double l2;
  double m1;
  double m2;

  unsigned int idB1;
  unsigned int idB2;
  unsigned int idB3;
  unsigned int idB4;
  unsigned int idB5;

  SpatialTransform X_p;
  SpatialTransform X_s;

};

struct FloatingFourBarLinkage {

  FloatingFourBarLinkage()
    : model()
    , cs()
    , q()
    , qd()
    , qdd()
    , tau()
    , l1(2.)
    , l2(2.)
    , m1(2.)
    , m2(2.)
    , idB0(0)
    , idB1(0)
    , idB2(0)
    , idB3(0)
    , idB4(0)
    , idB5(0)
    , X_p(Xtrans(Vector3d(l2, 0., 0.)))
    , X_s(Xtrans(Vector3d(0., 0., 0.))) {
    
    Body link1 = Body(m1, Vector3d(0.5 * l1, 0., 0.)
      , Vector3d(0., 0., m1 * l1 * l1 / 3.));
    Body link2 = Body(m2, Vector3d(0.5 * l2, 0., 0.)
      , Vector3d(0., 0., m2 * l2 * l2 / 3.));
    Vector3d vector3d_zero = Vector3d::Zero();
    Body virtual_body(0., vector3d_zero, vector3d_zero);
    Joint joint_trans(JointTypeTranslationXYZ);
    Joint joint_rev_z(JointTypeRevoluteZ);

    idB0 = model.AddBody(0, Xtrans(Vector3d(0., 0., 0.)), joint_trans
      , virtual_body);
    idB1 = model.AddBody(idB0, Xtrans(Vector3d(0., 0., 0.)), joint_rev_z, link1);
    idB2 = model.AddBody(idB1, Xtrans(Vector3d(l1, 0., 0.)), joint_rev_z, link2);
    idB3 = model.AddBody(idB0, Xtrans(Vector3d(0., 0., 0.)), joint_rev_z, link1);
    idB4 = model.AddBody(idB3, Xtrans(Vector3d(l1, 0., 0.)), joint_rev_z, link2);
    idB5 = model.AddBody(idB4, Xtrans(Vector3d(l2, 0., 0.)), joint_rev_z
      , virtual_body);

    cs.AddContactConstraint(idB0, Vector3d::Zero(), Vector3d(1,0,0));
    cs.AddContactConstraint(idB0, Vector3d::Zero(), Vector3d(0,1,0));
    cs.AddContactConstraint(idB0, Vector3d::Zero(), Vector3d(0,0,1));
    cs.AddLoopConstraint(idB2, idB5, X_p, X_s, SpatialVector(0,0,0,1,0,0), 0.1);
    cs.AddLoopConstraint(idB2, idB5, X_p, X_s, SpatialVector(0,0,0,0,1,0), 0.1);
    cs.AddLoopConstraint(idB2, idB5, X_p, X_s, SpatialVector(0,0,1,0,0,0), 0.1);

    cs.Bind(model);

    q = VectorNd::Zero(model.dof_count);
    qd = VectorNd::Zero(model.dof_count);
    qdd = VectorNd::Zero(model.dof_count);
    tau = VectorNd::Zero(model.dof_count);

  }

  Model model;
  ConstraintSet cs;

  VectorNd q;
  VectorNd qd;
  VectorNd qdd;
  VectorNd tau;

  double l1;
  double l2;
  double m1;
  double m2;

  unsigned int idB0;
  unsigned int idB1;
  unsigned int idB2;
  unsigned int idB3;
  unsigned int idB4;
  unsigned int idB5;

  SpatialTransform X_p;
  SpatialTransform X_s;

};

struct SliderCrank3D {

  SliderCrank3D()
    : model()
    , cs()
    , q()
    , qd()
    , id_p(0)
    , id_s(0)
    , X_p()
    , X_s() {

    double slider_mass = 5.;
    double slider_height = 0.1;
    double crank_link1_mass = 3.;
    double crank_link1_length = 1.;
    double crank_link2_mass = 1.;
    double crank_link2_radius = 0.2;
    double crank_link2_length = 3.;
    double crank_link1_height = crank_link2_length - crank_link1_length 
      + slider_height;

    Body slider(slider_mass, Vector3d::Zero(), Vector3d(1., 1., 1.));
    Body crankLink1(crank_link1_mass
      , Vector3d(0.5 * crank_link1_length, 0., 0.)
      , Vector3d(0., 0.
      , crank_link1_mass * crank_link1_length * crank_link1_length / 3.));
    Body crankLink2(crank_link2_mass
      , Vector3d(0.5 * crank_link2_length, 0., 0.)
      , Vector3d(crank_link2_mass * crank_link2_radius * crank_link2_radius / 2.
      , crank_link2_mass * (3. * crank_link2_radius * crank_link2_radius 
      + crank_link2_length * crank_link2_length) / 12.
      , crank_link2_mass * (3. * crank_link2_radius * crank_link2_radius 
      + crank_link2_length * crank_link2_length) / 12.));

    Joint joint_rev_z(JointTypeRevoluteZ);
    Joint joint_sphere(JointTypeEulerZYX);
    Joint joint_prs_x(SpatialVector(0.,0.,0.,1.,0.,0.));

    id_p = model.AddBody(0
      , SpatialTransform()
      , joint_prs_x, slider);
    unsigned int id_b1 = model.AddBody(0
      , Xroty(-0.5*M_PI) * Xtrans(Vector3d(0., 0., crank_link1_height)) 
      , joint_rev_z, crankLink1);
    id_s = model.AddBody(id_b1
      , Xroty(M_PI) * Xtrans(Vector3d(crank_link1_length, 0., 0.))
      , joint_sphere, crankLink2);

    X_p = Xtrans(Vector3d(0., 0., slider_height));
    X_s = SpatialTransform(roty(-0.5 * M_PI), Vector3d(crank_link2_length, 0, 0));

    cs.AddLoopConstraint(id_p, id_s, X_p, X_s, SpatialVector(0,0,0,1,0,0), 0.1);
    cs.AddLoopConstraint(id_p, id_s, X_p, X_s, SpatialVector(0,0,0,0,1,0), 0.1);
    cs.AddLoopConstraint(id_p, id_s, X_p, X_s, SpatialVector(0,0,0,0,0,1), 0.1);
    cs.AddLoopConstraint(id_p, id_s, X_p, X_s, SpatialVector(0,0,1,0,0,0), 0.1);

    cs.Bind(model);

    q = VectorNd::Zero(model.dof_count);
    qd = VectorNd::Zero(model.dof_count);
    qdd = VectorNd::Zero(model.dof_count);
    tau = VectorNd::Zero(model.dof_count);

    Matrix3d rot_ps 
      = (CalcBodyWorldOrientation(model, q, id_p).transpose() * X_p.E).transpose()
      * CalcBodyWorldOrientation(model, q, id_s).transpose() * X_s.E;
    assert(rot_ps(0,0) - 1. < TEST_PREC);
    assert(rot_ps(1,1) - 1. < TEST_PREC);
    assert(rot_ps(2,2) - 1. < TEST_PREC);
    assert(rot_ps(0,1) < TEST_PREC);
    assert(rot_ps(0,2) < TEST_PREC);
    assert(rot_ps(1,0) < TEST_PREC);
    assert(rot_ps(1,2) < TEST_PREC);
    assert(rot_ps(2,0) < TEST_PREC);
    assert(rot_ps(2,1) < TEST_PREC);
    assert((CalcBodyToBaseCoordinates(model, q, id_p, X_p.r)
      - CalcBodyToBaseCoordinates(model, q, id_s, X_s.r)).norm() < TEST_PREC);

  }

  Model model;
  ConstraintSet cs;

  VectorNd q;
  VectorNd qd;
  VectorNd qdd;
  VectorNd tau;

  unsigned int id_p;
  unsigned int id_s;
  SpatialTransform X_p;
  SpatialTransform X_s;

};

struct SliderCrank3DSphericalJoint {

  SliderCrank3DSphericalJoint()
    : model()
    , cs()
    , q()
    , qd()
    , id_p(0)
    , id_s(0)
    , X_p()
    , X_s() {

    double slider_mass = 5.;
    double slider_height = 0.1;
    double crank_link1_mass = 3.;
    double crank_link1_length = 1.;
    double crank_link2_mass = 1.;
    double crank_link2_radius = 0.2;
    double crank_link2_length = 3.;
    double crank_link1_height = crank_link2_length - crank_link1_length 
      + slider_height;

    Body slider(slider_mass, Vector3d::Zero(), Vector3d(1., 1., 1.));
    Body crankLink1(crank_link1_mass
      , Vector3d(0.5 * crank_link1_length, 0., 0.)
      , Vector3d(0., 0.
      , crank_link1_mass * crank_link1_length * crank_link1_length / 3.));
    Body crankLink2(crank_link2_mass
      , Vector3d(0.5 * crank_link2_length, 0., 0.)
      , Vector3d(crank_link2_mass * crank_link2_radius * crank_link2_radius / 2.
      , crank_link2_mass * (3. * crank_link2_radius * crank_link2_radius 
      + crank_link2_length * crank_link2_length) / 12.
      , crank_link2_mass * (3. * crank_link2_radius * crank_link2_radius 
      + crank_link2_length * crank_link2_length) / 12.));

    Joint joint_rev_z(JointTypeRevoluteZ);
    Joint joint_sphere(JointTypeSpherical);
    Joint joint_prs_x(SpatialVector(0.,0.,0.,1.,0.,0.));

    id_p = model.AddBody(0
      , SpatialTransform()
      , joint_prs_x, slider);
    unsigned int id_b1 = model.AddBody(0
      , Xroty(-0.5*M_PI) * Xtrans(Vector3d(0., 0., crank_link1_height)) 
      , joint_rev_z, crankLink1);
    id_s = model.AddBody(id_b1
      , Xroty(M_PI) * Xtrans(Vector3d(crank_link1_length, 0., 0.))
      , joint_sphere, crankLink2);

    X_p = Xtrans(Vector3d(0., 0., slider_height));
    X_s = SpatialTransform(roty(-0.5 * M_PI), Vector3d(crank_link2_length, 0, 0));

    cs.AddLoopConstraint(id_p, id_s, X_p, X_s, SpatialVector(0,0,0,1,0,0), 0.1);
    cs.AddLoopConstraint(id_p, id_s, X_p, X_s, SpatialVector(0,0,0,0,1,0), 0.1);
    cs.AddLoopConstraint(id_p, id_s, X_p, X_s, SpatialVector(0,0,0,0,0,1), 0.1);
    cs.AddLoopConstraint(id_p, id_s, X_p, X_s, SpatialVector(0,0,1,0,0,0), 0.1);

    cs.Bind(model);

    q = VectorNd::Zero(model.q_size);
    qd = VectorNd::Zero(model.dof_count);
    qdd = VectorNd::Zero(model.dof_count);
    tau = VectorNd::Zero(model.dof_count);

    Matrix3d rot_ps 
      = (CalcBodyWorldOrientation(model, q, id_p).transpose() * X_p.E).transpose()
      * CalcBodyWorldOrientation(model, q, id_s).transpose() * X_s.E;
    assert(rot_ps(0,0) - 1. < TEST_PREC);
    assert(rot_ps(1,1) - 1. < TEST_PREC);
    assert(rot_ps(2,2) - 1. < TEST_PREC);
    assert(rot_ps(0,1) < TEST_PREC);
    assert(rot_ps(0,2) < TEST_PREC);
    assert(rot_ps(1,0) < TEST_PREC);
    assert(rot_ps(1,2) < TEST_PREC);
    assert(rot_ps(2,0) < TEST_PREC);
    assert(rot_ps(2,1) < TEST_PREC);
    assert((CalcBodyToBaseCoordinates(model, q, id_p, X_p.r)
      - CalcBodyToBaseCoordinates(model, q, id_s, X_s.r)).norm() < TEST_PREC);

  }

  Model model;
  ConstraintSet cs;

  VectorNd q;
  VectorNd qd;
  VectorNd qdd;
  VectorNd tau;

  unsigned int id_p;
  unsigned int id_s;
  SpatialTransform X_p;
  SpatialTransform X_s;

};

TEST_FIXTURE(FourBarLinkage, TestFourBarLinkageConstraintErrors) {
  VectorNd err = VectorNd::Zero(cs.size());
  Vector3d pos1;
  Vector3d pos2;
  Vector3d posErr;
  Matrix3d rot_p;
  double angleErr;

  // Test in zero position.
  q[0] = 0.;
  q[1] = 0.;
  q[2] = 0.;
  q[3] = 0.;
  q[4] = 0.;

  CalcConstraintsPositionError(model, q, cs, err);

  CHECK_CLOSE(0., err[0], TEST_PREC);
  CHECK_CLOSE(0., err[1], TEST_PREC);
  CHECK_CLOSE(0., err[2], TEST_PREC);

  // Test in non-zero position.
  q[0] = M_PI * 3 / 4;
  q[1] = -M_PI;
  q[2] = M_PI - q[0];
  q[3] = -q[1];
  q[4] = 0.;
  angleErr = sin(-0.5 * M_PI);

  pos1 = CalcBodyToBaseCoordinates(model, q, idB2, X_p.r);
  pos2 = CalcBodyToBaseCoordinates(model, q, idB5, X_s.r);
  rot_p = CalcBodyWorldOrientation(model, q, idB2).transpose() * X_p.E;
  posErr = rot_p.transpose() * (pos2 - pos1);

  assert(std::fabs(posErr[1]) < TEST_PREC);
  assert(std::fabs(posErr[2]) < TEST_PREC);

  CalcConstraintsPositionError(model, q, cs, err);

  CHECK_CLOSE(posErr[0], err[0], TEST_PREC);
  CHECK_CLOSE(0., err[1], TEST_PREC);
  CHECK_CLOSE(angleErr, err[2], TEST_PREC);

  // Test in non-zero position.
  q[0] = 0.;
  q[1] = 0.;
  q[2] = M_PI + 0.1;
  q[3] = 0.;
  q[4] = 0.;
  angleErr = sin(-q[0] - q[1] + q[2] + q[3] + q[4]);

  pos1 = CalcBodyToBaseCoordinates(model, q, idB2, X_p.r);
  pos2 = CalcBodyToBaseCoordinates(model, q, idB5, X_s.r);
  rot_p = CalcBodyWorldOrientation(model, q, idB2).transpose() * X_p.E;
  posErr = rot_p.transpose() * (pos2 - pos1);

  CalcConstraintsPositionError(model, q, cs, err);

  CHECK_CLOSE(posErr[0], err[0], TEST_PREC);
  CHECK_CLOSE(posErr[1], err[1], TEST_PREC);
  CHECK_CLOSE(angleErr, err[2], TEST_PREC);

  // Test in non-zero position.
  q[0] = 0.8;
  q[1] = -0.4;
  q[2] = M_PI - q[0];
  q[3] = -q[1];
  q[4] = 0.;
  angleErr = sin(-q[0] - q[1] + q[2] + q[3] + q[4]);

  pos1 = CalcBodyToBaseCoordinates(model, q, idB2, X_p.r);
  pos2 = CalcBodyToBaseCoordinates(model, q, idB5, X_s.r);
  rot_p = CalcBodyWorldOrientation(model, q, idB2).transpose() * X_p.E;
  posErr = rot_p.transpose() * (pos2 - pos1);

  CalcConstraintsPositionError(model, q, cs, err);

  CHECK_CLOSE(posErr[0], err[0], TEST_PREC);
  CHECK_CLOSE(posErr[1], err[1], TEST_PREC);
  CHECK_CLOSE(angleErr, err[2], TEST_PREC);
}

TEST_FIXTURE(FourBarLinkage, TestFourBarLinkageConstraintJacobian) {
  MatrixNd G(MatrixNd::Zero(cs.size(), q.size()));
  VectorNd err(VectorNd::Zero(cs.size()));
  VectorNd errRef(VectorNd::Zero(cs.size()));

  // Zero Q configuration, both arms of the 4-bar laying on the x-axis
  q[0] = 0.;
  q[1] = 0.;
  q[2] = 0.;
  q[3] = 0.;
  q[4] = 0.;
  assert(q[0] + q[1] - q[2] - q[3] - q[4] == 0.);
  assert((CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) 
    - CalcBodyToBaseCoordinates(model, q, idB5, X_s.r)).norm() < TEST_PREC);

  qd[0] = -1.;
  qd[1] = -1.;
  qd[2] = -1.;
  qd[3] = -1.;
  qd[4] = 0.;
  assert((CalcPointVelocity6D(model, q, qd, idB2, X_p.r)
    - CalcPointVelocity6D(model, q, qd, idB5, X_s.r)).norm() < TEST_PREC);

  CalcConstraintsJacobian(model, q, cs, G);

  err = G * qd;

  CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC);

  // Both arms of the 4-bar laying on the y-axis
  q[0] = 0.5 * M_PI;
  q[1] = 0.;
  q[2] = 0.5 * M_PI;
  q[3] = 0.;
  q[4] = 0.;
  assert(q[0] + q[1] - q[2] - q[3] - q[4] == 0.);
  assert((CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) 
    - CalcBodyToBaseCoordinates(model, q, idB5, X_s.r)).norm() < TEST_PREC);

  qd[0] = -1.;
  qd[1] = -1.;
  qd[2] = -1.;
  qd[3] = -1.;
  qd[4] = 0.;
  assert((CalcPointVelocity6D(model, q, qd, idB2, X_p.r)
    - CalcPointVelocity6D(model, q, qd, idB5, X_s.r)).norm() < TEST_PREC);

  CalcConstraintsJacobian(model, q, cs, G);

  err = G * qd;

  CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC);

  // Arms symmetric wrt y axis.
  q[0] = M_PI * 3 / 4;
  q[1] = -0.5 * M_PI;
  q[2] = M_PI - q[0];
  q[3] = -q[1];
  q[4] = q[0] + q[1] - q[2] - q[3];
  assert(q[0] + q[1] - q[2] - q[3] - q[4] == 0.);
  assert((CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) 
    - CalcBodyToBaseCoordinates(model, q, idB5, X_s.r)).norm() < TEST_PREC);

  qd[0] = -1.;
  qd[1] = -1.;
  qd[2] = -2.;
  qd[3] = +1.;
  qd[4] = -1.;
  assert((CalcPointVelocity6D(model, q, qd, idB2, X_p.r)
    - CalcPointVelocity6D(model, q, qd, idB5, X_s.r)).norm() < TEST_PREC);

  CalcConstraintsJacobian(model, q, cs, G);

  err = G * qd;

  CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC);
}

TEST_FIXTURE(FourBarLinkage, TestFourBarLinkageConstraintsVelocityErrors) {
  VectorNd errd(VectorNd::Zero(cs.size()));
  VectorNd errdRef(VectorNd::Zero(cs.size()));
  MatrixNd G(cs.size(), model.dof_count);

  // Arms symmetric wrt y axis.
  q[0] = M_PI * 3 / 4;
  q[1] = -0.5 * M_PI;
  q[2] = M_PI - q[0];
  q[3] = -q[1];
  q[4] = q[0] + q[1] - q[2] - q[3];
  assert(q[0] + q[1] - q[2] - q[3] - q[4] == 0.);
  assert((CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) 
    - CalcBodyToBaseCoordinates(model, q, idB5, X_s.r)).norm() < TEST_PREC);

  qd[0] = -1.;
  qd[1] = -1.;
  qd[2] = -2.;
  qd[3] = +1.;
  qd[4] = -1.;

  CalcConstraintsVelocityError(model, q, qd, cs, errd);

  CHECK_ARRAY_CLOSE(errdRef, errd, cs.size(), TEST_PREC);

  // Invalid velocities.
  qd[0] = -1.;
  qd[1] = -1.;
  qd[2] = 0.;
  qd[3] = 0.;
  qd[4] = 0.;

  CalcConstraintsVelocityError(model, q, qd, cs, errd);
  CalcConstraintsJacobian(model, q, cs, G);
  errdRef = G * qd;
  CHECK_ARRAY_CLOSE(errdRef, errd, cs.size(), TEST_PREC);
}

TEST_FIXTURE(FourBarLinkage, TestFourBarLinkageQAssembly) {
  VectorNd weights(q.size());
  VectorNd err(cs.size());
  VectorNd errRef(VectorNd::Zero(cs.size()));

  weights[0] = 1.;
  weights[1] = 0.;
  weights[2] = 1.;
  weights[3] = 0.;
  weights[4] = 0.;

  VectorNd qRef = VectorNd::Zero(q.size());
  qRef[0] = M_PI * 3 / 4;
  qRef[1] = -0.5 * M_PI;
  qRef[2] = M_PI - qRef[0];
  qRef[3] = -qRef[1];
  qRef[4] = qRef[0] + qRef[1] - qRef[2] - qRef[3];
  assert(qRef[0] + qRef[1] - qRef[2] - qRef[3] - qRef[4] == 0.);

  bool success;  

  // Feasible initial guess.
  VectorNd qInit = VectorNd::Zero(q.size());
  qInit = qRef;
  
  success = CalcAssemblyQ(model, qInit, cs, q, weights, 1.e-12);
  CalcConstraintsPositionError(model, q, cs, err);

  CHECK_ARRAY_CLOSE(CalcBodyToBaseCoordinates(model, q, idB2, X_p.r)
    , CalcBodyToBaseCoordinates(model, q, idB5, X_s.r), 3, TEST_PREC);
  CHECK_CLOSE(inRange(q[0] + q[1]), inRange(q[2] + q[3] + q[4]), TEST_PREC);
  CHECK_CLOSE(qInit[0], q[0], TEST_PREC);
  CHECK_CLOSE(qInit[2], q[2], TEST_PREC);
  CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC);

  // Perturbed initial guess.
  qInit[0] = qRef[0];
  qInit[1] = qRef[1];
  qInit[2] = qRef[2];
  qInit[3] = qRef[3];
  qInit[4] = qRef[4] + 0.05;

  success = CalcAssemblyQ(model, qInit, cs, q, weights, 1.e-12);
  CalcConstraintsPositionError(model, q, cs, err);

  CHECK(success);
  CHECK_ARRAY_CLOSE(CalcBodyToBaseCoordinates(model, q, idB2, X_p.r)
    , CalcBodyToBaseCoordinates(model, q, idB5, X_s.r), 3, TEST_PREC);
  CHECK_CLOSE(inRange(q[0] + q[1]), inRange(q[2] + q[3] + q[4]), TEST_PREC);
  CHECK_CLOSE(qInit[0], q[0], TEST_PREC);
  CHECK_CLOSE(qInit[2], q[2], TEST_PREC);
  CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC);

  // Perturbed initial guess.
  qInit[0] = qRef[0] - 0.2;
  qInit[1] = qRef[1] - 0.;
  qInit[2] = qRef[2] + 0.1;
  qInit[3] = qRef[3] - 0.03;
  qInit[4] = qRef[4] + 0.05;

  success = CalcAssemblyQ(model, qInit, cs, q, weights, 1.e-12);
  CalcConstraintsPositionError(model, q, cs, err);

  CHECK(success);
  CHECK_ARRAY_CLOSE(CalcBodyToBaseCoordinates(model, q, idB2, X_p.r)
    , CalcBodyToBaseCoordinates(model, q, idB5, X_s.r), 3, TEST_PREC);
  CHECK_CLOSE(inRange(q[0] + q[1]), inRange(q[2] + q[3] + q[4]), TEST_PREC);
  CHECK_CLOSE(qInit[0], q[0], TEST_PREC);
  CHECK_CLOSE(qInit[2], q[2], TEST_PREC);
  CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC);

  // Perturbed initial guess.
  qInit[0] = qRef[0] + 0.01;
  qInit[1] = qRef[1] + 0.02;
  qInit[2] = qRef[2] - 0.03;
  qInit[3] = qRef[3] - 0.02;
  qInit[4] = qRef[4] + 0.01;

  success = CalcAssemblyQ(model, qInit, cs, q, weights, 1.e-12);
  CalcConstraintsPositionError(model, q, cs, err);

  CHECK(success);
  CHECK_ARRAY_CLOSE(CalcBodyToBaseCoordinates(model, q, idB2, X_p.r)
    , CalcBodyToBaseCoordinates(model, q, idB5, X_s.r), 3, TEST_PREC);
  CHECK_CLOSE(inRange(q[0] + q[1]), inRange(q[2] + q[3] + q[4]), TEST_PREC);
  CHECK_CLOSE(qInit[0], q[0], TEST_PREC);
  CHECK_CLOSE(qInit[2], q[2], TEST_PREC);
  CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC);
}

TEST_FIXTURE(FourBarLinkage, TestFourBarLinkageQDotAssembly) {
  VectorNd weights(q.size());

  weights[0] = 1.;
  weights[1] = 0.;
  weights[2] = 1.;
  weights[3] = 0.;
  weights[4] = 0.;

  q[0] = M_PI * 3 / 4;
  q[1] = -0.5 * M_PI;
  q[2] = M_PI - q[0];
  q[3] = -q[1];
  q[4] = q[0] + q[1] - q[2] - q[3];
  assert(q[0] + q[1] - q[2] - q[3] - q[4] == 0.);
  assert((CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) 
    - CalcBodyToBaseCoordinates(model, q, idB5, X_s.r)).norm() < TEST_PREC);

  VectorNd qdInit = VectorNd::Zero(q.size());
  qdInit[0] = 0.01;
  qdInit[1] = 0.5;
  qdInit[2] = -0.7;
  qdInit[3] = -0.5;
  qdInit[4] = 0.3;

  CalcAssemblyQDot(model, q, qdInit, cs, qd, weights);
  MatrixNd G(MatrixNd::Zero(cs.size(), q.size()));
  VectorNd err(VectorNd::Zero(cs.size()));
  VectorNd errRef(VectorNd::Zero(cs.size()));
  CalcConstraintsJacobian(model, q, cs, G);
  err = G * qd;

  CHECK_ARRAY_CLOSE(CalcPointVelocity6D(model, q, qd, idB2, X_p.r)
    , CalcPointVelocity6D(model, q, qd, idB5, X_s.r), 6, TEST_PREC);
  CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC);
  CHECK_CLOSE(qdInit[0], qd[0], TEST_PREC);
  CHECK_CLOSE(qdInit[2], qd[2], TEST_PREC);
}

TEST_FIXTURE(FourBarLinkage, TestFourBarLinkageForwardDynamics) {
  VectorNd qddDirect;
  VectorNd qddNullSpace;

  cs.SetSolver(LinearSolverColPivHouseholderQR);

#ifndef RBDL_USE_SIMPLE_MATH
  // SimpleMath has no solver that can solve the system in this configuration.
  // Configuration 1.

  q[0] = 0.;
  q[1] = 0.;
  q[2] = 0.;
  q[3] = 0.;
  q[4] = 0.;
  assert(q[0] + q[1] - q[2] - q[3] - q[4] == 0.);
  assert((CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) 
    - CalcBodyToBaseCoordinates(model, q, idB5, X_s.r)).norm() < TEST_PREC);

  qd[0] = 0.;
  qd[1] = 0.;
  qd[2] = 0.;
  qd[3] = 0.;
  qd[4] = 0.;
  assert(qd[0] + qd[1] - qd[2] - qd[3] - qd[4] == 0.);
  assert((CalcPointVelocity(model, q, qd, idB2, X_p.r)
    - CalcPointVelocity(model, q, qd, idB5, X_s.r)).norm() < TEST_PREC);

  tau[0] = 1.;
  tau[1] = -2.;
  tau[2] = 3.;
  tau[3] = -5.;
  tau[4] = 7.;

  qddDirect = VectorNd::Zero(q.size());
  ForwardDynamicsConstraintsDirect(model, q, qd, tau, cs, qddDirect);

  CHECK_ARRAY_CLOSE
    (CalcPointAcceleration6D(model, q, qd, qddDirect, idB2, X_p.r)
    , CalcPointAcceleration6D(model, q, qd, qddDirect, idB5, X_s.r)
    , 6, TEST_PREC);

  qddNullSpace = VectorNd::Zero(q.size());
  ForwardDynamicsConstraintsNullSpace(model, q, qd, tau, cs, qddNullSpace);

  CHECK_ARRAY_CLOSE
    (CalcPointAcceleration6D(model, q, qd, qddNullSpace, idB2, X_p.r)
    , CalcPointAcceleration6D(model, q, qd, qddNullSpace, idB5, X_s.r)
    , 6, TEST_PREC);
#endif

  // Configuration 2.

  q[0] = M_PI * 3 / 4;
  q[1] = -0.5 * M_PI;
  q[2] = M_PI - q[0];
  q[3] = -q[1];
  q[4] = q[0] + q[1] - q[2] - q[3];
  assert(q[0] + q[1] - q[2] - q[3] - q[4] == 0.);
  assert((CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) 
    - CalcBodyToBaseCoordinates(model, q, idB5, X_s.r)).norm() < TEST_PREC);

  qd[0] = -1.;
  qd[1] = -1.;
  qd[2] = -2.;
  qd[3] = +1.;
  qd[4] = -1.;
  assert(qd[0] + qd[1] - qd[2] - qd[3] - qd[4] == 0.);
  assert((CalcPointVelocity(model, q, qd, idB2, X_p.r)
    - CalcPointVelocity(model, q, qd, idB5, X_s.r)).norm() < TEST_PREC);

  tau[0] = 1.;
  tau[1] = -2.;
  tau[2] = 3.;
  tau[3] = -5.;
  tau[4] = 7.;

  qddDirect = VectorNd::Zero(q.size());
  ForwardDynamicsConstraintsDirect(model, q, qd, tau, cs, qddDirect);

  CHECK_ARRAY_CLOSE
    (CalcPointAcceleration6D(model, q, qd, qddDirect, idB2, X_p.r)
    , CalcPointAcceleration6D(model, q, qd, qddDirect, idB5, X_s.r)
    , 6, TEST_PREC);

  qddNullSpace = VectorNd::Zero(q.size());
  ForwardDynamicsConstraintsNullSpace(model, q, qd, tau, cs, qddNullSpace);

  CHECK_ARRAY_CLOSE
    (CalcPointAcceleration6D(model, q, qd, qddNullSpace, idB2, X_p.r)
    , CalcPointAcceleration6D(model, q, qd, qddNullSpace, idB5, X_s.r)
    , 6, TEST_PREC);

  // Note:
  // The Range Space Sparse method can't be used because the H matrix has a 0 on
  // the diagonal and the LTL factorization tries to divide by 0.

  // Note:
  // LinearSolverPartialPivLU does not work because the A matrix in the dynamic
  // system is not invertible.

  // Note:
  // LinearSolverHouseholderQR sometimes does not work well when the system is
  // in a singular configuration.
}

TEST_FIXTURE(FourBarLinkage, FourBarLinkageImpulse) {
  VectorNd qdPlusDirect(qd.size());
  VectorNd qdPlusRangeSpaceSparse(qd.size());
  VectorNd qdPlusNullSpace(qd.size());
  VectorNd errd(cs.size());

  q[0] = M_PI * 3 / 4;
  q[1] = -0.5 * M_PI;
  q[2] = M_PI - q[0];
  q[3] = -q[1];
  q[4] = q[0] + q[1] - q[2] - q[3];
  assert(q[0] + q[1] - q[2] - q[3] - q[4] == 0.);
  assert((CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) 
    - CalcBodyToBaseCoordinates(model, q, idB5, X_s.r)).norm() < TEST_PREC);

  cs.v_plus[0] = 1.;
  cs.v_plus[1] = 2.;
  cs.v_plus[2] = 3.;

  ComputeConstraintImpulsesDirect(model, q, qd, cs, qdPlusDirect);
  CalcConstraintsVelocityError(model, q, qdPlusDirect, cs, errd);

  CHECK_ARRAY_CLOSE(cs.v_plus, errd, cs.size(), TEST_PREC);

  cs.v_plus[0] = 0.;
  cs.v_plus[1] = 0.;
  cs.v_plus[2] = 0.;

  qd[0] = 1.;
  qd[1] = 2.;
  qd[2] = 3.;

  ComputeConstraintImpulsesDirect(model, q, qd, cs, qdPlusDirect);
  CalcConstraintsVelocityError(model, q, qdPlusDirect, cs, errd);

  CHECK_ARRAY_CLOSE(cs.v_plus, errd, cs.size(), TEST_PREC);
}



TEST_FIXTURE(SliderCrank3D, TestSliderCrank3DConstraintErrors) {
  VectorNd err(VectorNd::Zero(cs.size()));
  VectorNd errRef(VectorNd::Zero(cs.size()));
  Vector3d pos_p;
  Vector3d pos_s;
  Matrix3d rot_p;
  Matrix3d rot_s;
  Matrix3d rot_ps;
  Vector3d rotationVec;

  // Test in zero position.
  CalcConstraintsPositionError(model, q, cs, err);
  CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC);

  // Test in another configurations.

  q[0] = 0.4;
  q[1] = 0.25 * M_PI;
  q[2] = -0.25 * M_PI;
  q[3] = 0.01;
  q[4] = 0.01;

  CalcConstraintsPositionError(model, q, cs, err);

  pos_p = CalcBodyToBaseCoordinates(model, q, id_p, X_p.r);
  pos_s = CalcBodyToBaseCoordinates(model, q, id_s, X_s.r);
  rot_p = CalcBodyWorldOrientation(model, q, id_p).transpose() * X_p.E;
  rot_s = CalcBodyWorldOrientation(model, q, id_s).transpose() * X_s.E;
  rot_ps = rot_p.transpose() * rot_s;
  rotationVec = - 0.5 * Vector3d
    ( rot_ps(1,2) - rot_ps(2,1)
    , rot_ps(2,0) - rot_ps(0,2)
    , rot_ps(0,1) - rot_ps(1,0));
  errRef.block<3,1>(0,0) = pos_s - pos_p;
  errRef[3] = rotationVec[2];

  CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC);
}

TEST_FIXTURE(SliderCrank3D, TestSliderCrank3DConstraintJacobian) {
  MatrixNd G(MatrixNd::Zero(cs.size(), q.size()));

  // Test in zero position.

  G.setZero();
  CalcConstraintsJacobian(model, q, cs, G);

  VectorNd errRef(VectorNd::Zero(cs.size()));
  VectorNd err = G * qd;

  CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC);
}

TEST_FIXTURE(SliderCrank3D, TestSliderCrank3DConstraintsVelocityErrors) {
  VectorNd errd(VectorNd::Zero(cs.size()));
  VectorNd errdRef(VectorNd::Zero(cs.size()));
  MatrixNd G(cs.size(), model.dof_count);
  VectorNd qWeights(q.size());
  VectorNd qInit(q.size());
  bool success;

  // Compute assembled configuration.
  qWeights[0] = 1.;
  qWeights[1] = 1.;
  qWeights[2] = 1.;
  qWeights[3] = 1.;
  qWeights[4] = 1.;

  qInit[0] = 0.4;
  qInit[1] = 0.25 * M_PI;
  qInit[2] = -0.25 * M_PI;
  qInit[3] = 0.1;
  qInit[4] = 0.1;

  success = CalcAssemblyQ(model, qInit, cs, q, qWeights, TEST_PREC);
  assert(success);

  // Some random velocity.
  qd[0] = -0.2;
  qd[1] = 0.1 * M_PI;
  qd[2] = -0.1 * M_PI;
  qd[3] = 0.;
  qd[4] = 0.1 * M_PI;

  CalcConstraintsVelocityError(model, q, qd, cs, errd);
  CalcConstraintsJacobian(model, q, cs, G);
  errdRef = G * qd;

  CHECK_ARRAY_CLOSE(errdRef, errd, cs.size(), TEST_PREC);
}

TEST_FIXTURE(SliderCrank3D, TestSliderCrank3DAssemblyQ) {
  VectorNd weights(q.size());
  VectorNd qInit(q.size());

  Vector3d pos_p;
  Vector3d pos_s;
  Matrix3d rot_p;
  Matrix3d rot_s;
  Matrix3d rot_ps;

  bool success;

  weights[0] = 1.;
  weights[1] = 1.;
  weights[2] = 1.;
  weights[3] = 1.;
  weights[4] = 1.;

  qInit[0] = 0.4;
  qInit[1] = 0.25 * M_PI;
  qInit[2] = -0.25 * M_PI;
  qInit[3] = 0.1;
  qInit[4] = 0.1;

  success = CalcAssemblyQ(model, qInit, cs, q, weights, TEST_PREC);
  pos_p = CalcBodyToBaseCoordinates(model, q, id_p, X_p.r);
  pos_s = CalcBodyToBaseCoordinates(model, q, id_s, X_s.r);
  rot_p = CalcBodyWorldOrientation(model, q, id_p).transpose() * X_p.E;
  rot_s = CalcBodyWorldOrientation(model, q, id_s).transpose() * X_s.E;
  rot_ps = rot_p.transpose() * rot_s;

  CHECK(success);
  CHECK_ARRAY_CLOSE(pos_p, pos_s, 3, TEST_PREC);
  CHECK_CLOSE(0., rot_ps(0,1) - rot_ps(1,0), TEST_PREC);
}

TEST_FIXTURE(SliderCrank3D, TestSliderCrank3DAssemblyQDot) {
  VectorNd qWeights(q.size());
  VectorNd qdWeights(q.size());
  VectorNd qInit(q.size());
  VectorNd qdInit(q.size());

  SpatialVector vel_p;
  SpatialVector vel_s;

  bool success;

  qWeights[0] = 1.;
  qWeights[1] = 1.;
  qWeights[2] = 1.;
  qWeights[3] = 1.;
  qWeights[4] = 1.;

  qInit[0] = 0.4;
  qInit[1] = 0.25 * M_PI;
  qInit[2] = -0.25 * M_PI;
  qInit[3] = 0.1;
  qInit[4] = 0.1;

  qdWeights[0] = 1.;
  qdWeights[1] = 0.;
  qdWeights[2] = 0.;
  qdWeights[3] = 0.;
  qdWeights[4] = 0.;

  qdInit[0] = -0.2;
  qdInit[1] = 0.1 * M_PI;
  qdInit[2] = -0.1 * M_PI;
  qdInit[3] = 0.;
  qdInit[4] = 0.1 * M_PI;

  success = CalcAssemblyQ(model, qInit, cs, q, qWeights, TEST_PREC);
  assert(success);

  CalcAssemblyQDot(model, q, qdInit, cs, qd, qdWeights);

  vel_p = CalcPointVelocity6D(model, q, qd, id_p, X_p.r);
  vel_s = CalcPointVelocity6D(model, q, qd, id_s, X_s.r);

  for(size_t i = 2; i < 6; ++i) {
    CHECK_CLOSE(vel_p[i], vel_s[i], TEST_PREC);
  }
  CHECK_CLOSE(qdInit[0], qd[0], TEST_PREC);
}

TEST_FIXTURE(SliderCrank3D, TestSliderCrank3DForwardDynamics) {
  VectorNd qWeights(q.size());
  VectorNd qdWeights(q.size());
  VectorNd qInit(q.size());
  VectorNd qdInit(q.size());

  SpatialVector acc_p;
  SpatialVector acc_s;

  bool success;

#ifndef RBDL_USE_SIMPLE_MATH
  // The SimpleMath solver cannot solve the system close to a singular
  // configuration.
  // Test with zero q and qdot.

  tau[0] = 0.12;
  tau[1] = -0.3;
  tau[2] = 0.05;
  tau[3] = 0.7;
  tau[4] = -0.1;

  ForwardDynamicsConstraintsDirect(model, q, qd, tau, cs, qdd);

  acc_p = CalcPointAcceleration6D(model, q, qd, qdd, id_p, X_p.r);
  acc_s = CalcPointAcceleration6D(model, q, qd, qdd, id_s, X_s.r);

  for(size_t i = 2; i < 6; ++i) {
    CHECK_CLOSE(acc_p[i], acc_s[i], TEST_PREC);
  }

  ForwardDynamicsConstraintsNullSpace(model, q, qd, tau, cs, qdd);

  acc_p = CalcPointAcceleration6D(model, q, qd, qdd, id_p, X_p.r);
  acc_s = CalcPointAcceleration6D(model, q, qd, qdd, id_s, X_s.r);

  for(size_t i = 2; i < 6; ++i) {
    CHECK_CLOSE(acc_p[i], acc_s[i], TEST_PREC);
  }

  ForwardDynamicsConstraintsRangeSpaceSparse(model, q, qd, tau, cs, qdd);

  acc_p = CalcPointAcceleration6D(model, q, qd, qdd, id_p, X_p.r);
  acc_s = CalcPointAcceleration6D(model, q, qd, qdd, id_s, X_s.r);

  for(size_t i = 2; i < 6; ++i) {
    CHECK_CLOSE(acc_p[i], acc_s[i], TEST_PREC);
  }
#endif

  // Compute non-zero assembly q and qdot;

  qWeights[0] = 1.;
  qWeights[1] = 1.;
  qWeights[2] = 1.;
  qWeights[3] = 1.;
  qWeights[4] = 1.;

  qInit[0] = 0.4;
  qInit[1] = 0.25 * M_PI;
  qInit[2] = -0.25 * M_PI;
  qInit[3] = 0.1;
  qInit[4] = 0.1;

  qdWeights[0] = 1.;
  qdWeights[1] = 0.;
  qdWeights[2] = 0.;
  qdWeights[3] = 0.;
  qdWeights[4] = 0.;

  qdInit[0] = -0.2;
  qdInit[1] = 0.1 * M_PI;
  qdInit[2] = -0.1 * M_PI;
  qdInit[3] = 0.;
  qdInit[4] = 0.1 * M_PI;

  qdInit.setZero();

  success = CalcAssemblyQ(model, qInit, cs, q, qWeights, TEST_PREC);
  assert(success);
  CalcAssemblyQDot(model, q, qdInit, cs, qd, qdWeights);

  Matrix3d rot_ps 
    = (CalcBodyWorldOrientation(model, q, id_p).transpose() * X_p.E).transpose()
    * CalcBodyWorldOrientation(model, q, id_s).transpose() * X_s.E;
  assert((CalcBodyToBaseCoordinates(model, q, id_p, X_p.r)
    - CalcBodyToBaseCoordinates(model, q, id_p, X_p.r)).norm() < TEST_PREC);
  assert(rot_ps(0,1) - rot_ps(0,1) < TEST_PREC);
  assert((CalcPointVelocity6D(model, q, qd, id_p, X_p.r)
    -CalcPointVelocity6D(model, q, qd, id_p, X_p.r)).norm() < TEST_PREC);

  // Test with non-zero q and qdot.

  ForwardDynamicsConstraintsDirect(model, q, qd, tau, cs, qdd);

  acc_p = CalcPointAcceleration6D(model, q, qd, qdd, id_p, X_p.r);
  acc_s = CalcPointAcceleration6D(model, q, qd, qdd, id_s, X_s.r);

  for(size_t i = 2; i < 6; ++i) {
    CHECK_CLOSE(acc_p[i], acc_s[i], TEST_PREC);
  }

  ForwardDynamicsConstraintsNullSpace(model, q, qd, tau, cs, qdd);

  acc_p = CalcPointAcceleration6D(model, q, qd, qdd, id_p, X_p.r);
  acc_s = CalcPointAcceleration6D(model, q, qd, qdd, id_s, X_s.r);

  for(size_t i = 2; i < 6; ++i) {
    CHECK_CLOSE(acc_p[i], acc_s[i], TEST_PREC);
  }

  ForwardDynamicsConstraintsRangeSpaceSparse(model, q, qd, tau, cs, qdd);

  acc_p = CalcPointAcceleration6D(model, q, qd, qdd, id_p, X_p.r);
  acc_s = CalcPointAcceleration6D(model, q, qd, qdd, id_s, X_s.r);

  for(size_t i = 2; i < 6; ++i) {
    CHECK_CLOSE(acc_p[i], acc_s[i], TEST_PREC);
  }
}

TEST_FIXTURE(SliderCrank3D, TestSliderCrank3DImpulse) {
  VectorNd qdPlusDirect(qd.size());
  VectorNd qdPlusRangeSpaceSparse(qd.size());
  VectorNd qdPlusNullSpace(qd.size());
  VectorNd errdDirect(cs.size());
  VectorNd errdSpaceSparse(cs.size());
  VectorNd errdNullSpace(cs.size());
  
  VectorNd qWeights(q.size());
  qWeights[0] = 1.;
  qWeights[1] = 1.;
  qWeights[2] = 1.;
  qWeights[3] = 1.;
  qWeights[4] = 1.;

  VectorNd qInit(q.size());
  qInit[0] = 0.4;
  qInit[1] = 0.25 * M_PI;
  qInit[2] = -0.25 * M_PI;
  qInit[3] = 0.1;
  qInit[4] = 0.1;

  bool success = CalcAssemblyQ(model, qInit, cs, q, qWeights, TEST_PREC);
  assert(success);

  cs.v_plus[0] = 1.;
  cs.v_plus[1] = 2.;
  cs.v_plus[2] = 3.;
  cs.v_plus[3] = 4.;
  
  ComputeConstraintImpulsesDirect(model, q, qd, cs, qdPlusDirect);
  CalcConstraintsVelocityError(model, q, qdPlusDirect, cs, errdDirect);
  
  CHECK_ARRAY_CLOSE(cs.v_plus, errdDirect, cs.size(), TEST_PREC);

  ComputeConstraintImpulsesRangeSpaceSparse(model, q, qd, cs
    , qdPlusRangeSpaceSparse);
  CalcConstraintsVelocityError(model, q, qdPlusRangeSpaceSparse, cs
    , errdSpaceSparse);
  
  CHECK_ARRAY_CLOSE(cs.v_plus, errdSpaceSparse, cs.size(), TEST_PREC);

  ComputeConstraintImpulsesNullSpace(model, q, qd, cs, qdPlusNullSpace);
  CalcConstraintsVelocityError(model, q, qdPlusNullSpace, cs, errdNullSpace);

  CHECK_ARRAY_CLOSE(cs.v_plus, errdNullSpace, cs.size(), TEST_PREC);

  cs.v_plus[0] = 0.;
  cs.v_plus[1] = 0.;
  cs.v_plus[2] = 0.;
  cs.v_plus[3] = 0.;

  qd[0] = 1.;
  qd[1] = 2.;
  qd[2] = 3.;

  ComputeConstraintImpulsesDirect(model, q, qd, cs, qdPlusDirect);
  CalcConstraintsVelocityError(model, q, qdPlusDirect, cs, errdDirect);
  
  CHECK_ARRAY_CLOSE(cs.v_plus, errdDirect, cs.size(), TEST_PREC);

  ComputeConstraintImpulsesRangeSpaceSparse(model, q, qd, cs
    , qdPlusRangeSpaceSparse);
  CalcConstraintsVelocityError(model, q, qdPlusRangeSpaceSparse, cs
    , errdSpaceSparse);
  
  CHECK_ARRAY_CLOSE(cs.v_plus, errdSpaceSparse, cs.size(), TEST_PREC);

  ComputeConstraintImpulsesNullSpace(model, q, qd, cs, qdPlusNullSpace);
  CalcConstraintsVelocityError(model, q, qdPlusNullSpace, cs, errdNullSpace);

  CHECK_ARRAY_CLOSE(cs.v_plus, errdNullSpace, cs.size(), TEST_PREC);
}

TEST_FIXTURE(FloatingFourBarLinkage
  , TestFloatingFourBarLinkageConstraintErrors) {

  VectorNd err = VectorNd::Zero(cs.size());
  Vector3d pos0;
  Vector3d pos1;
  Vector3d pos2;
  Vector3d posErr;
  Matrix3d rot_p;
  double angleErr;

  // Test in zero position.
  q[0] = 0.;
  q[1] = 0.;
  q[2] = 0.;
  q[3] = 0.;
  q[4] = 0.;
  q[5] = 0.;
  q[6] = 0.;
  q[7] = 0.;

  CalcConstraintsPositionError(model, q, cs, err);

  CHECK_CLOSE(0., err[0], TEST_PREC);
  CHECK_CLOSE(0., err[1], TEST_PREC);
  CHECK_CLOSE(0., err[2], TEST_PREC);
  CHECK_CLOSE(0., err[3], TEST_PREC);
  CHECK_CLOSE(0., err[4], TEST_PREC);
  CHECK_CLOSE(0., err[5], TEST_PREC);

  // Test in non-zero position.
  q[0] = 1.;
  q[1] = 2.;
  q[2] = 3.;
  q[3] = M_PI * 3 / 4;
  q[4] = -M_PI;
  q[5] = M_PI - q[3];
  q[6] = -q[4];
  q[7] = 0.;
  angleErr = sin(-0.5 * M_PI);

  pos0 = CalcBodyToBaseCoordinates(model, q, idB0, Vector3d::Zero());
  pos1 = CalcBodyToBaseCoordinates(model, q, idB2, X_p.r);
  pos2 = CalcBodyToBaseCoordinates(model, q, idB5, X_s.r);
  rot_p = CalcBodyWorldOrientation(model, q, idB2).transpose() * X_p.E;
  posErr = rot_p.transpose() * (pos2 - pos1);

  assert(std::fabs(posErr[1]) < TEST_PREC);
  assert(std::fabs(posErr[2]) < TEST_PREC);

  CalcConstraintsPositionError(model, q, cs, err);

  CHECK_ARRAY_CLOSE(Vector3d(1.,2.,3.), pos0, 3, TEST_PREC);
  CHECK_CLOSE(posErr[0], err[3], TEST_PREC);
  CHECK_CLOSE(0., err[4], TEST_PREC);
  CHECK_CLOSE(angleErr, err[5], TEST_PREC);

  // Test in non-zero position.
  q[0] = 1.;
  q[1] = 2.;
  q[2] = 3.;
  q[3] = 0.;
  q[4] = 0.;
  q[5] = M_PI + 0.1;
  q[6] = 0.;
  q[7] = 0.;
  angleErr = sin(-q[3] - q[4] + q[5] + q[6] + q[7]);

  pos0 = CalcBodyToBaseCoordinates(model, q, idB0, Vector3d::Zero());
  pos1 = CalcBodyToBaseCoordinates(model, q, idB2, X_p.r);
  pos2 = CalcBodyToBaseCoordinates(model, q, idB5, X_s.r);
  rot_p = CalcBodyWorldOrientation(model, q, idB2).transpose() * X_p.E;
  posErr = rot_p.transpose() * (pos2 - pos1);

  CalcConstraintsPositionError(model, q, cs, err);

  CHECK_ARRAY_CLOSE(Vector3d(1.,2.,3.), pos0, 3, TEST_PREC);
  CHECK_CLOSE(posErr[0], err[3], TEST_PREC);
  CHECK_CLOSE(posErr[1], err[4], TEST_PREC);
  CHECK_CLOSE(angleErr, err[5], TEST_PREC);

  // Test in non-zero position.
  q[0] = 1.;
  q[1] = 2.;
  q[2] = 3.;
  q[3] = 0.8;
  q[4] = -0.4;
  q[5] = M_PI - q[3];
  q[6] = -q[4];
  q[7] = 0.;
  angleErr = sin(-q[3] - q[4] + q[5] + q[6] + q[7]);

  pos0 = CalcBodyToBaseCoordinates(model, q, idB0, Vector3d::Zero());
  pos1 = CalcBodyToBaseCoordinates(model, q, idB2, X_p.r);
  pos2 = CalcBodyToBaseCoordinates(model, q, idB5, X_s.r);
  rot_p = CalcBodyWorldOrientation(model, q, idB2).transpose() * X_p.E;
  posErr = rot_p.transpose() * (pos2 - pos1);

  CalcConstraintsPositionError(model, q, cs, err);

  CHECK_ARRAY_CLOSE(Vector3d(1.,2.,3.), pos0, 3, TEST_PREC);
  CHECK_CLOSE(posErr[0], err[3], TEST_PREC);
  CHECK_CLOSE(posErr[1], err[4], TEST_PREC);
  CHECK_CLOSE(angleErr, err[5], TEST_PREC);
}

TEST_FIXTURE(FloatingFourBarLinkage
  , TestFloatingFourBarLinkageConstraintJacobian) {

  MatrixNd G(MatrixNd::Zero(cs.size(), q.size()));
  VectorNd err(VectorNd::Zero(cs.size()));
  VectorNd errRef(VectorNd::Zero(cs.size()));

  // Zero Q configuration, both arms of the 4-bar laying on the x-axis
  q[0] = 0.;
  q[1] = 0.;
  q[2] = 0.;
  q[3] = 0.;
  q[4] = 0.;
  q[5] = 0.;
  q[6] = 0.;
  q[7] = 0.;
  assert(q[3] + q[4] - q[5] - q[6] - q[7] == 0.);
  assert((CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) 
    - CalcBodyToBaseCoordinates(model, q, idB5, X_s.r)).norm() < TEST_PREC);

  qd[0] = 0.;
  qd[1] = 0.;
  qd[2] = 0.;
  qd[3] = -1.;
  qd[4] = -1.;
  qd[5] = -1.;
  qd[6] = -1.;
  qd[7] = 0.;
  assert((CalcPointVelocity6D(model, q, qd, idB2, X_p.r)
    - CalcPointVelocity6D(model, q, qd, idB5, X_s.r)).norm() < TEST_PREC);

  CalcConstraintsJacobian(model, q, cs, G);

  err = G * qd;

  CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC);

  // Both arms of the 4-bar laying on the y-axis
  q[0] = 0.;
  q[1] = 0.;
  q[2] = 0.;
  q[3] = 0.5 * M_PI;
  q[4] = 0.;
  q[5] = 0.5 * M_PI;
  q[6] = 0.;
  q[7] = 0.;
  assert(q[3] + q[4] - q[5] - q[6] - q[7] == 0.);
  assert((CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) 
    - CalcBodyToBaseCoordinates(model, q, idB5, X_s.r)).norm() < TEST_PREC);

  qd[0] = 0.;
  qd[1] = 0.;
  qd[2] = 0.;
  qd[3] = -1.;
  qd[4] = -1.;
  qd[5] = -1.;
  qd[6] = -1.;
  qd[7] = 0.;
  assert((CalcPointVelocity6D(model, q, qd, idB2, X_p.r)
    - CalcPointVelocity6D(model, q, qd, idB5, X_s.r)).norm() < TEST_PREC);

  CalcConstraintsJacobian(model, q, cs, G);

  err = G * qd;

  CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC);

  // Arms symmetric wrt y axis.
  q[0] = 1.;
  q[1] = 2.;
  q[2] = 3.;
  q[3] = M_PI * 3 / 4;
  q[4] = -0.5 * M_PI;
  q[5] = M_PI - q[3];
  q[6] = -q[4];
  q[7] = q[3] + q[4] - q[5] - q[6];
  assert(q[3] + q[4] - q[5] - q[6] - q[7] == 0.);
  assert((CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) 
    - CalcBodyToBaseCoordinates(model, q, idB5, X_s.r)).norm() < TEST_PREC);

  qd[0] = 0.;
  qd[1] = 0.;
  qd[2] = 0.;
  qd[3] = -1.;
  qd[4] = -1.;
  qd[5] = -2.;
  qd[6] = +1.;
  qd[7] = -1.;
  assert((CalcPointVelocity6D(model, q, qd, idB2, X_p.r)
    - CalcPointVelocity6D(model, q, qd, idB5, X_s.r)).norm() < TEST_PREC);

  CalcConstraintsJacobian(model, q, cs, G);

  err = G * qd;

  CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC);
}

TEST_FIXTURE(FloatingFourBarLinkage
  , TestFloatingFourBarLinkageConstraintsVelocityErrors) {

  VectorNd errd(VectorNd::Zero(cs.size()));
  VectorNd errdRef(VectorNd::Zero(cs.size()));
  MatrixNd G(cs.size(), model.dof_count);

  // Arms symmetric wrt y axis.
  q[0] = 1.;
  q[1] = 2.;
  q[2] = 3.;
  q[3] = M_PI * 3 / 4;
  q[4] = -0.5 * M_PI;
  q[5] = M_PI - q[3];
  q[6] = -q[4];
  q[7] = q[3] + q[4] - q[5] - q[6];

  assert(q[3] + q[4] - q[5] - q[6] - q[7] == 0.);
  assert((CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) 
    - CalcBodyToBaseCoordinates(model, q, idB5, X_s.r)).norm() < TEST_PREC);

  qd[0] = 0.;
  qd[1] = 0.;
  qd[2] = 0.;
  qd[3] = -1.;
  qd[4] = -1.;
  qd[5] = -2.;
  qd[6] = +1.;
  qd[7] = -1.;

  CalcConstraintsVelocityError(model, q, qd, cs, errd);
  CHECK_ARRAY_CLOSE(errdRef, errd, cs.size(), TEST_PREC);

  // Invalid velocities.
  qd[0] = -1.;
  qd[1] = -1.;
  qd[2] = 0.;
  qd[3] = -1.;
  qd[4] = -1.;
  qd[5] = 0.;
  qd[6] = 0.;
  qd[7] = 0.;

  CalcConstraintsVelocityError(model, q, qd, cs, errd);
  CalcConstraintsJacobian(model, q, cs, G);
  errdRef = G * qd;
  CHECK_ARRAY_CLOSE(errdRef, errd, cs.size(), TEST_PREC);
}

TEST_FIXTURE(FloatingFourBarLinkage, TestFloatingFourBarLinkageQAssembly) {
  VectorNd weights(q.size());
  VectorNd err(cs.size());
  VectorNd errRef(VectorNd::Zero(cs.size()));

  weights[0] = 0.;
  weights[1] = 0.;
  weights[2] = 0.;
  weights[3] = 1.;
  weights[4] = 0.;
  weights[5] = 1.;
  weights[6] = 0.;
  weights[7] = 0.;

  VectorNd qRef = VectorNd::Zero(q.size());
  qRef[0] = 1.;
  qRef[1] = 2.;
  qRef[2] = 3.;
  qRef[3] = M_PI * 3 / 4;
  qRef[4] = -0.5 * M_PI;
  qRef[5] = M_PI - qRef[3];
  qRef[6] = -qRef[4];
  qRef[7] = qRef[3] + qRef[4] - qRef[5] - qRef[6];

  assert(qRef[3] + qRef[4] - qRef[5] - qRef[6] - qRef[7] == 0.);
  assert((CalcBodyToBaseCoordinates(model, qRef, idB2, X_p.r) 
    - CalcBodyToBaseCoordinates(model, qRef, idB5, X_s.r)).norm() < TEST_PREC);

  bool success;  

  // Feasible initial guess.
  VectorNd qInit = VectorNd::Zero(q.size());
  qInit = qRef;
  
  success = CalcAssemblyQ(model, qInit, cs, q, weights, 1.e-12);
  CalcConstraintsPositionError(model, q, cs, err);

  CHECK_ARRAY_CLOSE(CalcBodyToBaseCoordinates(model, q, idB2, X_p.r)
    , CalcBodyToBaseCoordinates(model, q, idB5, X_s.r), 3, TEST_PREC);
  CHECK_CLOSE(inRange(q[3] + q[4]), inRange(q[5] + q[6] + q[7]), TEST_PREC);
  CHECK_CLOSE(qInit[3], q[3], TEST_PREC);
  CHECK_CLOSE(qInit[5], q[5], TEST_PREC);
  CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC);

  // Perturbed initial guess.
  qInit[3] = qRef[3];
  qInit[4] = qRef[4];
  qInit[5] = qRef[5];
  qInit[6] = qRef[6];
  qInit[7] = qRef[7] + 0.05;

  success = CalcAssemblyQ(model, qInit, cs, q, weights, 1.e-12);
  CalcConstraintsPositionError(model, q, cs, err);

  CHECK(success);
  CHECK_ARRAY_CLOSE(CalcBodyToBaseCoordinates(model, q, idB2, X_p.r)
    , CalcBodyToBaseCoordinates(model, q, idB5, X_s.r), 3, TEST_PREC);
  CHECK_CLOSE(inRange(q[3] + q[4]), inRange(q[5] + q[6] + q[7]), TEST_PREC);
  CHECK_CLOSE(qInit[3], q[3], TEST_PREC);
  CHECK_CLOSE(qInit[5], q[5], TEST_PREC);
  CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC);

  // Perturbed initial guess.
  qInit[3] = qRef[3] - 0.2;
  qInit[4] = qRef[4] - 0.;
  qInit[5] = qRef[5] + 0.1;
  qInit[6] = qRef[6] - 0.03;
  qInit[7] = qRef[7] + 0.05;

  success = CalcAssemblyQ(model, qInit, cs, q, weights, 1.e-12);
  CalcConstraintsPositionError(model, q, cs, err);

  CHECK(success);
  CHECK_ARRAY_CLOSE(CalcBodyToBaseCoordinates(model, q, idB2, X_p.r)
    , CalcBodyToBaseCoordinates(model, q, idB5, X_s.r), 3, TEST_PREC);
  CHECK_CLOSE(inRange(q[3] + q[4]), inRange(q[5] + q[6] + q[7]), TEST_PREC);
  CHECK_CLOSE(qInit[3], q[3], TEST_PREC);
  CHECK_CLOSE(qInit[5], q[5], TEST_PREC);
  CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC);

  // Perturbed initial guess.
  qInit[3] = qRef[3] + 0.01;
  qInit[4] = qRef[4] + 0.02;
  qInit[5] = qRef[5] - 0.03;
  qInit[6] = qRef[6] - 0.02;
  qInit[7] = qRef[7] + 0.01;

  success = CalcAssemblyQ(model, qInit, cs, q, weights, 1.e-12);
  CalcConstraintsPositionError(model, q, cs, err);

  CHECK(success);
  CHECK_ARRAY_CLOSE(CalcBodyToBaseCoordinates(model, q, idB2, X_p.r)
    , CalcBodyToBaseCoordinates(model, q, idB5, X_s.r), 3, TEST_PREC);
  CHECK_CLOSE(inRange(q[3] + q[4]), inRange(q[5] + q[6] + q[7]), TEST_PREC);
  CHECK_CLOSE(qInit[3], q[3], TEST_PREC);
  CHECK_CLOSE(qInit[5], q[5], TEST_PREC);
  CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC);
}

TEST_FIXTURE(FloatingFourBarLinkage, TestFloatingFourBarLinkageQDotAssembly) {
  VectorNd weights(q.size());

  weights[0] = 0.;
  weights[1] = 0.;
  weights[2] = 0.;
  weights[3] = 1.;
  weights[4] = 0.;
  weights[5] = 1.;
  weights[6] = 0.;
  weights[7] = 0.;

  q[0] = 1.;
  q[1] = 2.;
  q[2] = 3.;
  q[3] = M_PI * 3 / 4;
  q[4] = -0.5 * M_PI;
  q[5] = M_PI - q[3];
  q[6] = -q[4];
  q[7] = q[3] + q[4] - q[5] - q[6];

  assert(q[3] + q[4] - q[5] - q[6] - q[7] == 0.);
  assert((CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) 
    - CalcBodyToBaseCoordinates(model, q, idB5, X_s.r)).norm() < TEST_PREC);

  VectorNd qdInit = VectorNd::Zero(q.size());
  qdInit[0] = 1.;
  qdInit[1] = 2.;
  qdInit[2] = 3.;
  qdInit[3] = 0.01;
  qdInit[4] = 0.5;
  qdInit[5] = -0.7;
  qdInit[6] = -0.5;
  qdInit[7] = 0.3;

  CalcAssemblyQDot(model, q, qdInit, cs, qd, weights);
  MatrixNd G(MatrixNd::Zero(cs.size(), q.size()));
  VectorNd err(VectorNd::Zero(cs.size()));
  VectorNd errRef(VectorNd::Zero(cs.size()));
  CalcConstraintsJacobian(model, q, cs, G);
  err = G * qd;

  CHECK_ARRAY_CLOSE(CalcPointVelocity6D(model, q, qd, idB2, X_p.r)
    , CalcPointVelocity6D(model, q, qd, idB5, X_s.r), 6, TEST_PREC);
  CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC);
  CHECK_CLOSE(qdInit[3], qd[3], TEST_PREC);
  CHECK_CLOSE(qdInit[5], qd[5], TEST_PREC);
}

TEST_FIXTURE(FloatingFourBarLinkage
  , TestFloatingFourBarLinkageForwardDynamics) {

  VectorNd qddDirect;
  VectorNd qddNullSpace;

  cs.SetSolver(LinearSolverColPivHouseholderQR);

#ifndef RBDL_USE_SIMPLE_MATH
  // The SimpleMath solver cannot solve the system close to a singular
  // configuration.
  // Configuration 1.
  q[0] = 0.;
  q[1] = 0.;
  q[2] = 0.;
  q[3] = 0.;
  q[4] = 0.;
  q[5] = 0.;
  q[6] = 0.;
  q[7] = 0.;
  assert(q[3] + q[4] - q[5] - q[6] - q[7] == 0.);
  assert((CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) 
    - CalcBodyToBaseCoordinates(model, q, idB5, X_s.r)).norm() < TEST_PREC);

  qd[0] = 0.;
  qd[1] = 0.;
  qd[2] = 0.;
  qd[3] = 0.;
  qd[4] = 0.;
  qd[5] = 0.;
  qd[6] = 0.;
  qd[7] = 0.;
  assert(qd[3] + qd[4] - qd[5] - qd[6] - qd[7] == 0.);
  assert((CalcPointVelocity(model, q, qd, idB2, X_p.r)
    - CalcPointVelocity(model, q, qd, idB5, X_s.r)).norm() < TEST_PREC);

  tau[0] = 0.;
  tau[1] = 0.;
  tau[2] = 0.;
  tau[3] = 1.;
  tau[4] = -2.;
  tau[5] = 3.;
  tau[6] = -5.;
  tau[7] = 7.;

  qddDirect = VectorNd::Zero(q.size());
  ForwardDynamicsConstraintsDirect(model, q, qd, tau, cs, qddDirect);

  CHECK_ARRAY_CLOSE
    (CalcPointAcceleration6D(model, q, qd, qddDirect, idB2, X_p.r)
    , CalcPointAcceleration6D(model, q, qd, qddDirect, idB5, X_s.r)
    , 6, TEST_PREC);

  qddNullSpace = VectorNd::Zero(q.size());
  ForwardDynamicsConstraintsNullSpace(model, q, qd, tau, cs, qddNullSpace);

  CHECK_ARRAY_CLOSE
    (CalcPointAcceleration6D(model, q, qd, qddNullSpace, idB2, X_p.r)
    , CalcPointAcceleration6D(model, q, qd, qddNullSpace, idB5, X_s.r)
    , 6, TEST_PREC);
#endif

  // Configuration 2.
  q[0] = 1.;
  q[1] = 2.;
  q[2] = 3.;
  q[3] = M_PI * 3 / 4;
  q[4] = -0.5 * M_PI;
  q[5] = M_PI - q[3];
  q[6] = -q[4];
  q[7] = q[3] + q[4] - q[5] - q[6];

  assert(q[3] + q[4] - q[5] - q[6] - q[7] == 0.);
  assert((CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) 
    - CalcBodyToBaseCoordinates(model, q, idB5, X_s.r)).norm() < TEST_PREC);

  qd[0] = 0.;
  qd[1] = 0.;
  qd[2] = 0.;
  qd[3] = -1.;
  qd[4] = -1.;
  qd[5] = -2.;
  qd[6] = +1.;
  qd[7] = -1.;
  assert(qd[3] + qd[4] - qd[5] - qd[6] - qd[7] == 0.);
  assert((CalcPointVelocity(model, q, qd, idB2, X_p.r)
    - CalcPointVelocity(model, q, qd, idB5, X_s.r)).norm() < TEST_PREC);

  tau[0] = 0.;
  tau[1] = 0.;
  tau[2] = 0.;
  tau[3] = 1.;
  tau[4] = -2.;
  tau[5] = 3.;
  tau[6] = -5.;
  tau[7] = 7.;

  qddDirect = VectorNd::Zero(q.size());
  ForwardDynamicsConstraintsDirect(model, q, qd, tau, cs, qddDirect);

  CHECK_ARRAY_CLOSE
    (CalcPointAcceleration6D(model, q, qd, qddDirect, idB2, X_p.r)
    , CalcPointAcceleration6D(model, q, qd, qddDirect, idB5, X_s.r)
    , 6, TEST_PREC);

  qddNullSpace = VectorNd::Zero(q.size());
  ForwardDynamicsConstraintsNullSpace(model, q, qd, tau, cs, qddNullSpace);

  CHECK_ARRAY_CLOSE
    (CalcPointAcceleration6D(model, q, qd, qddNullSpace, idB2, X_p.r)
    , CalcPointAcceleration6D(model, q, qd, qddNullSpace, idB5, X_s.r)
    , 6, TEST_PREC);
}

TEST_FIXTURE(FloatingFourBarLinkage, TestFloatingFourBarLinkageImpulse) {
  VectorNd qdPlusDirect(qd.size());
  VectorNd qdPlusRangeSpaceSparse(qd.size());
  VectorNd qdPlusNullSpace(qd.size());
  VectorNd errd(cs.size());

  q[0] = 1.;
  q[1] = 2.;
  q[2] = 3.;
  q[3] = M_PI * 3 / 4;
  q[4] = -0.5 * M_PI;
  q[5] = M_PI - q[3];
  q[6] = -q[4];
  q[7] = q[3] + q[4] - q[5] - q[6];

  assert(q[3] + q[4] - q[5] - q[6] - q[7] == 0.);
  assert((CalcBodyToBaseCoordinates(model, q, idB2, X_p.r) 
    - CalcBodyToBaseCoordinates(model, q, idB5, X_s.r)).norm() < TEST_PREC);

  cs.v_plus[0] = 1.;
  cs.v_plus[1] = 2.;
  cs.v_plus[2] = 3.;
  cs.v_plus[3] = 4.;
  cs.v_plus[4] = 5.;
  cs.v_plus[5] = 6.;

  ComputeConstraintImpulsesDirect(model, q, qd, cs, qdPlusDirect);
  CalcConstraintsVelocityError(model, q, qdPlusDirect, cs, errd);

  CHECK_ARRAY_CLOSE(cs.v_plus, errd, cs.size(), TEST_PREC);

  cs.v_plus[0] = 0.;
  cs.v_plus[1] = 0.;
  cs.v_plus[2] = 0.;
  cs.v_plus[3] = 0.;
  cs.v_plus[4] = 0.;
  cs.v_plus[5] = 0.;

  qd[0] = 1.;
  qd[2] = 2.;
  qd[4] = 3.;

  ComputeConstraintImpulsesDirect(model, q, qd, cs, qdPlusDirect);
  CalcConstraintsVelocityError(model, q, qdPlusDirect, cs, errd);

  CHECK_ARRAY_CLOSE(cs.v_plus, errd, cs.size(), TEST_PREC);
}

TEST_FIXTURE(SliderCrank3DSphericalJoint
  , TestSliderCrank3DSphericalJointConstraintErrors) {
  VectorNd err(VectorNd::Zero(cs.size()));
  VectorNd errRef(VectorNd::Zero(cs.size()));
  Vector3d pos_p;
  Vector3d pos_s;
  Matrix3d rot_p;
  Matrix3d rot_s;
  Matrix3d rot_ps;
  Vector3d rotationVec;

  // Test in zero position.
  CalcConstraintsPositionError(model, q, cs, err);
  CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC);

  // Test in another configuration.
  Quaternion quat = Quaternion::fromZYXAngles(Vector3d(-0.25 * M_PI, 0.01, 0.01));
  q[0] = 0.4;
  q[1] = 0.25 * M_PI;
  model.SetQuaternion(id_s, quat, q);

  CalcConstraintsPositionError(model, q, cs, err);

  pos_p = CalcBodyToBaseCoordinates(model, q, id_p, X_p.r);
  pos_s = CalcBodyToBaseCoordinates(model, q, id_s, X_s.r);
  rot_p = CalcBodyWorldOrientation(model, q, id_p).transpose() * X_p.E;
  rot_s = CalcBodyWorldOrientation(model, q, id_s).transpose() * X_s.E;
  rot_ps = rot_p.transpose() * rot_s;
  rotationVec = - 0.5 * Vector3d
    ( rot_ps(1,2) - rot_ps(2,1)
    , rot_ps(2,0) - rot_ps(0,2)
    , rot_ps(0,1) - rot_ps(1,0));
  errRef.block<3,1>(0,0) = pos_s - pos_p;
  errRef[3] = rotationVec[2];

  CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC);
}

TEST_FIXTURE(SliderCrank3DSphericalJoint
  , TestSliderCrank3DSphericalJointConstraintJacobian) {
  MatrixNd G(MatrixNd::Zero(cs.size(), model.dof_count));

  // Test in zero position.

  G.setZero();
  CalcConstraintsJacobian(model, q, cs, G);

  VectorNd errRef(VectorNd::Zero(cs.size()));
  VectorNd err = G * qd;

  CHECK_ARRAY_CLOSE(errRef, err, cs.size(), TEST_PREC);
}

TEST_FIXTURE(SliderCrank3DSphericalJoint
  , TestSliderCrank3DSphericalJointConstraintsVelocityErrors) {
  VectorNd errd(VectorNd::Zero(cs.size()));
  VectorNd errdRef(VectorNd::Zero(cs.size()));
  MatrixNd G(cs.size(), model.dof_count);
  VectorNd qWeights(model.dof_count);
  VectorNd qInit(model.q_size);
  bool success;

  // Compute assembled configuration.
  qWeights[0] = 1.;
  qWeights[1] = 1.;
  qWeights[2] = 1.;
  qWeights[3] = 1.;
  qWeights[4] = 1.;

  Quaternion quat = Quaternion::fromZYXAngles(Vector3d(-0.25 * M_PI, 0.1, 0.1));
  qInit[0] = 0.4;
  qInit[1] = 0.25 * M_PI;
  model.SetQuaternion(id_s, quat, qInit);

  success = CalcAssemblyQ(model, qInit, cs, q, qWeights, 1e-14, 800);
  assert(success);

  // Some random velocity.
  qd[0] = -0.2;
  qd[1] = 0.1 * M_PI;
  qd[2] = -0.1 * M_PI;
  qd[3] = 0.;
  qd[4] = 0.1 * M_PI;

  CalcConstraintsVelocityError(model, q, qd, cs, errd);
  CalcConstraintsJacobian(model, q, cs, G);
  errdRef = G * qd;

  CHECK_ARRAY_CLOSE(errdRef, errd, cs.size(), TEST_PREC);
}

TEST_FIXTURE(SliderCrank3DSphericalJoint
  , TestSliderCrank3DSphericalJointAssemblyQ) {
  VectorNd weights(model.dof_count);
  VectorNd qInit(model.q_size);

  Vector3d pos_p;
  Vector3d pos_s;
  Matrix3d rot_p;
  Matrix3d rot_s;
  Matrix3d rot_ps;

  bool success;

  weights[0] = 1.;
  weights[1] = 1.;
  weights[2] = 1.;
  weights[3] = 1.;
  weights[4] = 1.;

  Quaternion quat = Quaternion::fromZYXAngles(Vector3d(-0.25 * M_PI, 0.1, 0.1));
  qInit[0] = 0.4;
  qInit[1] = 0.25 * M_PI;
  model.SetQuaternion(id_s, quat, qInit);

  success = CalcAssemblyQ(model, qInit, cs, q, weights, 1e-14, 800);
  pos_p = CalcBodyToBaseCoordinates(model, q, id_p, X_p.r);
  pos_s = CalcBodyToBaseCoordinates(model, q, id_s, X_s.r);
  rot_p = CalcBodyWorldOrientation(model, q, id_p).transpose() * X_p.E;
  rot_s = CalcBodyWorldOrientation(model, q, id_s).transpose() * X_s.E;
  rot_ps = rot_p.transpose() * rot_s;

  CHECK(success);
  CHECK_ARRAY_CLOSE(pos_p, pos_s, 3, TEST_PREC);
  CHECK_CLOSE(0., rot_ps(0,1) - rot_ps(1,0), TEST_PREC);
}

TEST_FIXTURE(SliderCrank3DSphericalJoint
  , TestSliderCrank3DSphericalJointAssemblyQDot) {
  VectorNd qWeights(model.dof_count);
  VectorNd qdWeights(model.dof_count);
  VectorNd qInit(model.q_size);
  VectorNd qdInit(model.dof_count);

  SpatialVector vel_p;
  SpatialVector vel_s;

  bool success;

  qWeights[0] = 1.;
  qWeights[1] = 1.;
  qWeights[2] = 1.;
  qWeights[3] = 1.;
  qWeights[4] = 1.;

  Quaternion quat = Quaternion::fromZYXAngles(Vector3d(-0.25 * M_PI, 0.1, 0.1));
  qInit[0] = 0.4;
  qInit[1] = 0.25 * M_PI;
  model.SetQuaternion(id_s, quat, qInit);

  qdWeights[0] = 1.;
  qdWeights[1] = 0.;
  qdWeights[2] = 0.;
  qdWeights[3] = 0.;
  qdWeights[4] = 0.;

  qdInit[0] = -0.2;
  qdInit[1] = 0.1 * M_PI;
  qdInit[2] = -0.1 * M_PI;
  qdInit[3] = 0.;
  qdInit[4] = 0.1 * M_PI;

  success = CalcAssemblyQ(model, qInit, cs, q, qWeights, 1e-14, 800);
  assert(success);

  CalcAssemblyQDot(model, q, qdInit, cs, qd, qdWeights);

  vel_p = CalcPointVelocity6D(model, q, qd, id_p, X_p.r);
  vel_s = CalcPointVelocity6D(model, q, qd, id_s, X_s.r);

  for(size_t i = 2; i < 6; ++i) {
    CHECK_CLOSE(vel_p[i], vel_s[i], TEST_PREC);
  }
  CHECK_CLOSE(qdInit[0], qd[0], TEST_PREC);
}

TEST_FIXTURE(SliderCrank3DSphericalJoint
  , TestSliderCrank3DSphericalJointForwardDynamics) {

  VectorNd qWeights(model.dof_count);
  VectorNd qdWeights(model.dof_count);
  VectorNd qInit(model.q_size);
  VectorNd qdInit(model.dof_count);

  SpatialVector acc_p;
  SpatialVector acc_s;

  bool success;

#ifndef RBDL_USE_SIMPLE_MATH
  // The SimpleMath solver cannot solve the system close to a singular
  // configuration.
  // Test with zero q and qdot.

  tau[0] = 0.12;
  tau[1] = -0.3;
  tau[2] = 0.05;
  tau[3] = 0.7;
  tau[4] = -0.1;

  ForwardDynamicsConstraintsDirect(model, q, qd, tau, cs, qdd);

  acc_p = CalcPointAcceleration6D(model, q, qd, qdd, id_p, X_p.r);
  acc_s = CalcPointAcceleration6D(model, q, qd, qdd, id_s, X_s.r);

  for(size_t i = 2; i < 6; ++i) {
    CHECK_CLOSE(acc_p[i], acc_s[i], TEST_PREC);
  }

  ForwardDynamicsConstraintsNullSpace(model, q, qd, tau, cs, qdd);

  acc_p = CalcPointAcceleration6D(model, q, qd, qdd, id_p, X_p.r);
  acc_s = CalcPointAcceleration6D(model, q, qd, qdd, id_s, X_s.r);

  for(size_t i = 2; i < 6; ++i) {
    CHECK_CLOSE(acc_p[i], acc_s[i], TEST_PREC);
  }

  ForwardDynamicsConstraintsRangeSpaceSparse(model, q, qd, tau, cs, qdd);

  acc_p = CalcPointAcceleration6D(model, q, qd, qdd, id_p, X_p.r);
  acc_s = CalcPointAcceleration6D(model, q, qd, qdd, id_s, X_s.r);

  for(size_t i = 2; i < 6; ++i) {
    CHECK_CLOSE(acc_p[i], acc_s[i], TEST_PREC);
  }
#endif

  // Compute non-zero assembly q and qdot;

  qWeights[0] = 1.;
  qWeights[1] = 1.;
  qWeights[2] = 1.;
  qWeights[3] = 1.;
  qWeights[4] = 1.;

  Quaternion quat = Quaternion::fromZYXAngles(Vector3d(-0.25 * M_PI, 0.1, 0.1));
  qInit[0] = 0.4;
  qInit[1] = 0.25 * M_PI;
  model.SetQuaternion(id_s, quat, qInit);

  qdWeights[0] = 1.;
  qdWeights[1] = 0.;
  qdWeights[2] = 0.;
  qdWeights[3] = 0.;
  qdWeights[4] = 0.;

  qdInit[0] = -0.2;
  qdInit[1] = 0.1 * M_PI;
  qdInit[2] = -0.1 * M_PI;
  qdInit[3] = 0.;
  qdInit[4] = 0.1 * M_PI;

  qdInit.setZero();

  success = CalcAssemblyQ(model, qInit, cs, q, qWeights, 1e-14, 800);
  assert(success);
  CalcAssemblyQDot(model, q, qdInit, cs, qd, qdWeights);

  Matrix3d rot_ps 
    = (CalcBodyWorldOrientation(model, q, id_p).transpose() * X_p.E).transpose()
    * CalcBodyWorldOrientation(model, q, id_s).transpose() * X_s.E;
  assert((CalcBodyToBaseCoordinates(model, q, id_p, X_p.r)
    - CalcBodyToBaseCoordinates(model, q, id_p, X_p.r)).norm() < TEST_PREC);
  assert(rot_ps(0,1) - rot_ps(0,1) < TEST_PREC);
  assert((CalcPointVelocity6D(model, q, qd, id_p, X_p.r)
    -CalcPointVelocity6D(model, q, qd, id_p, X_p.r)).norm() < TEST_PREC);

  // Test with non-zero q and qdot.

  ForwardDynamicsConstraintsDirect(model, q, qd, tau, cs, qdd);

  acc_p = CalcPointAcceleration6D(model, q, qd, qdd, id_p, X_p.r);
  acc_s = CalcPointAcceleration6D(model, q, qd, qdd, id_s, X_s.r);

  for(size_t i = 2; i < 6; ++i) {
    CHECK_CLOSE(acc_p[i], acc_s[i], TEST_PREC);
  }

  ForwardDynamicsConstraintsNullSpace(model, q, qd, tau, cs, qdd);

  acc_p = CalcPointAcceleration6D(model, q, qd, qdd, id_p, X_p.r);
  acc_s = CalcPointAcceleration6D(model, q, qd, qdd, id_s, X_s.r);

  for(size_t i = 2; i < 6; ++i) {
    CHECK_CLOSE(acc_p[i], acc_s[i], TEST_PREC);
  }

  ForwardDynamicsConstraintsRangeSpaceSparse(model, q, qd, tau, cs, qdd);

  acc_p = CalcPointAcceleration6D(model, q, qd, qdd, id_p, X_p.r);
  acc_s = CalcPointAcceleration6D(model, q, qd, qdd, id_s, X_s.r);

  for(size_t i = 2; i < 6; ++i) {
    CHECK_CLOSE(acc_p[i], acc_s[i], TEST_PREC);
  }
}

TEST_FIXTURE(SliderCrank3DSphericalJoint
  , TestSliderCrank3DSphericalJointImpulse) {

  VectorNd qdPlusDirect(model.dof_count);
  VectorNd qdPlusRangeSpaceSparse(model.dof_count);
  VectorNd qdPlusNullSpace(model.dof_count);
  VectorNd errdDirect(cs.size());
  VectorNd errdSpaceSparse(cs.size());
  VectorNd errdNullSpace(cs.size());
  VectorNd qWeights(model.dof_count);
  VectorNd qInit(model.q_size);

  qWeights[0] = 1.;
  qWeights[1] = 1.;
  qWeights[2] = 1.;
  qWeights[3] = 1.;
  qWeights[4] = 1.;

  Quaternion quat = Quaternion::fromZYXAngles(Vector3d(-0.25 * M_PI, 0.1, 0.1));
  qInit[0] = 0.4;
  qInit[1] = 0.25 * M_PI;
  model.SetQuaternion(id_s, quat, qInit);

  bool success = CalcAssemblyQ(model, qInit, cs, q, qWeights, 1e-14, 800);
  assert(success);

  cs.v_plus[0] = 1.;
  cs.v_plus[1] = 2.;
  cs.v_plus[2] = 3.;
  cs.v_plus[3] = 4.;
  
  ComputeConstraintImpulsesDirect(model, q, qd, cs, qdPlusDirect);
  CalcConstraintsVelocityError(model, q, qdPlusDirect, cs, errdDirect);
  
  CHECK_ARRAY_CLOSE(cs.v_plus, errdDirect, cs.size(), TEST_PREC);

  ComputeConstraintImpulsesRangeSpaceSparse(model, q, qd, cs
    , qdPlusRangeSpaceSparse);
  CalcConstraintsVelocityError(model, q, qdPlusRangeSpaceSparse, cs
    , errdSpaceSparse);
  
  CHECK_ARRAY_CLOSE(cs.v_plus, errdSpaceSparse, cs.size(), TEST_PREC);

  ComputeConstraintImpulsesNullSpace(model, q, qd, cs, qdPlusNullSpace);
  CalcConstraintsVelocityError(model, q, qdPlusNullSpace, cs, errdNullSpace);

  CHECK_ARRAY_CLOSE(cs.v_plus, errdNullSpace, cs.size(), TEST_PREC);

  cs.v_plus[0] = 0.;
  cs.v_plus[1] = 0.;
  cs.v_plus[2] = 0.;
  cs.v_plus[3] = 0.;

  qd[0] = 1.;
  qd[1] = 2.;
  qd[2] = 3.;

  ComputeConstraintImpulsesDirect(model, q, qd, cs, qdPlusDirect);
  CalcConstraintsVelocityError(model, q, qdPlusDirect, cs, errdDirect);
  
  CHECK_ARRAY_CLOSE(cs.v_plus, errdDirect, cs.size(), TEST_PREC);

  ComputeConstraintImpulsesRangeSpaceSparse(model, q, qd, cs
    , qdPlusRangeSpaceSparse);
  CalcConstraintsVelocityError(model, q, qdPlusRangeSpaceSparse, cs
    , errdSpaceSparse);
  
  CHECK_ARRAY_CLOSE(cs.v_plus, errdSpaceSparse, cs.size(), TEST_PREC);

  ComputeConstraintImpulsesNullSpace(model, q, qd, cs, qdPlusNullSpace);
  CalcConstraintsVelocityError(model, q, qdPlusNullSpace, cs, errdNullSpace);

  CHECK_ARRAY_CLOSE(cs.v_plus, errdNullSpace, cs.size(), TEST_PREC);
}