int main(int argc, char **argv) {
  cout << "--------------------------------------------------------------------------------" << endl;
  cout << " Multi-Body System Benchmark in OpenSim" << endl;
  cout << " Benchmark reference url: http://lim.ii.udc.es/mbsbenchmark/" << endl;
  cout << " Problem A03: Andrew's Mechanism Model Creator" << endl;
  cout << " Copyright (C) 2013-2015 Luca Tagliapietra, Michele Vivian, Elena Ceseracciu, and Monica Reggiani" << endl;
  cout << "--------------------------------------------------------------------------------" << endl;

  if (argc != 2){
    cout << " ******************************************************************************" << endl;
    cout << " Multi-Body System Benchmark in OpenSim: Creator for Model A03" << endl;
    cout << " Usage: ./AndrewsMechanismCreateModel dataDirectory" << endl;
    cout << "       dataDirectory must contain a vtpFiles folder" << endl;
    cout << " ******************************************************************************" << endl;
    exit(EXIT_FAILURE);
  }

  const std::string dataDir = argv[1];
  cout << "Data directory: " + dataDir << endl;

  OpenSim::Model andrewsMechanism;
  andrewsMechanism.setName("Andrew's Mechanism");
  andrewsMechanism.setAuthors("L.Tagliapietra, M. Vivian, M.Reggiani");

  // Get a reference to the model's ground body
  OpenSim::Body& ground = andrewsMechanism.getGroundBody();
  andrewsMechanism.setGravity(gravityVector);

  //******************************
  // Create OF element
  //******************************
  SimTK::Inertia OFbarInertia(0.1,0.1,OFinertia);
  OpenSim::Body *OF = new OpenSim::Body("OF", OFmass, OFMassCenter, OFbarInertia);

  //Set transformation for visualization pourpose
  SimTK::Rotation rot(SimTK::Pi/2, SimTK::UnitVec3(0,0,1));
  SimTK::Transform trans = SimTK::Transform(rot);

  //Set visualization properties
  OF->addDisplayGeometry(rodGeometry);
  OpenSim::VisibleObject* visOF = OF->updDisplayer();
  visOF -> updTransform() =  trans;
  visOF -> setScaleFactors(SimTK::Vec3(0.001,OFlength, 0.001));
  visOF -> setDisplayPreference(OpenSim::DisplayGeometry::DisplayPreference(1));

  SimTK::Vec3 orientationInParent(0), orientationInBody(0);
  OpenSim::PinJoint *OJoint = new OpenSim::PinJoint("joint_O", ground, SimTK::Vec3(0), orientationInParent, *OF, SimTK::Vec3(-OFlength/2,0,0), orientationInBody);
  OpenSim::CoordinateSet& OCoordinateSet = OJoint -> upd_CoordinateSet();
  OCoordinateSet[0].setName("joint_O");
  OCoordinateSet[0].setDefaultValue(OAngleAtZero);
  andrewsMechanism.addBody(OF);

  //********************************
  // Create FE element
  //********************************
  SimTK::Inertia EFbarInertia(0.1,0.1,EFinertia);
  OpenSim::Body *EF = new OpenSim::Body("EF", EFmass, EFMassCenter, EFbarInertia);

  //Set visualization properties
  EF->addDisplayGeometry(rodGeometry);
  OpenSim::VisibleObject* visEF = EF->updDisplayer();
  visEF -> updTransform() =  trans;
  visEF -> setScaleFactors(SimTK::Vec3(0.001,EFlength, 0.001));
  visEF -> setDisplayPreference(OpenSim::DisplayGeometry::DisplayPreference(1));

  OpenSim::PinJoint *FJoint = new OpenSim::PinJoint("joint_F", *OF, SimTK::Vec3(OFlength/2,0,0), orientationInParent, *EF, SimTK::Vec3(EFlength/2,0,0), orientationInBody);
  OpenSim::CoordinateSet& FCoordinateSet = FJoint -> upd_CoordinateSet();
  FCoordinateSet[0].setName("joint_F");
  FCoordinateSet[0].setDefaultValue(FAngleAtZero);
  andrewsMechanism.addBody(EF);

  //********************************
  // Create EG element
  //********************************
  SimTK::Inertia GEbarInertia(0.1,0.1,GEinertia);
  OpenSim::Body *GE = new OpenSim::Body("GE", GEmass, GEMassCenter, GEbarInertia);

  //Set visualization properties
  GE->addDisplayGeometry(rodGeometry);
  OpenSim::VisibleObject* visGE = GE->updDisplayer();
  visGE -> updTransform() =  trans;
  visGE -> setScaleFactors(SimTK::Vec3(0.001,GElength, 0.001));
  visGE -> setDisplayPreference(OpenSim::DisplayGeometry::DisplayPreference(1));

  OpenSim::PinJoint *E1Joint = new OpenSim::PinJoint("joint_E1", *EF, SimTK::Vec3(-EFlength/2,0,0), orientationInParent, *GE, SimTK::Vec3(GElength/2,0,0), orientationInBody);
  OpenSim::CoordinateSet& E1CoordinateSet = E1Joint -> upd_CoordinateSet();
  E1CoordinateSet[0].setName("joint_E1");
  E1CoordinateSet[0].setDefaultValue(E1AngleAtZero);
  andrewsMechanism.addBody(GE);

  //********************************
  // Create AG element
  //********************************
  SimTK::Inertia AGbarInertia(0.1,0.1,AGinertia);
  OpenSim::Body *AG = new OpenSim::Body("AG", AGmass, AGMassCenter, AGbarInertia);

  //Set visualization properties
  AG->addDisplayGeometry(rodGeometry);
  OpenSim::VisibleObject* visAG = AG->updDisplayer();
  visAG -> updTransform() =  trans;
  visAG -> setScaleFactors(SimTK::Vec3(0.001,AGlength, 0.001));
  visAG -> setDisplayPreference(OpenSim::DisplayGeometry::DisplayPreference(1));

  OpenSim::PinJoint *GJoint = new OpenSim::PinJoint("joint_G", *GE, SimTK::Vec3(-GElength/2,0,0), orientationInParent, *AG, SimTK::Vec3(AGlength/2,0,0), orientationInBody);
  OpenSim::CoordinateSet& GCoordinateSet = GJoint -> upd_CoordinateSet();
  GCoordinateSet[0].setName("joint_G");
  GCoordinateSet[0].setDefaultValue(GAngleAtZero);
  andrewsMechanism.addBody(AG);

  //********************************
  // Create point constraint between AG element and ground to simulate joint A
  //********************************
   createPointCostraint(andrewsMechanism, std::string("ground"), SimTK::Vec3(-0.06934, -0.00227,0), std::string("AG"), SimTK::Vec3(-AGlength/2,0,0));

  //********************************
  // Create HE element
  //********************************
  SimTK::Inertia HEbarInertia(0.1,0.1,GEinertia);
  OpenSim::Body *HE = new OpenSim::Body("HE", HEmass, HEMassCenter, HEbarInertia);

  //Set visualization properties
  HE->addDisplayGeometry(rodGeometry);
  OpenSim::VisibleObject* visHE = HE->updDisplayer();
  visHE -> updTransform() =  trans;
  visHE -> setScaleFactors(SimTK::Vec3(0.001,HElength, 0.001));
  visHE -> setDisplayPreference(OpenSim::DisplayGeometry::DisplayPreference(1));

  OpenSim::PinJoint *E2Joint = new OpenSim::PinJoint("joint_E2", *EF, SimTK::Vec3(-EFlength/2,0,0), orientationInParent, *HE, SimTK::Vec3(HElength/2,0,0), orientationInBody);
  OpenSim::CoordinateSet& E2CoordinateSet = E2Joint -> upd_CoordinateSet();
  E2CoordinateSet[0].setName("joint_E2");
  E2CoordinateSet[0].setDefaultValue(E2AngleAtZero);
  andrewsMechanism.addBody(HE);

  //********************************
  //Create AH element
  //********************************
  SimTK::Inertia AHbarInertia(0.1,0.1,AHinertia);
  OpenSim::Body *AH = new OpenSim::Body("AH", AHmass, AHMassCenter, AHbarInertia);

  //Set visualization properties
  AH->addDisplayGeometry(rodGeometry);
  OpenSim::VisibleObject* visAH = AH->updDisplayer();
  visAH -> updTransform() =  trans;
  visAH -> setScaleFactors(SimTK::Vec3(0.001,AHlength, 0.001));
  visAH -> setDisplayPreference(OpenSim::DisplayGeometry::DisplayPreference(1));

  OpenSim::PinJoint *HJoint = new OpenSim::PinJoint("joint_H", *HE, SimTK::Vec3(-HElength/2,0,0), orientationInParent, *AH, SimTK::Vec3(AHlength/2,0,0), orientationInBody);
  OpenSim::CoordinateSet& HCoordinateSet = HJoint -> upd_CoordinateSet();
  HCoordinateSet[0].setName("joint_H");
  HCoordinateSet[0].setDefaultValue(HAngleAtZero);
  andrewsMechanism.addBody(AH);

  //********************************
  //Create point constraint between AH element and ground to simulate joint A
  //********************************
  createPointCostraint(andrewsMechanism, std::string("ground"), SimTK::Vec3(-0.06934, -0.00227,0), std::string("AH"), SimTK::Vec3(-AHlength/2,0,0));

  //********************************
  // Create BDE element
  //********************************
  SimTK::Inertia BDEInertia(0.1,0.1,BDEinertia);
  OpenSim::Body *BDE = new OpenSim::Body("BDE", BDEmass, BDEMassCenter, BDEInertia);

  //Set visualization properties
  BDE->addDisplayGeometry(triangleGeometry);
  OpenSim::VisibleObject* visBDE = BDE->updDisplayer();
  visBDE -> updTransform() =  trans;
  visBDE -> setScaleFactors(SimTK::Vec3(0.01, 0.01, 0.0005));
  visBDE -> setDisplayPreference(OpenSim::DisplayGeometry::DisplayPreference(1));

  OpenSim::PinJoint *E3Joint = new OpenSim::PinJoint("joint_E3", *EF, SimTK::Vec3(-EFlength/2,0,0), orientationInParent, *BDE, SimTK::Vec3(BElength/2,0,0), orientationInBody);
  OpenSim::CoordinateSet& E3CoordinateSet = E3Joint -> upd_CoordinateSet();
  E3CoordinateSet[0].setName("joint_E3");
  E3CoordinateSet[0].setDefaultValue(E3AngleAtZero);
  andrewsMechanism.addBody(BDE);

  //********************************
  // Create point constraint between BDE element and ground to simulate joint B
  //********************************
  createPointCostraint(andrewsMechanism, std::string("ground"), SimTK::Vec3(-0.03635, 0.03273,0), std::string("BDE"), SimTK::Vec3(-BElength/2,0,0));

  //********************************
  // Add the spring between BDE and ground
  //********************************
  OpenSim::PointToPointSpring *spring = new OpenSim::PointToPointSpring(std::string("ground"), SimTK::Vec3(0.014,0.072,0), std::string("BDE"), SimTK::Vec3(-BElength/2+0.018, 0.02,0), springK, springRestLength);
  andrewsMechanism.addComponent(spring);

  // Save to file the model
  andrewsMechanism.print((dataDir+"/"+modelName+std::string(".osim")).c_str());

  cout << "Model stored in: " << dataDir << "/" << modelName << ".osim" << endl;
}
/**
 * Method for building the Luxo Jr articulating model. It sets up the system of
 * rigid bodies and joint articulations to define Luxo Jr lamp geometry.
 */
void createLuxoJr(OpenSim::Model &model){
    
    // Create base
    //--------------
    OpenSim::Body* base = new OpenSim::Body("base", baseMass, Vec3(0.0),
                Inertia::cylinderAlongY(0.1, baseHeight));
    
    // Add visible geometry
    base->attachMeshGeometry("Base_meters.obj");
    
    
    // Define base to float relative to ground via free joint
    FreeJoint* base_ground = new FreeJoint("base_ground",
                // parent body, location in parent body, orientation in parent
                model.getGround(), Vec3(0.0), Vec3(0.0),
                // child body, location in child body, orientation in child
                *base, Vec3(0.0,-baseHeight/2.0,0.0),Vec3(0.0));
    
    // add base to model
    model.addBody(base); model.addJoint(base_ground);
    
    /*for (int i = 0; i<base_ground->get_CoordinateSet().getSize(); ++i) {
        base_ground->upd_CoordinateSet()[i].set_locked(true);
    }*/
    
    // Fix a frame to the base axis for attaching the bottom bracket
    SimTK::Transform* shift_and_rotate = new SimTK::Transform();
    //shift_and_rotate->setToZero();
    shift_and_rotate->set(Rotation(-1*SimTK::Pi/2,
                                   SimTK::CoordinateAxis::XCoordinateAxis()),
                          Vec3(0.0, bracket_location, 0.0));

    PhysicalOffsetFrame pivot_frame_on_base("pivot_frame_on_base",
            *base, *shift_and_rotate);

    // Create bottom bracket
    //-----------------------
    OpenSim::Body* bottom_bracket = new OpenSim::Body("bottom_bracket",
                                            bracket_mass, Vec3(0.0),
                                            Inertia::brick(0.03, 0.03, 0.015));
    // add bottom bracket to model
    model.addBody(bottom_bracket);
    
    // Fix a frame to the bracket for attaching joint
    shift_and_rotate->setP(Vec3(0.0));
    PhysicalOffsetFrame pivot_frame_on_bottom_bracket(
        "pivot_frame_on_bottom_bracket", *bottom_bracket, *shift_and_rotate);
    
    // Add visible geometry
    bottom_bracket->attachMeshGeometry("bottom_bracket_meters.obj");

    // Make bottom bracket to twist on base with vertical pin joint.
    // You can create a joint from any existing physical frames attached to
    // rigid bodies. One way to reference them is by name, like this...
    PinJoint* base_pivot = new PinJoint("base_pivot", pivot_frame_on_base,
                                        pivot_frame_on_bottom_bracket);
    
    base_pivot->append_frames(pivot_frame_on_base);
    base_pivot->append_frames(pivot_frame_on_bottom_bracket);
    // add base pivot joint to the model
    model.addJoint(base_pivot);
    
    // add some damping to the pivot
    // initialized to zero stiffness and damping
    BushingForce* pivotDamper = new BushingForce("pivot_bushing",
                                         "pivot_frame_on_base",
                                         "pivot_frame_on_bottom_bracket");
    
    pivotDamper->set_rotational_damping(pivot_damping);
    
    model.addForce(pivotDamper);
    
    // Create posterior leg
    //-----------------------
    OpenSim::Body* posteriorLegBar = new OpenSim::Body("posterior_leg_bar",
                                    bar_mass,
                                    Vec3(0.0),
                                    Inertia::brick(leg_bar_dimensions/2.0));
    
    posteriorLegBar->attachMeshGeometry("Leg_meters.obj");

    PhysicalOffsetFrame posterior_knee_on_bottom_bracket(
        "posterior_knee_on_bottom_bracket",
            *bottom_bracket, Transform(posterior_bracket_hinge_location) );

    PhysicalOffsetFrame posterior_knee_on_posterior_bar(
        "posterior_knee_on_posterior_bar",
            *posteriorLegBar, Transform(inferior_bar_hinge_location) );

    // Attach posterior leg to bottom bracket using another pin joint.
    // Another way to reference physical frames in a joint is by creating them
    // in place, like this...
    OpenSim::PinJoint* posteriorKnee = new OpenSim::PinJoint("posterior_knee",
                                    posterior_knee_on_bottom_bracket,
                                    posterior_knee_on_posterior_bar);
    // posteriorKnee will own and serialize the attachment offset frames 
    posteriorKnee->append_frames(posterior_knee_on_bottom_bracket);
    posteriorKnee->append_frames(posterior_knee_on_posterior_bar);

    
    // add posterior leg to model
    model.addBody(posteriorLegBar); model.addJoint(posteriorKnee);
    
    // allow this joint's coordinate to float freely when assembling constraints
    // the joint we create next will drive the pose of the 4-bar linkage
    posteriorKnee->upd_CoordinateSet()[0]
                                    .set_is_free_to_satisfy_constraints(true);
    
    // Create anterior leg Hlink
    //----------------------------
    OpenSim::Body* leg_Hlink = new OpenSim::Body("leg_Hlink",
                                    bar_mass,
                                    Vec3(0.0),
                                    Inertia::brick(leg_Hlink_dimensions/2.0));
    
    leg_Hlink->attachMeshGeometry("H_Piece_meters.obj");
    
    
    PhysicalOffsetFrame anterior_knee_on_bottom_bracket(
        "anterior_knee_on_bottom_bracket",
            *bottom_bracket, Transform(anterior_bracket_hinge_location));

    
    PhysicalOffsetFrame anterior_knee_on_anterior_bar(
        "anterior_knee_on_anterior_bar",
            *leg_Hlink, Transform(inferior_Hlink_hinge_location));


    // Connect anterior leg to bottom bracket via pin joint
    OpenSim::PinJoint* anterior_knee = new OpenSim::PinJoint("anterior_knee",
                                    anterior_knee_on_bottom_bracket,
                                    anterior_knee_on_anterior_bar);
    anterior_knee->append_frames(anterior_knee_on_bottom_bracket);
    anterior_knee->append_frames(anterior_knee_on_anterior_bar);

    
    // add anterior leg to model
    model.addBody(leg_Hlink); model.addJoint(anterior_knee);
    
    // this anterior knee joint defines the motion of the lower 4-bar linkage
    // set it's default coordinate value to a slightly flexed position.
    anterior_knee->upd_CoordinateSet()[0].set_default_value(SimTK::Pi/6);
    
    // Create pelvis bracket
    //-----------------------
    OpenSim::Body* pelvisBracket = new OpenSim::Body("pelvis_bracket",
                                            bracket_mass,
                                            Vec3(0.0),
                                        Inertia::brick(pelvis_dimensions/2.0));
    
    pelvisBracket->attachMeshGeometry("Pelvis_bracket_meters.obj");
    
    // Connect pelvis to Hlink via pin joint
    SimTK::Transform pelvis_anterior_shift(
                                        anterior_superior_pelvis_pin_location);

    PhysicalOffsetFrame anterior_hip_on_Hlink(
        "anterior_hip_on_Hlink",
        *leg_Hlink, Transform(superior_Hlink_hinge_location));

    PhysicalOffsetFrame anterior_hip_on_pelvis(
            "anterior_hip_on_pelvis",
        *pelvisBracket, pelvis_anterior_shift);

    OpenSim::PinJoint* anteriorHip = new OpenSim::PinJoint("anterior_hip",
                                                    anterior_hip_on_Hlink,
                                                    anterior_hip_on_pelvis);
    anteriorHip->append_frames(anterior_hip_on_Hlink);
    anteriorHip->append_frames(anterior_hip_on_pelvis);
    
    // add anterior leg to model
    model.addBody(pelvisBracket); model.addJoint(anteriorHip);
    
    // since the previous, anterior knee joint drives the pose of the lower
    // 4-bar linkage, set the anterior hip angle such that it's free to satisfy
    // constraints that couple it to the 4-bar linkage.
    anteriorHip->upd_CoordinateSet()[0]
                                    .set_is_free_to_satisfy_constraints(true);
    
    // Close the loop for the lower, four-bar linkage with a constraint
    //------------------------------------------------------------------
    
    // Create and configure point on line constraint
    OpenSim::PointOnLineConstraint* posteriorHip =
        new OpenSim::PointOnLineConstraint();
    
    posteriorHip->setLineBodyByName(pelvisBracket->getName());
    posteriorHip->setLineDirection(Vec3(0.0,0.0,1.0));
    posteriorHip->setPointOnLine(inferior_pelvis_pin_location);
    posteriorHip->setFollowerBodyByName(posteriorLegBar->getName());
    posteriorHip->setPointOnFollower(superior_bar_hinge_location);
    
    // add constraint to model
    model.addConstraint(posteriorHip);
    
    // Create chest piece
    //-----------------------
    OpenSim::Body* chest = new OpenSim::Body("chest_bar", bar_mass,
                                     Vec3(0.0),
                                     Inertia::brick(torso_bar_dimensions/2.0));
    
    chest->attachMeshGeometry("Anterior_torso_bar.obj");

    PhysicalOffsetFrame anterior_torso_hinge_on_pelvis(
        "anterior_torso_hinge_on_pelvis",
        *pelvisBracket, Transform(anterior_superior_pelvis_pin_location) );

    PhysicalOffsetFrame anterior_torso_hinge_on_chest(
        "anterior_torso_hinge_on_chest",
        *chest, Transform(inferior_torso_hinge_location) );
    
    // Attach chest piece to pelvice with pin joint
    OpenSim::PinJoint* anteriorTorsoHinge = new OpenSim::PinJoint(
                                              "anterior_torso_hinge",
                                              anterior_torso_hinge_on_pelvis,
                                              anterior_torso_hinge_on_chest);
    anteriorTorsoHinge->append_frames(anterior_torso_hinge_on_pelvis);
    anteriorTorsoHinge->append_frames(anterior_torso_hinge_on_chest);
    
    // add posterior leg to model
    model.addBody(chest); model.addJoint(anteriorTorsoHinge);
    
    // set torso rotation slightly anterior
    anteriorTorsoHinge->upd_CoordinateSet()[0].setDefaultValue(-1*SimTK::Pi/4);
    
    // Create chest piece
    //-----------------------
    OpenSim::Body* back = new OpenSim::Body("back_bar", bar_mass,
                                     Vec3(0.0),
                                     Inertia::brick(torso_bar_dimensions/2.0));
    
    back->attachMeshGeometry("Posterior_torso_bar.obj");

    PhysicalOffsetFrame posterior_torso_hinge_on_pelvis(
        "posterior_torso_hinge_on_pelvis",
        *pelvisBracket, Transform(posterior_superior_pelvis_pin_location) );

    PhysicalOffsetFrame posterior_torso_hinge_on_back(
        "posterior_torso_hinge_on_back",
        *back, Transform(back_peg_center) );
    
    // Attach chest piece to pelvis with pin joint
    OpenSim::PinJoint* posteriorTorsoHinge = new OpenSim::PinJoint(
                                          "posterior_torso_hinge",
                                          posterior_torso_hinge_on_pelvis,
                                          posterior_torso_hinge_on_back);
    posteriorTorsoHinge->append_frames(posterior_torso_hinge_on_pelvis);
    posteriorTorsoHinge->append_frames(posterior_torso_hinge_on_back);
    
    // add posterior leg to model
    model.addBody(back); model.addJoint(posteriorTorsoHinge);
    
    // set posterior back joint to freely follow anterior joint through 4-bar
    // linkage coupling.
    posteriorTorsoHinge->upd_CoordinateSet()[0]
    .set_is_free_to_satisfy_constraints(true);
    
    // Create shoulder bracket
    //-----------------------
    OpenSim::Body* shoulderBracket = new OpenSim::Body("shoulder_bracket",
                                     bracket_mass,
                                     Vec3(0.0),
                                     Inertia::brick(shoulder_dimensions/2.0));

    shoulderBracket->attachMeshGeometry("Shoulder_meters.obj");
    // add anterior leg to model
    model.addBody(shoulderBracket);

    PhysicalOffsetFrame anterior_thoracic_joint_on_chest(
        "anterior_thoracic_joint_on_chest",
        *chest, Transform(superior_torso_hinge_location) );

    PhysicalOffsetFrame anterior_thoracic_joint_on_shoulder(
        "anterior_thoracic_joint_on_shoulder",
        *shoulderBracket, Transform(anterior_thoracic_joint_center));
    
    // Connect pelvis to Hlink via pin joint
    OpenSim::PinJoint* anteriorThoracicJoint =
                        new OpenSim::PinJoint("anterior_thoracic_joint",
                                       anterior_thoracic_joint_on_chest,
                                       anterior_thoracic_joint_on_shoulder);
    anteriorThoracicJoint->append_frames(anterior_thoracic_joint_on_chest);
    anteriorThoracicJoint->append_frames(anterior_thoracic_joint_on_shoulder);
    // add back joint
    model.addJoint(anteriorThoracicJoint);
    
    // since the previous, anterior thoracic joint drives the pose of the lower
    // 4-bar linkage, set the anterior shoulder angle such that it's free to
    // satisfy constraints that couple it to the 4-bar linkage.
    anteriorThoracicJoint->upd_CoordinateSet()[0]
    .set_is_free_to_satisfy_constraints(true);
    
    // Close the loop for the lower, four-bar linkage with a constraint
    //------------------------------------------------------------------
    // Create and configure point on line constraint
    OpenSim::PointOnLineConstraint* posteriorShoulder =
    new OpenSim::PointOnLineConstraint();
    
    posteriorShoulder->setLineBodyByName(shoulderBracket->getName());
    posteriorShoulder->setLineDirection(Vec3(0.0,0.0,1.0));
    posteriorShoulder->setPointOnLine(posterior_thoracic_joint_center);
    posteriorShoulder->setFollowerBodyByName(back->getName());
    posteriorShoulder->setPointOnFollower(superior_torso_hinge_location);
    
    // add constraint to model
    model.addConstraint(posteriorShoulder);
    
    // Create and add luxo head
    OpenSim::Body* head = new OpenSim::Body("head", head_mass, Vec3(0),
            Inertia::cylinderAlongX(0.5*head_dimension[1], head_dimension[1]));
    
    head->attachMeshGeometry("luxo_head_meters.obj");
    head->attachMeshGeometry("Bulb_meters.obj");
    model.addBody(head);


    PhysicalOffsetFrame cervical_joint_on_shoulder("cervical_joint_on_shoulder",
        *shoulderBracket, Transform(superior_shoulder_hinge_location) );

    PhysicalOffsetFrame cervical_joint_on_head("cervical_joint_on_head",
        *head, Transform(cervicle_joint_center));

    // attach to shoulder via pin joint
    OpenSim::PinJoint* cervicalJoint = new OpenSim::PinJoint("cervical_joint",
                                  cervical_joint_on_shoulder,
                                  cervical_joint_on_head);
    
    cervicalJoint->append_frames(cervical_joint_on_shoulder);
    cervicalJoint->append_frames(cervical_joint_on_head);
    // add a neck joint
     model.addJoint(cervicalJoint);
    
    // lock the kneck coordinate so the head doens't spin without actuators or
    // passive forces
    cervicalJoint->upd_CoordinateSet()[0].set_locked(true);
    
    // Coordinate Limit forces for restricting leg range of motion.
    //-----------------------------------------------------------------------
    CoordinateLimitForce* kneeLimitForce =
            new CoordinateLimitForce(
                     anterior_knee->get_CoordinateSet()[0].getName(),
                     knee_flexion_max, joint_softstop_stiffness,
                     knee_flexion_min, joint_softstop_stiffness,
                     joint_softstop_damping,
                     transition_region);
    model.addForce(kneeLimitForce);
    
    // Coordinate Limit forces for restricting back range motion.
    //-----------------------------------------------------------------------
    CoordinateLimitForce* backLimitForce =
    new CoordinateLimitForce(
                         anteriorTorsoHinge->get_CoordinateSet()[0].getName(),
                         back_extension_max, joint_softstop_stiffness,
                         back_extension_min, joint_softstop_stiffness,
                         joint_softstop_damping,
                         transition_region);
    model.addForce(backLimitForce);
    
    
    // Contact
    //-----------------------------------------------------------------------
    ContactHalfSpace* floor_surface = new ContactHalfSpace(SimTK::Vec3(0),
                                       SimTK::Vec3(0, 0, -0.5*SimTK::Pi),
                                       model.updGround(), "floor_surface");
    
    OpenSim::ContactMesh* foot_surface = new ContactMesh(
                                         "thin_disc_0.11_by_0.01_meters.obj",
                                          SimTK::Vec3(0),
                                          SimTK::Vec3(0),
                                          *base,
                                          "foot_surface");
    
    // add contact geometry to model
    model.addContactGeometry(floor_surface);
    model.addContactGeometry(foot_surface);
    
    // define contact as an elastic foundation force
    OpenSim::ElasticFoundationForce::ContactParameters* contactParameters =
            new OpenSim::ElasticFoundationForce::ContactParameters(
                                               stiffness,
                                               dissipation,
                                               friction,
                                               friction,
                                               viscosity);
    
    contactParameters->addGeometry("foot_surface");
    contactParameters->addGeometry("floor_surface");
    
    OpenSim::ElasticFoundationForce* contactForce =
                        new OpenSim::ElasticFoundationForce(contactParameters);
    contactForce->setName("contact_force");
    
    model.addForce(contactForce);
    
    
    
    // MUSCLES
    //-----------------------------------------------------------------------
    
    // add a knee extensor to control the lower 4-bar linkage
    Millard2012EquilibriumMuscle* kneeExtensorRight = new Millard2012EquilibriumMuscle(
                                            "knee_extensor_right",
                                            knee_extensor_F0, knee_extensor_lm0,
                                            knee_extensor_lts, pennationAngle);
    kneeExtensorRight->addNewPathPoint("knee_extensor_right_origin", *leg_Hlink,
                                  knee_extensor_origin);
    kneeExtensorRight->addNewPathPoint("knee_extensor_right_insertion",
                                      *bottom_bracket,
                                        knee_extensor_insertion);
    kneeExtensorRight->set_ignore_tendon_compliance(true);
    model.addForce(kneeExtensorRight);
    
    // add a second copy of this knee extensor for the left side
    Millard2012EquilibriumMuscle* kneeExtensorLeft =
                                new Millard2012EquilibriumMuscle(*kneeExtensorRight);
    kneeExtensorLeft->setName("kneeExtensorLeft");
    
    // flip the z coordinates of all path points
    PathPointSet& points = kneeExtensorLeft->updGeometryPath().updPathPointSet();
    for (int i=0; i<points.getSize(); ++i) {
        points[i].setLocationCoord(2, -1*points[i].getLocationCoord(2));
    }

    kneeExtensorLeft->set_ignore_tendon_compliance(true);
    model.addForce(kneeExtensorLeft);
    
    // add a back extensor to controll the upper 4-bar linkage
    Millard2012EquilibriumMuscle* backExtensorRight = new Millard2012EquilibriumMuscle(
                                            "back_extensor_right",
                                            back_extensor_F0, back_extensor_lm0,
                                            back_extensor_lts, pennationAngle);
    
    backExtensorRight->addNewPathPoint("back_extensor_right_origin", *chest,
                                      back_extensor_origin);
    backExtensorRight->addNewPathPoint("back_extensor_right_insertion", *back,
                                      back_extensor_insertion);
    backExtensorRight->set_ignore_tendon_compliance(true);
    model.addForce(backExtensorRight);
    
    // copy right back extensor and use to make left extensor
    Millard2012EquilibriumMuscle* backExtensorLeft =
            new Millard2012EquilibriumMuscle(*backExtensorRight);
    
    backExtensorLeft->setName("back_extensor_left");
    
    PathPointSet& pointsLeft = backExtensorLeft->updGeometryPath()
        .updPathPointSet();
    for (int i=0; i<points.getSize(); ++i) {
        pointsLeft[i].setLocationCoord(2, -1*pointsLeft[i].getLocationCoord(2));
    }
    backExtensorLeft->set_ignore_tendon_compliance(true);
    model.addForce(backExtensorLeft);
    
    
    
    // MUSCLE CONTROLLERS
    //________________________________________________________________________
    
    // specify a piecwise linear function for the muscle excitations
    PiecewiseConstantFunction* x_of_t = new PiecewiseConstantFunction(3, times,
                                                                  excitations);
    
    
    PrescribedController* kneeController = new PrescribedController();
    kneeController->addActuator(*kneeExtensorLeft);
    kneeController->addActuator(*kneeExtensorRight);
    kneeController->prescribeControlForActuator(0, x_of_t);
    kneeController->prescribeControlForActuator(1, x_of_t->clone());
    
    model.addController(kneeController);
    
    PrescribedController* backController = new PrescribedController();
    backController->addActuator(*backExtensorLeft);
    backController->addActuator(*backExtensorRight);
    backController->prescribeControlForActuator(0, x_of_t->clone());
    backController->prescribeControlForActuator(1, x_of_t->clone());
    
    model.addController(backController);

    
    /* You'll find that these muscles can make Luxo Myo stand, but not jump.
     * Jumping will require an assistive device. We'll add two frames for
     * attaching a point to point assistive actuator.
     */
    
    
    // add frames for connecting a back assitance device between the chest
    // and pelvis
    PhysicalOffsetFrame* back_assist_origin_frame = new
        PhysicalOffsetFrame("back_assist_origin",
                            *chest,
                            back_assist_origin_transform);
    
    PhysicalOffsetFrame* back_assist_insertion_frame = new
        PhysicalOffsetFrame("back_assist_insertion",
                            *pelvisBracket,
                            back_assist_insertion_transform);
    
    model.addFrame(back_assist_origin_frame);
    model.addFrame(back_assist_insertion_frame);
    
    // add frames for connecting a knee assistance device between the posterior
    // leg and bottom bracket.
    PhysicalOffsetFrame* knee_assist_origin_frame = new
    PhysicalOffsetFrame("knee_assist_origin",
                        *posteriorLegBar,
                        knee_assist_origin_transform);
    
    PhysicalOffsetFrame* knee_assist_insertion_frame = new
    PhysicalOffsetFrame("knee_assist_insertion",
                        *bottom_bracket,
                        knee_assist_insertion_transform);
    
    model.addFrame(knee_assist_origin_frame);
    model.addFrame(knee_assist_insertion_frame);

    // Temporary: make the frame geometry disappear.
    for (auto& c : model.getComponentList<OpenSim::FrameGeometry>()) {
        const_cast<OpenSim::FrameGeometry*>(&c)->set_scale_factors(
                SimTK::Vec3(0.001, 0.001, 0.001));
    }
    
}
Ejemplo n.º 3
0
/**
 * Creates two cat models for flipping. The first is simply two segments
 * connected by a 2-degree-of-freedom (no twist) joint. It exhibits the
 * counter-rotation mechanism of flipping. The second adds actuate-able
 * legs to the first model, as well as unlocking the twist degree of
 * freedom between the model's two halves. This model exhibits the variable-
 * inertia mechanism of flipping.
 * */
int main(int argc, char *argv[])
{
    // First model: exhibiting the counter-rotation mechanism of flipping
    // ========================================================================

    // Properties
    // ------------------------------------------------------------------------
    double segmentalLength = 0.175;		                   // m
    double segmentalDiam = 0.15;			               // m
    double segmentalMass = 1;				               // kg
    double segmentalTransverseMomentOfInertia = 1;         // kg-m^2
    
    // Ratio of transverse to axial moment of inertia (Kane and Scher, 1969):
    double JIratio = 0.25;
    
    // For actuators:
	double maxTorque = 40.0;                               // N-m


    // Basics
    // ------------------------------------------------------------------------
    // Create the cat model.
    OpenSim::Model cat;

    // Name the cat after the founder of Stanford University.
    // This model will be able to hunch and wag (see the joints below).
    cat.setName("Leland_hunch_wag");

    // 'Turn off' gravity so it's easier to watch an animation of the flip.
    using SimTK::Vec3;
    cat.setGravity(Vec3(0, 0, 0));


    // Define anterior and posterior halves of the cat
    // ------------------------------------------------------------------------
    // Prepare inertia properties for the 2 primary segments of the cat.
    double segmentalAxialMomentOfInertia = JIratio * segmentalTransverseMomentOfInertia;
    double Ixx = segmentalAxialMomentOfInertia;
    double Iyy = segmentalTransverseMomentOfInertia;
    double Izz = segmentalTransverseMomentOfInertia;
    double Ixy = 0;
    double Ixz = 0;
    double Iyz = 0;
    using SimTK::Inertia;
    Inertia segmentalInertia = Inertia(Ixx, Iyy, Izz, Ixz, Ixz, Iyz);

    // Anterior half of cat.
    using OpenSim::Body;
    Body * anteriorBody = new Body();
    anteriorBody->setName("anteriorBody");
    anteriorBody->setMass(segmentalMass);
    // By choosing the following as the mass center, we choose the origin of
    // the anteriorBody frame to be at the body's positive-X extent. That is,
    // the anterior body sits to the -X direction from its origin.
    anteriorBody->setMassCenter(Vec3(-0.5 * segmentalLength, 0, 0));
    anteriorBody->setInertia(segmentalInertia);

    // Posterior half of cat (same mass properties as anterior half).
    Body * posteriorBody = new Body();
    posteriorBody->setName("posteriorBody");
    posteriorBody->setMass(segmentalMass);
    // Posterior body sits to the +X direction from its origin.
    posteriorBody->setMassCenter(Vec3(0.5 * segmentalLength, 0, 0));
    posteriorBody->setInertia(segmentalInertia);


    // Define joints between bodies (ground and two halves)
    // ------------------------------------------------------------------------
    Body & ground = cat.getGroundBody();

    // Anterior body to the ground via a CustomJoint
    // `````````````````````````````````````````````
    using OpenSim::CustomJoint;
	// Rotation is defined via YZX Euler angles, named yaw, pitch, and
	// roll respectively.

    // To pass to the CustomJoint (farther down), define a SpatialTransform.
    // The SpatialTransfrom has 6 transform axes. The first 3 are rotations,
    // defined about the axes of our choosing. The remaining 3 are translations,
    // which we choose to be along the X, Y, and Z directions of the ground's
    // frame.
    using OpenSim::Array;
    OpenSim::SpatialTransform groundAnteriorST;
    groundAnteriorST.updTransformAxis(0).setCoordinateNames(
            Array<std::string>("yaw", 1));
    groundAnteriorST.updTransformAxis(0).setAxis(Vec3(0, 1, 0));

    groundAnteriorST.updTransformAxis(1).setCoordinateNames(
            Array<std::string>("pitch", 1));
    groundAnteriorST.updTransformAxis(1).setAxis(Vec3(0, 0, 1));

    groundAnteriorST.updTransformAxis(2).setCoordinateNames(
            Array<std::string>("roll", 1));
    groundAnteriorST.updTransformAxis(2).setAxis(Vec3(1, 0, 0));

    groundAnteriorST.updTransformAxis(3).setCoordinateNames(
            Array<std::string>("tx", 1));
    groundAnteriorST.updTransformAxis(3).setAxis(Vec3(1, 0, 0));

    groundAnteriorST.updTransformAxis(4).setCoordinateNames(
            Array<std::string>("ty", 1));
    groundAnteriorST.updTransformAxis(4).setAxis(Vec3(0, 1, 0));

    groundAnteriorST.updTransformAxis(5).setCoordinateNames(
            Array<std::string>("tz", 1));
    groundAnteriorST.updTransformAxis(5).setAxis(Vec3(0, 0, 1));

	Vec3 locGAInGround(0);
    Vec3 orientGAInGround(0);
    Vec3 locGAInAnterior(0);
    Vec3 orientGAInAnterior(0);

    CustomJoint * groundAnterior = new CustomJoint("ground_anterior",
            ground, locGAInGround, orientGAInGround,
            *anteriorBody, locGAInAnterior, orientGAInAnterior,
            groundAnteriorST);

    // Edit the Coordinate's created by the CustomJoint. The 6 coordinates
    // correspond to the TransformAxis's we set above.
    using OpenSim::CoordinateSet;
    using SimTK::convertDegreesToRadians;
    using SimTK::Pi;
    CoordinateSet & groundAnteriorCS = groundAnterior->upd_CoordinateSet();
    // yaw
    // As is, the range only affects how one can vary this coordinate in the
    // GUI. The range is not a joint limit, and does not affect dynamics.
    double groundAnteriorCS0range[2] = {-Pi, Pi};
    groundAnteriorCS[0].setRange(groundAnteriorCS0range);
    groundAnteriorCS[0].setDefaultValue(0);
    groundAnteriorCS[0].setDefaultLocked(false);
    // pitch
    double groundAnteriorCS1range[2] = {-Pi, Pi};
    groundAnteriorCS[1].setRange(groundAnteriorCS1range);
    groundAnteriorCS[1].setDefaultValue(convertDegreesToRadians(-15));
    groundAnteriorCS[1].setDefaultLocked(false);
    // roll
    double groundAnteriorCS2range[2] = {-Pi, Pi};
    groundAnteriorCS[2].setRange(groundAnteriorCS2range);
    groundAnteriorCS[2].setDefaultValue(0);
    groundAnteriorCS[2].setDefaultLocked(false);
    // tx
    double groundAnteriorCS3range[2] = {-1, 1};
    groundAnteriorCS[3].setRange(groundAnteriorCS3range);
    groundAnteriorCS[3].setDefaultValue(0);
	groundAnteriorCS[3].setDefaultLocked(false);
    // ty
    double groundAnteriorCS4range[2] = {-1, 5};
    groundAnteriorCS[4].setRange(groundAnteriorCS4range);
    groundAnteriorCS[4].setDefaultValue(0);
	groundAnteriorCS[4].setDefaultLocked(false);
    // tz
    double groundAnteriorCS5range[2] = {-1, 1};
    groundAnteriorCS[5].setRange(groundAnteriorCS5range);
    groundAnteriorCS[5].setDefaultValue(0);
    groundAnteriorCS[5].setDefaultLocked(false);

    // Anterior to posterior body via a CustomJoint
    // ````````````````````````````````````````````
	// Rotation is defined via ZYX Euler angles.

    OpenSim::SpatialTransform anteriorPosteriorST;
	anteriorPosteriorST.updTransformAxis(0).setCoordinateNames(
            Array<std::string>("hunch", 1));
    anteriorPosteriorST.updTransformAxis(0).setAxis(Vec3(0, 0, 1));

    anteriorPosteriorST.updTransformAxis(1).setCoordinateNames(
            Array<std::string>("wag", 1));
    anteriorPosteriorST.updTransformAxis(1).setAxis(Vec3(0, 1, 0));

    anteriorPosteriorST.updTransformAxis(2).setCoordinateNames(
            Array<std::string>("twist", 1));
	anteriorPosteriorST.updTransformAxis(2).setAxis(Vec3(1, 0, 0));
    // There is no translation between the segments, and so we do not name the
    // remaining 3 TransformAxis's in the SpatialTransform.

    Vec3 locAPInAnterior(0);
    Vec3 orientAPInAnterior(0);
    Vec3 locAPInPosterior(0);
    Vec3 orientAPInPosterior(0);

    CustomJoint * anteriorPosterior = new CustomJoint("anterior_posterior",
            *anteriorBody, locAPInAnterior, orientAPInAnterior,
            *posteriorBody, locAPInPosterior, orientAPInPosterior,
			anteriorPosteriorST);

    // Set coordinate limits and default values from empirical data (i.e.,
    // photos & video).
	CoordinateSet & anteriorPosteriorCS = anteriorPosterior->upd_CoordinateSet();
    // hunch: [-20, +90] degrees
    double anteriorPosteriorCS0range[2] = {convertDegreesToRadians(-20),
										   convertDegreesToRadians(90)};
    anteriorPosteriorCS[0].setRange(anteriorPosteriorCS0range);
    anteriorPosteriorCS[0].setDefaultValue(convertDegreesToRadians(30));
    anteriorPosteriorCS[0].setDefaultLocked(false);
	// wag: [-45, 45] degrees
    double anteriorPosteriorCS1range[2] = {-0.25 * Pi, 0.25 * Pi};
    anteriorPosteriorCS[1].setRange(anteriorPosteriorCS1range);
    anteriorPosteriorCS[1].setDefaultValue(0);
    anteriorPosteriorCS[1].setDefaultLocked(false);
	// twist: [-80, 80] degrees
    double anteriorPosteriorCS2range[2] = {convertDegreesToRadians(-80),
										   convertDegreesToRadians(80)};
    anteriorPosteriorCS[2].setRange(anteriorPosteriorCS2range);
    anteriorPosteriorCS[2].setDefaultValue(0);
    // This first model can't twist; we'll unlock this for the next model.
    anteriorPosteriorCS[2].setDefaultLocked(true);


    // Add bodies to the model
    // ------------------------------------------------------------------------
    // ...now that we have connected the bodies via joints.
    cat.addBody(anteriorBody);
    cat.addBody(posteriorBody);


    // Display geometry
    // ------------------------------------------------------------------------
    using OpenSim::DisplayGeometry;
    // So that we can see what the cat's up to.
    // By default, the cylinder has a diameter of 1 meter, height of 1 meter,
    // its centroid is at (0, 0, 0) (its origin), and its axis of symmetry is
    // its Y axis.

    // Anterior body
    // `````````````
    // 'cylinder.vtp' is in the Geometry folder of an OpenSim installation.
    DisplayGeometry * anteriorDisplay = new DisplayGeometry("cylinder.vtp");
    anteriorDisplay->setOpacity(0.5);
    anteriorDisplay->setColor(Vec3(0.5, 0.5, 0.5));

    // We want the centroid to be at (-0.5 * segmentalLength, 0, 0), and for
    // its axis of symmetry to be its body's (i.e., the body it's helping us
    // to visualize) X axis.
    SimTK::Rotation rot;
    // Rotate the cylinder's symmetry (Y) axis to align with the body's X axis:
    rot.setRotationFromAngleAboutZ(0.5 * Pi);
    // Tranform combines rotation and translation:
    anteriorDisplay->setTransform(
            SimTK::Transform(rot, Vec3(-0.5 * segmentalLength, 0, 0)));
    anteriorDisplay->setScaleFactors(
            Vec3(segmentalDiam, segmentalLength, segmentalDiam));
    anteriorBody->updDisplayer()->updGeometrySet().adoptAndAppend(anteriorDisplay);
    anteriorBody->updDisplayer()->setShowAxes(true);

    // Posterior body
    // ``````````````
    DisplayGeometry * posteriorDisplay = new DisplayGeometry("cylinder.vtp");
    posteriorDisplay->setOpacity(0.5);
    posteriorDisplay->setColor(Vec3(0.7, 0.7, 0.7));
    
    posteriorDisplay->setTransform(
            SimTK::Transform(rot, Vec3(0.5 * segmentalLength, 0, 0)));
    posteriorDisplay->setScaleFactors(
            Vec3(segmentalDiam, segmentalLength, segmentalDiam));
    posteriorBody->updDisplayer()->updGeometrySet().adoptAndAppend(posteriorDisplay);
    posteriorBody->updDisplayer()->setShowAxes(true);


    // Actuation
    // ------------------------------------------------------------------------
    // Since the coordinates between the segments are angles, the actuators are
    // effectively torque actuators. The reason to use a CoordinateActuator
    // instead of a TorqueActuator is that for a CoordinateActuator we needn't
    // specify the axis of the actuation, or the bodies on which it acts.
    using OpenSim::CoordinateActuator;

    // hunch
    CoordinateActuator * hunchAct = new CoordinateActuator("hunch");
    hunchAct->setName("hunch_actuator");
    hunchAct->setMinControl(-maxTorque);
    hunchAct->setMaxControl(maxTorque);
    cat.addForce(hunchAct);

    // wag
    CoordinateActuator * wagAct = new CoordinateActuator("wag");
    wagAct->setName("wag_actuator");
    wagAct->setMinControl(-maxTorque);
    wagAct->setMaxControl(maxTorque);
    cat.addForce(wagAct);


    // Print the model
    // ------------------------------------------------------------------------
    cat.print("flippinfelines_hunch_wag.osim");
    
    
    

    // Second model: adding legs for the variable-inertia mechanism of flipping
    // ========================================================================
    // NOTE: Most of the set-up for this model has already been done. Many of
    // ----  the variables defined above are simply updated or renamed below.
    
    // This model will additionally be able to twist, and has legs.
    cat.setName("Leland_hunch_wag_twist_legs");

    // Allow twist.
    anteriorPosteriorCS[2].setDefaultLocked(false);

    CoordinateActuator * twistAct = new CoordinateActuator("twist");
    twistAct->setName("twist_actuator");
    twistAct->setMinControl(-maxTorque);
    twistAct->setMaxControl(maxTorque);
    cat.addForce(twistAct);
    
    
    // Leg properties
    // ------------------------------------------------------------------------
    double legsLength = 0.125;                             // m
    double legsDiam = 0.1 * legsLength;                    // m
    // Sum of both legs (60% distance across the belly):
    double legsWidth = 0.6 * segmentalDiam;                // m
    double legsMass = 0.2;                                 // kg
    
    
    // Define leg bodies
    // ------------------------------------------------------------------------
    // Scale the segmental inertia.
    Inertia legsInertia = (legsMass/segmentalMass) * segmentalInertia;

    // Anterior and posterior legs.
    Body * anteriorLegs = new Body();
    anteriorLegs->setName("anteriorLegs");
    anteriorLegs->setMass(legsMass);
    anteriorLegs->setMassCenter(Vec3(0.5 * legsLength, 0, 0));
    anteriorLegs->setInertia(legsInertia);

    Body * posteriorLegs = new Body();
    posteriorLegs->setName("posteriorLegs");
    posteriorLegs->setMass(legsMass);
    posteriorLegs->setMassCenter(Vec3(0.5 * legsLength, 0, 0));
    posteriorLegs->setInertia(legsInertia);
    
    
    // Define leg joints (i.e., between legs and two halves of cat)
    // ------------------------------------------------------------------------
    using OpenSim::PinJoint;

    // Anterior leg
    // ````````````
	Vec3 locALegsInAnterior(-0.75 * segmentalLength, 0.5 * segmentalDiam, 0);
    Vec3 orientALegsInAnterior(0);
    Vec3 locALegsInLegs(0);
    // Rotate the leg about what will become the pin-joint axis (the leg's
    // Z axis) so that it points straight out from the belly when leg angle
    // is 0. In other words, position the leg's long (X) axis normal to the
    // half of the cat.
    Vec3 orientALegsInLegs(0, 0, -0.5 * Pi);
    
    PinJoint * anteriorToLegs = new PinJoint("anterior_legs",
            *anteriorBody, locALegsInAnterior, orientALegsInAnterior,
            *anteriorLegs, locALegsInLegs, orientALegsInLegs);
    CoordinateSet & anteriorToLegsCS = anteriorToLegs->upd_CoordinateSet();
    anteriorToLegsCS[0].setName("frontLegs");
    double anteriorToLegsCS0range[2] = {-0.5 * Pi, 0.5 * Pi};
    anteriorToLegsCS[0].setRange(anteriorToLegsCS0range);

    // So that the legs are directed strictly upwards initially:
    double pitch = groundAnteriorCS[1].getDefaultValue();
    anteriorToLegsCS[0].setDefaultValue(-pitch);
    anteriorToLegsCS[0].setDefaultLocked(false);

    // Posterior leg
    // `````````````
	Vec3 locPLegsInPosterior(0.75 * segmentalLength, 0.5 * segmentalDiam, 0);
    Vec3 orientPLegsInPosterior(0, Pi, 0);
    Vec3 locPLegsInLegs(0);
    Vec3 orientPLegsInLegs(0, 0, -0.5 * Pi);
    
    PinJoint * posteriorToLegs = new PinJoint("posterior_legs",
            *posteriorBody, locPLegsInPosterior, orientPLegsInPosterior,
            *posteriorLegs, locPLegsInLegs, orientPLegsInLegs);
    CoordinateSet & posteriorToLegsCS = posteriorToLegs->upd_CoordinateSet();
    posteriorToLegsCS[0].setName("backLegs");
    double posteriorToLegsCS0range[2] = {-0.5 * Pi, 0.5 * Pi};
    posteriorToLegsCS[0].setRange(posteriorToLegsCS0range);
    posteriorToLegsCS[0].setDefaultValue(-pitch);
    posteriorToLegsCS[0].setDefaultLocked(false);
    
    
    // Add bodies to the model
    // ------------------------------------------------------------------------
    // ...now that we have connected the bodies via joints.
    cat.addBody(anteriorLegs);
    cat.addBody(posteriorLegs);
    
    
    // Display geometry
    // ------------------------------------------------------------------------
    // Both legs have the same display geometry.

    // 'box.vtp' is in the Geometry folder of an OpenSim installation.
    DisplayGeometry legsDisplay = DisplayGeometry("box.vtp");
    legsDisplay.setOpacity(0.5);
    legsDisplay.setColor(Vec3(0.7, 0.7, 0.7));
    legsDisplay.setTransform(Transform(Vec3(0.3 * legsLength, 0, 0)));
    legsDisplay.setScaleFactors(Vec3(legsLength, legsDiam, legsWidth));

    anteriorLegs->updDisplayer()->updGeometrySet().cloneAndAppend(legsDisplay);
    anteriorLegs->updDisplayer()->setShowAxes(true);

    posteriorLegs->updDisplayer()->updGeometrySet().cloneAndAppend(legsDisplay);
    posteriorLegs->updDisplayer()->setShowAxes(true);
    

    // Actuation
    // ------------------------------------------------------------------------
    // front legs.
    CoordinateActuator * frontLegsAct = new CoordinateActuator("frontLegs");
    frontLegsAct->setName("frontLegs_actuator");
    frontLegsAct->setMinControl(-maxTorque);
    frontLegsAct->setMaxControl(maxTorque);
    cat.addForce(frontLegsAct);

    // back legs.
    CoordinateActuator * backLegsAct = new CoordinateActuator("backLegs");
    backLegsAct->setName("backLegs_actuator");
    backLegsAct->setMinControl(-maxTorque);
    backLegsAct->setMaxControl(maxTorque);
    cat.addForce(backLegsAct);

    
    // Enforce joint limits on the legs
    // ------------------------------------------------------------------------
    using OpenSim::CoordinateLimitForce;

    CoordinateLimitForce * frontLegsLimitForce = new CoordinateLimitForce(
                "frontLegs", 90, 1.0E2, -90, 1.0E2, 1.0E1, 2.0, false);
	cat.addForce(frontLegsLimitForce);

    CoordinateLimitForce * backLegsLimitForce = new CoordinateLimitForce(
                "backLegs", 90, 1.0E2, -90, 1.0E2, 1.0E1, 2.0, false);
	cat.addForce(backLegsLimitForce);
    
    
    // Print the model
    // ------------------------------------------------------------------------
    cat.print("flippinfelines_hunch_wag_twist_legs.osim");

    return EXIT_SUCCESS;
};