void addCustomMarkers(OpenSim::Model& model,
SimTK::Markers& markers, SimTK::OrientationSensors& imus)
{
// add orientation osensor
m_humerus_mx = imus.addOSensor(
"humerus",
model.updBodySet().get(m_humerus_body_name).getMobilizedBodyIndex(),
SimTK::Rotation(SimTK::BodyOrSpaceType::BodyRotationSequence,
-SimTK::Pi / 2, SimTK::ZAxis,
SimTK::Pi / 2, SimTK::XAxis),
1);
m_radius_mx = imus.addOSensor(
"radius",
model.updBodySet().get(m_radius_body_name).getMobilizedBodyIndex(),
SimTK::Rotation(SimTK::BodyOrSpaceType::BodyRotationSequence,
-SimTK::Pi / 2, SimTK::ZAxis,
SimTK::Pi , SimTK::XAxis),
1);
// finalize observation order (to allocate ObservationIx)
static const char* osensor_observation_order[OSENSORS] = {"humerus", "radius"};
imus.defineObservationOrder(OSENSORS, osensor_observation_order);
// get all ObservationIx
m_humerus_ox = imus.getObservationIxForOSensor(m_humerus_mx);
m_radius_ox = imus.getObservationIxForOSensor(m_radius_mx);
}
/**
* 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));
}
}
void createPointCostraint(OpenSim::Model &aModel, std::string parentBodyName, SimTK::Vec3 locationInParent, std::string childBodyName, SimTK::Vec3 locationInChild ){
OpenSim::PointConstraint *costr = new OpenSim::PointConstraint(aModel.updBodySet().get(parentBodyName), locationInParent, aModel.updBodySet().get(childBodyName), locationInChild);
costr->setName(childBodyName+"To"+parentBodyName);
aModel.addConstraint(costr);
}
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;
}
/**
* 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;
};