Model* constructPendulumWithMarkers()
{
Model* pendulum = new Model();
pendulum->setName("pendulum");
Body* ball =
new Body("ball", 1.0, SimTK::Vec3(0), SimTK::Inertia::sphere(0.05));
pendulum->addBody(ball);
// PinJoint hinge is 1m above ground origin and 1m above the ball in the ball
// reference frame such that the ball center is at the origin with the hinge
// angle is zero
PinJoint* hinge = new PinJoint("hinge", pendulum->getGround(),
SimTK::Vec3(0, 1.0, 0), SimTK::Vec3(0),
*ball, SimTK::Vec3(0, 1.0, 0), SimTK::Vec3(0));
hinge->updCoordinate().setName("theta");
pendulum->addJoint(hinge);
// Add Markers
Marker*m0 = new Marker();
m0->setName("m0");
m0->setParentFrame(*ball);
m0->set_location(SimTK::Vec3(0));
pendulum->addMarker(m0);
// Shifted Right 1cm
Marker*mR = new Marker();
mR->setName("mR");
mR->setParentFrame(*ball);
mR->set_location(SimTK::Vec3(0.01, 0, 0));
pendulum->addMarker(mR);
// Shifted Left 2cm
Marker*mL = new Marker();
mL->setName("mL");
mL->setParentFrame(*ball);
mL->set_location(SimTK::Vec3(-0.02, 0, 0));
pendulum->addMarker(mL);
return pendulum;
}
//=============================================================================
// Simbody Model building.
//=============================================================================
//_____________________________________________________________________________
void PinJoint::addToSystem(SimTK::MultibodySystem& system) const
{
const SimTK::Vec3& orientation = getProperty_orientation().getValue();
const SimTK::Vec3& location = getProperty_location().getValue();
// CHILD TRANSFORM
Rotation rotation(BodyRotationSequence, orientation[0],XAxis, orientation[1],YAxis, orientation[2],ZAxis);
SimTK::Transform childTransform(rotation, location);
const SimTK::Vec3& orientationInParent = getProperty_orientation_in_parent().getValue();
const SimTK::Vec3& locationInParent = getProperty_location_in_parent().getValue();
// PARENT TRANSFORM
Rotation parentRotation(BodyRotationSequence, orientationInParent[0],XAxis, orientationInParent[1],YAxis, orientationInParent[2],ZAxis);
SimTK::Transform parentTransform(parentRotation, locationInParent);
PinJoint* mutableThis = const_cast<PinJoint*>(this);
mutableThis->createMobilizedBody(parentTransform, childTransform);
// TODO: Joints require super class to be called last.
Super::addToSystem(system);
}
Model* constructLegWithOrientationFrames()
{
std::unique_ptr<Model> leg{ new Model() };
leg->setName("leg");
Body* thigh =
new Body("thigh", 5.0, SimTK::Vec3(0),
SimTK::Inertia::cylinderAlongY(0.1, 0.5) );
leg->addBody(thigh);
Body* shank =
new Body("shank", 2.0, SimTK::Vec3(0),
SimTK::Inertia::cylinderAlongY(0.04, 0.4) );
leg->addBody(shank);
Body* foot =
new Body("foot", 1.0, SimTK::Vec3(0),
SimTK::Inertia::cylinderAlongY(0.02, 0.1));
leg->addBody(foot);
// PinJoint hip is 1m above ground origin and 1m above the ball in the ball
// reference frame such that the ball center is at the origin with the hinge
// angle is zero
PinJoint* hip = new PinJoint("hip", leg->getGround(),
SimTK::Vec3(0, 1.0, 0), SimTK::Vec3(0),
*thigh, SimTK::Vec3(0, 0.25, 0), SimTK::Vec3(0));
hip->updCoordinate().setName("flex");
leg->addJoint(hip);
PinJoint* knee = new PinJoint("knee", *thigh,
SimTK::Vec3(0, -0.25, 0), SimTK::Vec3(0),
*shank, SimTK::Vec3(0, 0.2, 0), SimTK::Vec3(0));
knee->updCoordinate().setName("flex");
leg->addJoint(knee);
PinJoint* ankle = new PinJoint("ankle", *shank,
SimTK::Vec3(0, -0.2, 0), SimTK::Vec3(0),
*foot, SimTK::Vec3(0, 0.1, 0), SimTK::Vec3(0));
ankle->updCoordinate().setName("flex");
leg->addJoint(ankle);
// Add Orientation Sensor Frames
SimTK::Transform offset(SimTK::Rotation(0.378, SimTK::YAxis) );
thigh->addComponent(new PhysicalOffsetFrame("thigh_imu", *thigh, offset));
shank->addComponent(new PhysicalOffsetFrame("shank_imu", *shank, offset));
foot->addComponent(new PhysicalOffsetFrame("foot_imu", *foot, offset));
return leg.release();
}
/**
* Run a simulation of block sliding with contact on by two muscles sliding with contact
*/
int main()
{
try {
// Create a new OpenSim model
Model osimModel;
osimModel.setName("osimModel");
double Pi = SimTK::Pi;
// Get the ground body
OpenSim::Body& ground = osimModel.getGroundBody();
ground.addDisplayGeometry("checkered_floor.vtp");
// create linkage body
double linkageMass = 0.001, linkageLength = 0.5, linkageDiameter = 0.06;
Vec3 linkageDimensions(linkageDiameter, linkageLength, linkageDiameter);
Vec3 linkageMassCenter(0,linkageLength/2,0);
Inertia linkageInertia = Inertia::cylinderAlongY(linkageDiameter/2.0, linkageLength/2.0);
OpenSim::Body* linkage1 = new OpenSim::Body("linkage1", linkageMass, linkageMassCenter, linkageMass*linkageInertia);
// Graphical representation
linkage1->addDisplayGeometry("cylinder.vtp");
//This cylinder.vtp geometry is 1 meter tall, 1 meter diameter. Scale and shift it to look pretty
GeometrySet& geometry = linkage1->updDisplayer()->updGeometrySet();
DisplayGeometry& thinCylinder = geometry[0];
thinCylinder.setScaleFactors(linkageDimensions);
thinCylinder.setTransform(Transform(Vec3(0.0,linkageLength/2.0,0.0)));
linkage1->addDisplayGeometry("sphere.vtp");
//This sphere.vtp is 1 meter in diameter. Scale it.
geometry[1].setScaleFactors(Vec3(0.1));
// Creat a second linkage body
OpenSim::Body* linkage2 = new OpenSim::Body(*linkage1);
linkage2->setName("linkage2");
// Creat a block to be the pelvis
double blockMass = 20.0, blockSideLength = 0.2;
Vec3 blockMassCenter(0);
Inertia blockInertia = blockMass*Inertia::brick(blockSideLength, blockSideLength, blockSideLength);
OpenSim::Body *block = new OpenSim::Body("block", blockMass, blockMassCenter, blockInertia);
block->addDisplayGeometry("block.vtp");
//This block.vtp is 0.1x0.1x0.1 meters. scale its appearance
block->updDisplayer()->updGeometrySet()[0].setScaleFactors(Vec3(2.0));
// Create 1 degree-of-freedom pin joints between the bodies to creat a kinematic chain from ground through the block
Vec3 orientationInGround(0), locationInGround(0), locationInParent(0.0, linkageLength, 0.0), orientationInChild(0), locationInChild(0);
PinJoint *ankle = new PinJoint("ankle", ground, locationInGround, orientationInGround, *linkage1,
locationInChild, orientationInChild);
PinJoint *knee = new PinJoint("knee", *linkage1, locationInParent, orientationInChild, *linkage2,
locationInChild, orientationInChild);
PinJoint *hip = new PinJoint("hip", *linkage2, locationInParent, orientationInChild, *block,
locationInChild, orientationInChild);
double range[2] = {-SimTK::Pi*2, SimTK::Pi*2};
CoordinateSet& ankleCoordinateSet = ankle->upd_CoordinateSet();
ankleCoordinateSet[0].setName("q1");
ankleCoordinateSet[0].setRange(range);
CoordinateSet& kneeCoordinateSet = knee->upd_CoordinateSet();
kneeCoordinateSet[0].setName("q2");
kneeCoordinateSet[0].setRange(range);
CoordinateSet& hipCoordinateSet = hip->upd_CoordinateSet();
hipCoordinateSet[0].setName("q3");
hipCoordinateSet[0].setRange(range);
// Add the bodies to the model
osimModel.addBody(linkage1);
osimModel.addBody(linkage2);
osimModel.addBody(block);
// Define contraints on the model
// Add a point on line constraint to limit the block to vertical motion
Vec3 lineDirection(0,1,0), pointOnLine(0,0,0), pointOnBlock(0);
PointOnLineConstraint *lineConstraint = new PointOnLineConstraint(ground, lineDirection, pointOnLine, *block, pointOnBlock);
osimModel.addConstraint(lineConstraint);
// Add PistonActuator between the first linkage and the block
Vec3 pointOnBodies(0);
PistonActuator *piston = new PistonActuator();
piston->setName("piston");
piston->setBodyA(linkage1);
piston->setBodyB(block);
piston->setPointA(pointOnBodies);
piston->setPointB(pointOnBodies);
piston->setOptimalForce(200.0);
piston->setPointsAreGlobal(false);
osimModel.addForce(piston);
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Added ControllableSpring between the first linkage and the second block
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
ControllableSpring *spring = new ControllableSpring;
spring->setName("spring");
spring->setBodyA(block);
spring->setBodyB(linkage1);
spring->setPointA(pointOnBodies);
spring->setPointB(pointOnBodies);
spring->setOptimalForce(2000.0);
spring->setPointsAreGlobal(false);
spring->setRestLength(0.8);
osimModel.addForce(spring);
// define the simulation times
double t0(0.0), tf(15);
// create a controller to control the piston and spring actuators
// the prescribed controller sets the controls as functions of time
PrescribedController *legController = new PrescribedController();
// give the legController control over all (two) model actuators
legController->setActuators(osimModel.updActuators());
// specify some control nodes for spring stiffness control
double t[] = {0.0, 4.0, 7.0, 10.0, 15.0};
double x[] = {1.0, 1.0, 0.25, 0.25, 5.0};
// specify the control function for each actuator
legController->prescribeControlForActuator("piston", new Constant(0.1));
legController->prescribeControlForActuator("spring", new PiecewiseLinearFunction(5, t, x));
// add the controller to the model
osimModel.addController(legController);
// define the acceration due to gravity
osimModel.setGravity(Vec3(0, -9.80665, 0));
// enable the model visualizer see the model in action, which can be
// useful for debugging
osimModel.setUseVisualizer(true);
// Initialize system
SimTK::State& si = osimModel.initSystem();
// Pin joint initial states
double q1_i = -Pi/4;
double q2_i = - 2*q1_i;
CoordinateSet &coordinates = osimModel.updCoordinateSet();
coordinates[0].setValue(si, q1_i, true);
coordinates[1].setValue(si,q2_i, true);
// Setup integrator and manager
SimTK::RungeKuttaMersonIntegrator integrator(osimModel.getMultibodySystem());
integrator.setAccuracy(1.0e-3);
ForceReporter *forces = new ForceReporter(&osimModel);
osimModel.updAnalysisSet().adoptAndAppend(forces);
Manager manager(osimModel, integrator);
//Examine the model
osimModel.printDetailedInfo(si, std::cout);
// Save the model
osimModel.print("toyLeg.osim");
// Print out the initial position and velocity states
si.getQ().dump("Initial q's");
si.getU().dump("Initial u's");
std::cout << "Initial time: " << si.getTime() << std::endl;
// Integrate
manager.setInitialTime(t0);
manager.setFinalTime(tf);
std::cout<<"\n\nIntegrating from " << t0 << " to " << tf << std::endl;
manager.integrate(si);
// Save results
osimModel.printControlStorage("SpringActuatedLeg_controls.sto");
Storage statesDegrees(manager.getStateStorage());
osimModel.updSimbodyEngine().convertRadiansToDegrees(statesDegrees);
//statesDegrees.print("PistonActuatedLeg_states_degrees.mot");
statesDegrees.print("SpringActuatedLeg_states_degrees.mot");
forces->getForceStorage().print("actuator_forces.mot");
}
catch (const std::exception& ex)
{
std::cout << "Exception in toyLeg_example: " << ex.what() << std::endl;
return 1;
}
std::cout << "Exiting" << std::endl;
return 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;
};
void KaneScherFig2Modeling::addBaseJointsAndBodies()
{
// NOTE: Massless bodies are simply coordinate frames for rotation. We
// ---- have attempted to be as faithful to Kane and Scher (1969) as
// possible. See their paper to understand the notation used.
// BODY CONNECTIONS: ground -> anterior -> K -> P -> B2 -> posterior
//
// JOINTS/COORDINATES: custom (u_integ) -> pin (alpha) -> pin (theta) ->
// pin (beta) -> pin (v_integ)
// Connecting the anterior body (A) to the ground via a custom joint (same
// as in 'model_dembiasketch'). Rotation is defined via ZXY Euler angles,
// named flip, u_integ (AKA. sprawl), and roll respectively. The important
// translation is in Y, the direction of gravity.
Body& ground = _cat.getGroundBody();
SpatialTransform groundAnteriorST;
groundAnteriorST.updTransformAxis(0).setCoordinateNames(
Array<string>("anterior_flip", 1));
groundAnteriorST.updTransformAxis(0).setAxis(Vec3(0, 0, 1));
groundAnteriorST.updTransformAxis(1).setCoordinateNames(
Array<string>("kane_u_integ", 1));
groundAnteriorST.updTransformAxis(1).setAxis(Vec3(1, 0, 0));
groundAnteriorST.updTransformAxis(2).setCoordinateNames(
Array<string>("anterior_roll", 1));
groundAnteriorST.updTransformAxis(2).setAxis(Vec3(0, 1, 0));
groundAnteriorST.updTransformAxis(3).setCoordinateNames(
Array<string>("anterior_tx", 1));
groundAnteriorST.updTransformAxis(3).setAxis(Vec3(1, 0, 0));
groundAnteriorST.updTransformAxis(4).setCoordinateNames(
Array<string>("anterior_ty", 1));
groundAnteriorST.updTransformAxis(4).setAxis(Vec3(0, 1, 0));
groundAnteriorST.updTransformAxis(5).setCoordinateNames(
Array<string>("anterior_tz", 1));
groundAnteriorST.updTransformAxis(5).setAxis(Vec3(0, 0, 1));
Vec3 locGAInGround(0);
Vec3 orientGAInGround(0);
Vec3 locGAInAnterior(0);
Vec3 orientGAInAnterior(Pi, 0, 0);
CustomJoint* groundAnterior = new CustomJoint("ground_anterior",
ground, locGAInGround, orientGAInGround,
*_anteriorBody, locGAInAnterior, orientGAInAnterior,
groundAnteriorST);
CoordinateSet & groundAnteriorCS = groundAnterior->upd_CoordinateSet();
// flip
double groundAnteriorCS0range[2] = {-Pi, Pi};
groundAnteriorCS[0].setRange(groundAnteriorCS0range);
groundAnteriorCS[0].setDefaultValue(0);
groundAnteriorCS[0].setDefaultLocked(true);
// u_integ (sprawl)
double groundAnteriorCS1range[2] = {-Pi, Pi};
groundAnteriorCS[1].setRange(groundAnteriorCS1range);
groundAnteriorCS[1].setDefaultValue(0);
groundAnteriorCS[1].setDefaultLocked(false);
// roll
double groundAnteriorCS2range[2] = {-3 * Pi, 3 * Pi};
groundAnteriorCS[2].setRange(groundAnteriorCS2range);
groundAnteriorCS[2].setDefaultValue(0);
groundAnteriorCS[2].setDefaultLocked(true);
// tx
double groundAnteriorCS3range[2] = {-10, 10};
groundAnteriorCS[3].setRange(groundAnteriorCS3range);
groundAnteriorCS[3].setDefaultValue(0);
groundAnteriorCS[3].setDefaultLocked(true);
// ty
double groundAnteriorCS4range[2] = {-1, 100};
groundAnteriorCS[4].setRange(groundAnteriorCS4range);
groundAnteriorCS[4].setDefaultValue(0);
groundAnteriorCS[4].setDefaultLocked(false);
// tz
double groundAnteriorCS5range[2] = {-5, 5};
groundAnteriorCS[5].setRange(groundAnteriorCS5range);
groundAnteriorCS[5].setDefaultValue(0);
groundAnteriorCS[5].setDefaultLocked(true);
// Frame in which the X-axis points along ray K. K lies in the A1-A2
// plane, so frame K is formed by rotation (by angle alpha) about A3.
Body* frameK = newMasslessBody("K");
Vec3 locationAKInAnterior(0);
Vec3 orientationAKInAnterior(0);
Vec3 locationAKinFrameK(0);
Vec3 orientationAKinFrameK(0);
PinJoint* anteriorFrameK = new PinJoint("anterior_K",
*_anteriorBody, locationAKInAnterior, orientationAKInAnterior,
*frameK, locationAKinFrameK, orientationAKinFrameK, false);
CoordinateSet& anteriorFrameKCS = anteriorFrameK->upd_CoordinateSet();
anteriorFrameKCS[0].setName("kane_alpha");
//double anteriorFrameKCS0range[2] = {convertDegreesToRadians(35),
// convertDegreesToRadians(85)};
double anteriorFrameKCS0range[2] = {-Pi, 0};
anteriorFrameKCS[0].setRange(anteriorFrameKCS0range);
//anteriorFrameKCS[0].setDefaultValue(convertDegreesToRadians(60));
anteriorFrameKCS[0].setDefaultValue(0);
anteriorFrameKCS[0].setDefaultLocked(true);
// Defining the following axes:
// B1: X-axis of coordinate frame attached to the posterior body
// B2: normal to B1 and lying in the plane defined by B1 and ray K
// B3: mutually perpendicular to B1 and B2
// Frame with Z-axis as B3 (named P in Kane and Scher), formed by rotation
// about K by angle theta.
Body* frameP = newMasslessBody("P");
Vec3 locationKPInFrameK(0);
Vec3 orientationKPInFrameK(0, 0.5 * Pi, 0);
Vec3 locationKPInFrameP(0);
Vec3 orientationKPInFrameP(0, -0.5 * Pi, 0);
PinJoint* FrameKFrameP = new PinJoint("K_P",
*frameK, locationKPInFrameK, orientationKPInFrameK,
*frameP, locationKPInFrameP, orientationKPInFrameP, false);
CoordinateSet& FrameKFramePCS = FrameKFrameP->upd_CoordinateSet();
FrameKFramePCS[0].setName("kane_theta");
double FrameKFramePCS0range[2] = {0, 2 * Pi};
FrameKFramePCS[0].setRange(FrameKFramePCS0range);
FrameKFramePCS[0].setDefaultValue(0);
FrameKFramePCS[0].setDefaultLocked(false);
// Frame with Y-axis as B2, formed by rotation about B3 by angle beta.
Body* frameB2 = newMasslessBody("B2");
Vec3 locationPB2InFrameB3(0);
Vec3 orientationPB2InFrameB3(0);
Vec3 locationPB2InFrameB2(0);
Vec3 orientationPB2InFrameB2(0);
PinJoint* FramePFrameB2 = new PinJoint("P_B2",
*frameP, locationPB2InFrameB3, orientationPB2InFrameB3,
*frameB2, locationPB2InFrameB2, orientationPB2InFrameB2, false);
CoordinateSet& FramePFrameB2CS = FramePFrameB2->upd_CoordinateSet();
FramePFrameB2CS[0].setName("kane_beta");
//double FramePFrameB2CS0range[2] = {convertDegreesToRadians(60),
// convertDegreesToRadians(90)};
double FramePFrameB2CS0range[2] = {0, Pi};
FramePFrameB2CS[0].setRange(FramePFrameB2CS0range);
//FramePFrameB2CS[0].setDefaultValue(convertDegreesToRadians(75));
FramePFrameB2CS[0].setDefaultValue(0);
FramePFrameB2CS[0].setDefaultLocked(true);
// Connecting the posterior body (i.e., B1).
Vec3 locationB2PostInFrameB2(0);
Vec3 orientationB2PostInFrameB2(0, 0, -0.5 * Pi);
Vec3 locationB2PostInPosterior(0);
Vec3 orientationB2PostInPosterior(Pi, 0, 0.5 * Pi);
PinJoint* FrameB2Posterior = new PinJoint("B2_posterior",
*frameB2, locationB2PostInFrameB2, orientationB2PostInFrameB2,
*_posteriorBody, locationB2PostInPosterior, orientationB2PostInPosterior, false);
CoordinateSet& FrameB2PosteriorCS = FrameB2Posterior->upd_CoordinateSet();
FrameB2PosteriorCS[0].setName("kane_v_integ");
double FrameB2PosteriorCS0range[2] = {-Pi, Pi};
FrameB2PosteriorCS[0].setRange(FrameB2PosteriorCS0range);
FrameB2PosteriorCS[0].setDefaultValue(0);
FrameB2PosteriorCS[0].setDefaultLocked(false);
// TODO: add N, define gamma?
// -- Add bodies.
_cat.addBody(_anteriorBody);
_cat.addBody(frameK);
_cat.addBody(frameP);
_cat.addBody(frameB2);
_cat.addBody(_posteriorBody);
//cat.addBody(frameN);
// -- Add "no-twist" constraint. (TODO: is this right? don't we want to
// constrain speeds?)
CoordinateCouplerConstraint * twistConstr = new CoordinateCouplerConstraint();
twistConstr->setName("twist");
twistConstr->setIndependentCoordinateNames(Array<string>("kane_u_integ", 1));
twistConstr->setDependentCoordinateName("kane_v_integ");
Array<double> twistConstrFcnCoeff;
twistConstrFcnCoeff.append(1);
twistConstr->setFunction(new LinearFunction(twistConstrFcnCoeff));
_cat.addConstraint(twistConstr);
}
int main() {
Model model;
model.setName("bicep_curl");
#ifdef VISUALIZE
model.setUseVisualizer(true);
#endif
// Create two links, each with a mass of 1 kg, center of mass at the body's
// origin, and moments and products of inertia of zero.
OpenSim::Body* humerus = new OpenSim::Body("humerus", 1, Vec3(0), Inertia(0));
OpenSim::Body* radius = new OpenSim::Body("radius", 1, Vec3(0), Inertia(0));
// Connect the bodies with pin joints. Assume each body is 1 m long.
PinJoint* shoulder = new PinJoint("shoulder",
// Parent body, location in parent, orientation in parent.
model.getGround(), Vec3(0), Vec3(0),
// Child body, location in child, orientation in child.
*humerus, Vec3(0, 1, 0), Vec3(0));
PinJoint* elbow = new PinJoint("elbow",
*humerus, Vec3(0), Vec3(0), *radius, Vec3(0, 1, 0), Vec3(0));
// Add a muscle that flexes the elbow.
Millard2012EquilibriumMuscle* biceps = new
Millard2012EquilibriumMuscle("biceps", 200, 0.6, 0.55, 0);
biceps->addNewPathPoint("origin", *humerus, Vec3(0, 0.8, 0));
biceps->addNewPathPoint("insertion", *radius, Vec3(0, 0.7, 0));
// Add a controller that specifies the excitation of the muscle.
PrescribedController* brain = new PrescribedController();
brain->addActuator(*biceps);
// Muscle excitation is 0.3 for the first 0.5 seconds, then increases to 1.
brain->prescribeControlForActuator("biceps",
new StepFunction(0.5, 3, 0.3, 1));
// Add components to the model.
model.addBody(humerus); model.addBody(radius);
model.addJoint(shoulder); model.addJoint(elbow);
model.addForce(biceps);
model.addController(brain);
// Add a console reporter to print the muscle fiber force and elbow angle.
ConsoleReporter* reporter = new ConsoleReporter();
reporter->set_report_time_interval(1.0);
reporter->addToReport(biceps->getOutput("fiber_force"));
reporter->addToReport(
elbow->getCoordinate(PinJoint::Coord::RotationZ).getOutput("value"),
"elbow_angle");
model.addComponent(reporter);
// Add display geometry.
Ellipsoid bodyGeometry(0.1, 0.5, 0.1);
bodyGeometry.setColor(Gray);
// Attach an ellipsoid to a frame located at the center of each body.
PhysicalOffsetFrame* humerusCenter = new PhysicalOffsetFrame(
"humerusCenter", *humerus, Transform(Vec3(0, 0.5, 0)));
humerus->addComponent(humerusCenter);
humerusCenter->attachGeometry(bodyGeometry.clone());
PhysicalOffsetFrame* radiusCenter = new PhysicalOffsetFrame(
"radiusCenter", *radius, Transform(Vec3(0, 0.5, 0)));
radius->addComponent(radiusCenter);
radiusCenter->attachGeometry(bodyGeometry.clone());
// Configure the model.
State& state = model.initSystem();
// Fix the shoulder at its default angle and begin with the elbow flexed.
shoulder->getCoordinate().setLocked(state, true);
elbow->getCoordinate().setValue(state, 0.5 * Pi);
model.equilibrateMuscles(state);
// Configure the visualizer.
#ifdef VISUALIZE
model.updMatterSubsystem().setShowDefaultGeometry(true);
Visualizer& viz = model.updVisualizer().updSimbodyVisualizer();
viz.setBackgroundType(viz.SolidColor);
viz.setBackgroundColor(White);
#endif
// Simulate.
simulate(model, state, 10.0);
return 0;
};
