/**
* 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));
}
}
//==========================================================================================================
// Test Cases
//==========================================================================================================
int testBouncingBall(bool useMesh)
{
// Setup OpenSim model
Model *osimModel = new Model;
//OpenSim bodies
OpenSim::Body& ground = *new OpenSim::Body("ground", SimTK::Infinity,
Vec3(0), Inertia());
osimModel->addBody(&ground);
OpenSim::Body ball;
ball.setName("ball");
ball.set_mass(mass);
ball.set_mass_center(Vec3(0));
ball.setInertia(Inertia(1.0));
// Add joints
FreeJoint free("free", ground, Vec3(0), Vec3(0), ball, Vec3(0), Vec3(0));
osimModel->addBody(&ball);
osimModel->addJoint(&free);
// Create ContactGeometry.
ContactHalfSpace *floor = new ContactHalfSpace(Vec3(0), Vec3(0, 0, -0.5*SimTK_PI), ground, "ground");
osimModel->addContactGeometry(floor);
OpenSim::ContactGeometry* geometry;
if (useMesh)
geometry = new ContactMesh(mesh_file, Vec3(0), Vec3(0), ball, "ball");
else
geometry = new ContactSphere(radius, Vec3(0), ball, "ball");
osimModel->addContactGeometry(geometry);
OpenSim::Force* force;
if (useMesh)
{
// Add an ElasticFoundationForce.
OpenSim::ElasticFoundationForce::ContactParameters* contactParams = new OpenSim::ElasticFoundationForce::ContactParameters(1.0e6/radius, 1e-5, 0.0, 0.0, 0.0);
contactParams->addGeometry("ball");
contactParams->addGeometry("ground");
force = new OpenSim::ElasticFoundationForce(contactParams);
osimModel->addForce(force);
}
else
{
// Add a HuntCrossleyForce.
OpenSim::HuntCrossleyForce::ContactParameters* contactParams = new OpenSim::HuntCrossleyForce::ContactParameters(1.0e6, 1e-5, 0.0, 0.0, 0.0);
contactParams->addGeometry("ball");
contactParams->addGeometry("ground");
force = new OpenSim::HuntCrossleyForce(contactParams);
osimModel->addForce(force);
}
osimModel->setGravity(gravity_vec);
osimModel->setName("TestContactGeomtery_Ball");
osimModel->clone()->print("TestContactGeomtery_Ball.osim");
Kinematics* kin = new Kinematics(osimModel);
osimModel->addAnalysis(kin);
SimTK::State& osim_state = osimModel->initSystem();
osim_state.updQ()[4] = height;
osimModel->getMultibodySystem().realize(osim_state, Stage::Position );
//Initial system energy is all potential
double Etot_orig = mass*(-gravity_vec[1])*height;
//==========================================================================================================
// Simulate it and see if it bounces correctly.
cout << "stateY=" << osim_state.getY() << std::endl;
RungeKuttaMersonIntegrator integrator(osimModel->getMultibodySystem() );
integrator.setAccuracy(integ_accuracy);
Manager manager(*osimModel, integrator);
for (unsigned int i = 0; i < duration/interval; ++i)
{
manager.setInitialTime(i*interval);
manager.setFinalTime((i+1)*interval);
manager.integrate(osim_state);
double time = osim_state.getTime();
osimModel->getMultibodySystem().realize(osim_state, Stage::Acceleration);
Vec3 pos, vel;
osimModel->updSimbodyEngine().getPosition(osim_state, osimModel->getBodySet().get("ball"), Vec3(0), pos);
osimModel->updSimbodyEngine().getVelocity(osim_state, osimModel->getBodySet().get("ball"), Vec3(0), vel);
double Etot = mass*((-gravity_vec[1])*pos[1] + 0.5*vel[1]*vel[1]);
//cout << "starting system energy = " << Etot_orig << " versus current energy = " << Etot << endl;
// contact absorbs and returns energy so make sure not in contact
if (pos[1] > 2*radius)
{
ASSERT_EQUAL(Etot_orig, Etot, 1e-2, __FILE__, __LINE__, "Bouncing ball on plane Failed: energy was not conserved.");
}
else
{
cout << "In contact at time = " << time << endl;
ASSERT(pos[1] < 5.0 && pos[1] > 0);
}
ASSERT_EQUAL(0.0, pos[0], 1e-4);
ASSERT_EQUAL(0.0, pos[2], 1e-4);
ASSERT_EQUAL(0.0, vel[0], 1e-3);
ASSERT_EQUAL(0.0, vel[2], 1e-3);
}
std::string prefix = useMesh?"Kinematics_Mesh":"Kinematics_NoMesh";
kin->printResults(prefix);
osimModel->disownAllComponents();
// model takes ownership of components unless container set is told otherwise
delete osimModel;
return 0;
}
/**
* Run a simulation of block sliding with contact on by two muscles sliding with contact
*/
int main()
{
clock_t startTime = clock();
try {
//////////////////////
// MODEL PARAMETERS //
//////////////////////
// Specify body mass of a 20 kg, 0.1m sides of cubed block body
double blockMass = 20.0, blockSideLength = 0.1;
// Constant distance of constraint to limit the block's motion
double constantDistance = 0.2;
// Contact parameters
double stiffness = 1.0e7, dissipation = 0.1, friction = 0.2, viscosity=0.01;
///////////////////////////////////////////
// DEFINE BODIES AND JOINTS OF THE MODEL //
///////////////////////////////////////////
// Create an OpenSim model and set its name
Model osimModel;
osimModel.setName("tugOfWar");
// GROUND BODY
// Get a reference to the model's ground body
OpenSim::Body& ground = osimModel.getGroundBody();
// Add display geometry to the ground to visualize in the Visualizer and GUI
// add a checkered floor
ground.addDisplayGeometry("checkered_floor.vtp");
// add anchors for the muscles to be fixed too
ground.addDisplayGeometry("block.vtp");
ground.addDisplayGeometry("block.vtp");
// block is 0.1 by 0.1 by 0.1m cube and centered at origin.
// transform anchors to be placed at the two extremes of the sliding block (to come)
GeometrySet& geometry = ground.updDisplayer()->updGeometrySet();
DisplayGeometry& anchor1 = geometry[1];
DisplayGeometry& anchor2 = geometry[2];
// scale the anchors
anchor1.setScaleFactors(Vec3(5, 1, 1));
anchor2.setScaleFactors(Vec3(5, 1, 1));
// reposition the anchors
anchor1.setTransform(Transform(Vec3(0, 0.05, 0.35)));
anchor2.setTransform(Transform(Vec3(0, 0.05, -0.35)));
// BLOCK BODY
Vec3 blockMassCenter(0);
Inertia blockInertia = blockMass*Inertia::brick(blockSideLength, blockSideLength, blockSideLength);
// Create a new block body with the specified properties
OpenSim::Body *block = new OpenSim::Body("block", blockMass, blockMassCenter, blockInertia);
// Add display geometry to the block to visualize in the GUI
block->addDisplayGeometry("block.vtp");
// FREE JOINT
// Create a new free joint with 6 degrees-of-freedom (coordinates) between the block and ground bodies
Vec3 locationInParent(0, blockSideLength/2, 0), orientationInParent(0), locationInBody(0), orientationInBody(0);
FreeJoint *blockToGround = new FreeJoint("blockToGround", ground, locationInParent, orientationInParent, *block, locationInBody, orientationInBody);
// Get a reference to the coordinate set (6 degrees-of-freedom) between the block and ground bodies
CoordinateSet& jointCoordinateSet = blockToGround->upd_CoordinateSet();
// Set the angle and position ranges for the coordinate set
double angleRange[2] = {-SimTK::Pi/2, SimTK::Pi/2};
double positionRange[2] = {-1, 1};
jointCoordinateSet[0].setRange(angleRange);
jointCoordinateSet[1].setRange(angleRange);
jointCoordinateSet[2].setRange(angleRange);
jointCoordinateSet[3].setRange(positionRange);
jointCoordinateSet[4].setRange(positionRange);
jointCoordinateSet[5].setRange(positionRange);
// GRAVITY
// Obtaine the default acceleration due to gravity
Vec3 gravity = osimModel.getGravity();
// Define non-zero default states for the free joint
jointCoordinateSet[3].setDefaultValue(constantDistance); // set x-translation value
double h_start = blockMass*gravity[1]/(stiffness*blockSideLength*blockSideLength);
jointCoordinateSet[4].setDefaultValue(h_start); // set y-translation which is height
// Add the block and joint to the model
osimModel.addBody(block);
osimModel.addJoint(blockToGround);
///////////////////////////////////////////////
// DEFINE THE SIMULATION START AND END TIMES //
///////////////////////////////////////////////
// Define the initial and final simulation times
double initialTime = 0.0;
double finalTime = 3.00;
/////////////////////////////////////////////
// DEFINE CONSTRAINTS IMPOSED ON THE MODEL //
/////////////////////////////////////////////
Vec3 pointOnGround(0, blockSideLength/2 ,0);
Vec3 pointOnBlock(0, 0, 0);
// Create a new constant distance constraint
ConstantDistanceConstraint *constDist =
new ConstantDistanceConstraint(ground,
pointOnGround, *block, pointOnBlock, constantDistance);
// Add the new point on a line constraint to the model
osimModel.addConstraint(constDist);
///////////////////////////////////////
// DEFINE FORCES ACTING ON THE MODEL //
///////////////////////////////////////
// MUSCLE FORCES
// Create two new muscles with identical properties
double maxIsometricForce = 1000.0, optimalFiberLength = 0.25, tendonSlackLength = 0.1, pennationAngle = 0.0;
Thelen2003Muscle *muscle1 = new Thelen2003Muscle("muscle1",maxIsometricForce,optimalFiberLength,tendonSlackLength,pennationAngle);
Thelen2003Muscle *muscle2 = new Thelen2003Muscle("muscle2",maxIsometricForce,optimalFiberLength,tendonSlackLength,pennationAngle);
// Specify the paths for the two muscles
// Path for muscle 1
muscle1->addNewPathPoint("muscle1-point1", ground, Vec3(0.0,0.05,-0.35));
muscle1->addNewPathPoint("muscle1-point2", *block, Vec3(0.0,0.0,-0.05));
// Path for muscle 2
muscle2->addNewPathPoint("muscle2-point1", ground, Vec3(0.0,0.05,0.35));
muscle2->addNewPathPoint("muscle2-point2", *block, Vec3(0.0,0.0,0.05));
// Add the two muscles (as forces) to the model
osimModel.addForce(muscle1);
osimModel.addForce(muscle2);
// CONTACT FORCE
// Define contact geometry
// Create new floor contact halfspace
ContactHalfSpace *floor = new ContactHalfSpace(SimTK::Vec3(0), SimTK::Vec3(0, 0, -0.5*SimTK_PI), ground, "floor");
// Create new cube contact mesh
OpenSim::ContactMesh *cube = new OpenSim::ContactMesh("blockMesh.obj", SimTK::Vec3(0), SimTK::Vec3(0), *block, "cube");
// Add contact geometry to the model
osimModel.addContactGeometry(floor);
osimModel.addContactGeometry(cube);
// Define contact parameters for elastic foundation force
OpenSim::ElasticFoundationForce::ContactParameters *contactParams =
new OpenSim::ElasticFoundationForce::ContactParameters(stiffness, dissipation, friction, friction, viscosity);
contactParams->addGeometry("cube");
contactParams->addGeometry("floor");
// Create a new elastic foundation (contact) force between the floor and cube.
OpenSim::ElasticFoundationForce *contactForce = new OpenSim::ElasticFoundationForce(contactParams);
contactForce->setName("contactForce");
// Add the new elastic foundation force to the model
osimModel.addForce(contactForce);
// PRESCRIBED FORCE
// Create a new prescribed force to be applied to the block
PrescribedForce *prescribedForce = new PrescribedForce(block);
prescribedForce->setName("prescribedForce");
// Specify properties of the force function to be applied to the block
double time[2] = {0, finalTime}; // time nodes for linear function
double fXofT[2] = {0, -blockMass*gravity[1]*3.0}; // force values at t1 and t2
// Create linear function for the force components
PiecewiseLinearFunction *forceX = new PiecewiseLinearFunction(2, time, fXofT);
// Set the force and point functions for the new prescribed force
prescribedForce->setForceFunctions(forceX, new Constant(0.0), new Constant(0.0));
prescribedForce->setPointFunctions(new Constant(0.0), new Constant(0.0), new Constant(0.0));
// Add the new prescribed force to the model
osimModel.addForce(prescribedForce);
///////////////////////////////////
// DEFINE CONTROLS FOR THE MODEL //
///////////////////////////////////
// Create a prescribed controller that simply applies controls as function of time
// For muscles, controls are normalized motor-neuron excitations
PrescribedController *muscleController = new PrescribedController();
muscleController->setActuators(osimModel.updActuators());
// Define linear functions for the control values for the two muscles
Array<double> slopeAndIntercept1(0.0, 2); // array of 2 doubles
Array<double> slopeAndIntercept2(0.0, 2);
// muscle1 control has slope of -1 starting 1 at t = 0
slopeAndIntercept1[0] = -1.0/(finalTime-initialTime); slopeAndIntercept1[1] = 1.0;
// muscle2 control has slope of 0.95 starting 0.05 at t = 0
slopeAndIntercept2[0] = 0.95/(finalTime-initialTime); slopeAndIntercept2[1] = 0.05;
// Set the indiviudal muscle control functions for the prescribed muscle controller
muscleController->prescribeControlForActuator("muscle1", new LinearFunction(slopeAndIntercept1));
muscleController->prescribeControlForActuator("muscle2", new LinearFunction(slopeAndIntercept2));
// Add the muscle controller to the model
osimModel.addController(muscleController);
///////////////////////////////////
// SPECIFY MODEL DEFAULT STATES //
///////////////////////////////////
// Define the default states for the two muscles
// Activation
muscle1->setDefaultActivation(slopeAndIntercept1[1]);
muscle2->setDefaultActivation(slopeAndIntercept2[1]);
// Fiber length
muscle2->setDefaultFiberLength(optimalFiberLength);
muscle1->setDefaultFiberLength(optimalFiberLength);
// Save the model to a file
osimModel.print("tugOfWar_model.osim");
//////////////////////////
// PERFORM A SIMULATION //
//////////////////////////
// set use visualizer to true to visualize the simulation live
osimModel.setUseVisualizer(false);
// Initialize the system and get the default state
SimTK::State& si = osimModel.initSystem();
// Enable constraint consistent with current configuration of the model
constDist->setDisabled(si, false);
cout << "Start height = "<< h_start << endl;
osimModel.getMultibodySystem().realize(si, Stage::Velocity);
// Compute initial conditions for muscles
osimModel.equilibrateMuscles(si);
double mfv1 = muscle1->getFiberVelocity(si);
double mfv2 = muscle2->getFiberVelocity(si);
// Create the force reporter for obtaining the forces applied to the model
// during a forward simulation
ForceReporter* reporter = new ForceReporter(&osimModel);
osimModel.addAnalysis(reporter);
// Create the integrator for integrating system dynamics
SimTK::RungeKuttaMersonIntegrator integrator(osimModel.getMultibodySystem());
integrator.setAccuracy(1.0e-6);
// Create the manager managing the forward integration and its outputs
Manager manager(osimModel, integrator);
// Print out details of the model
osimModel.printDetailedInfo(si, cout);
// Integrate from initial time to final time
manager.setInitialTime(initialTime);
manager.setFinalTime(finalTime);
cout<<"\nIntegrating from "<<initialTime<<" to "<<finalTime<<endl;
manager.integrate(si);
//////////////////////////////
// SAVE THE RESULTS TO FILE //
//////////////////////////////
// Save the model states from forward integration
Storage statesDegrees(manager.getStateStorage());
statesDegrees.print("tugOfWar_states.sto");
// Save the forces
reporter->getForceStorage().print("tugOfWar_forces.mot");
}
catch (const std::exception& ex)
{
cerr << ex.what() << endl;
return 1;
}
catch (...)
{
cerr << "UNRECOGNIZED EXCEPTION" << endl;
return 1;
}
cout << "main() routine time = " << 1.e3*(clock()-startTime)/CLOCKS_PER_SEC << "ms\n";
cout << "OpenSim example completed successfully." << endl;
return 0;
}
// test sphere to sphere contact using elastic foundation with and without
// meshes and their combination
int testBallToBallContact(bool useElasticFoundation, bool useMesh1, bool useMesh2)
{
// Setup OpenSim model
Model *osimModel = new Model;
//OpenSim bodies
OpenSim::Body& ground = *new OpenSim::Body("ground", SimTK::Infinity,
Vec3(0), Inertia());
osimModel->addBody(&ground);
OpenSim::Body ball;
ball.setName("ball");
ball.setMass(mass);
ball.setMassCenter(Vec3(0));
ball.setInertia(Inertia(1.0));
// Add joints
FreeJoint free("free", ground, Vec3(0), Vec3(0), ball, Vec3(0), Vec3(0));
osimModel->addBody(&ball);
osimModel->addJoint(&free);
// Create ContactGeometry.
OpenSim::ContactGeometry *ball1, *ball2;
if (useElasticFoundation && useMesh1)
ball1 = new ContactMesh(mesh_file, Vec3(0), Vec3(0), ground, "ball1");
else
ball1 = new ContactSphere(radius, Vec3(0), ground, "ball1");
if (useElasticFoundation && useMesh2)
ball2 = new ContactMesh(mesh_file, Vec3(0), Vec3(0), ball, "ball2");
else
ball2 = new ContactSphere(radius, Vec3(0), ball, "ball2");
osimModel->addContactGeometry(ball1);
osimModel->addContactGeometry(ball2);
OpenSim::Force* force;
std::string prefix;
if (useElasticFoundation){
}
else{
}
if (useElasticFoundation)
{
// Add an ElasticFoundationForce.
OpenSim::ElasticFoundationForce::ContactParameters* contactParams = new OpenSim::ElasticFoundationForce::ContactParameters(1.0e6/(2*radius), 0.001, 0.0, 0.0, 0.0);
contactParams->addGeometry("ball1");
contactParams->addGeometry("ball2");
force = new OpenSim::ElasticFoundationForce(contactParams);
prefix = "EF_";
prefix += useMesh1 ?"Mesh":"noMesh";
prefix += useMesh2 ? "_to_Mesh":"_to_noMesh";
}
else
{
// Add a Hertz HuntCrossleyForce.
OpenSim::HuntCrossleyForce::ContactParameters* contactParams = new OpenSim::HuntCrossleyForce::ContactParameters(1.0e6, 0.001, 0.0, 0.0, 0.0);
contactParams->addGeometry("ball1");
contactParams->addGeometry("ball2");
force = new OpenSim::HuntCrossleyForce(contactParams);
prefix = "Hertz";
}
force->setName("contact");
osimModel->addForce(force);
osimModel->setGravity(gravity_vec);
osimModel->setName(prefix);
osimModel->clone()->print(prefix+".osim");
Kinematics* kin = new Kinematics(osimModel);
osimModel->addAnalysis(kin);
ForceReporter* reporter = new ForceReporter(osimModel);
osimModel->addAnalysis(reporter);
SimTK::State& osim_state = osimModel->initSystem();
osim_state.updQ()[4] = height;
osimModel->getMultibodySystem().realize(osim_state, Stage::Position );
//==========================================================================================================
// Simulate it and see if it bounces correctly.
cout << "stateY=" << osim_state.getY() << std::endl;
RungeKuttaMersonIntegrator integrator(osimModel->getMultibodySystem() );
integrator.setAccuracy(integ_accuracy);
integrator.setMaximumStepSize(100*integ_accuracy);
Manager manager(*osimModel, integrator);
manager.setInitialTime(0.0);
manager.setFinalTime(duration);
manager.integrate(osim_state);
kin->printResults(prefix);
reporter->printResults(prefix);
osimModel->disownAllComponents();
// model takes ownership of components unless container set is told otherwise
delete osimModel;
return 0;
}