//pass in the amount of time you want to simulate
//in real time, this is the delta time between called to update
//in simulation time, this can be any amount -- but this function won't return until all updates are processed
//so it holds up the thread
int IESoRWorld::updateWorld(double msUpdate)
{
msUpdate = 4*msUpdate;
//we'll keep track of the number of times we simulated during this update
int stepCount = 0;
//# of seconds since last time
double frameTime = msUpdate/1000.0f;
//maximum frame time, to prevent what is called the spiral of death
if (frameTime > .35)
frameTime = .35;
//we accumulate all the time we haven't rendered things in
this->accumulator += frameTime;
//as long as we have a chunk of time to simulate, we need to do the simulation
while (accumulator >= simulationRate)
{
//how many times did we run this loop, we shoudl keep track --
//that's the number of times we ran the physics engine
stepCount++;
//move the muscles quicker using this toggle
float speedup = 3;
//we loop through all our muscles, and update the lengths associated with the connectionsj
for (std::vector<Muscle*>::iterator it = muscleList.begin() ; it != muscleList.end(); ++it)
{
//grab our muscle pointer from the list
Muscle* muscle = *it;
//get our distance joint -- holder of physical joint info
b2DistanceJoint* dJoint = (b2DistanceJoint*)muscle->GetJoint();
//Javascript version
//muscle.SetLength(muscle.m_length + muscle.amplitude/this.scale*Math.cos(rad + muscle.phase*2*Math.PI));
//double lengthCalc = (dJoint->GetLength() + muscle->GetAmplitude()*cos(radians + muscle->GetPhase() * 2 * M_PI));
//fetch the original length of the distance joint, and add some fraction of that amount to the length
//depending on the current location in the muscle cycle
double lengthCalc = (1.0 + muscle->GetAmplitude() * cos(radians + muscle->GetPhase() * 2 * M_PI)) * muscle->GetOriginalLength();
//we set our length as the calculate value
dJoint->SetLength(lengthCalc);
}
//step the physics world
this->world->Step(
this->simulationRate //frame-rate
, 10 //velocity iterations
, 10 //position iterations
);
//manually clear forces when doing fixed time steps,
//NOTE: that we disabled auto clear after step inside of the b2World
this->world->ClearForces();
//increment the radians for the muscles
radians += speedup * this->simulationRate;
//decrement the accumulator - we ran a chunk just now!
accumulator -= this->simulationRate;
}
//interpolation is basically a measure of how far into the next step we should have simulated
//it's meant to ease the drawing stages
//(you linearly interpolate the last frame and the current frame using this value)
this->interpolation = accumulator / this->simulationRate;
//how many times did we run the simulator?
return stepCount;
}
/**
* Allocate storage for the muscle variables.
*/
void MuscleAnalysis::allocateStorageObjects()
{
if(_model==NULL) return;
// CLEAR EXISTING WORK ARRAYS
_storageList.setMemoryOwner(true);
_storageList.setSize(0);
_momentArmStorageArray.setMemoryOwner(true);
_momentArmStorageArray.setSize(0);
_muscleArray.setMemoryOwner(false);
_muscleArray.setSize(0);
// FOR MOMENT ARMS AND MOMENTS
if(_computeMoments) {
const CoordinateSet& qSet = _model->getCoordinateSet();
_coordinateList = _coordinateListProp.getValueStrArray();
if(IO::Lowercase(_coordinateList[0]) == "all"){
_coordinateList.setSize(0);
for(int i=0; i < qSet.getSize(); ++i) {
_coordinateList.append(qSet[i].getName());
}
}
else{
int i=0;
while(i <_coordinateList.getSize()){
int found = qSet.getIndex(_coordinateList[i]);
if(found < 0){
cout << "MuscleAnalysis: WARNING - coordinate ";
cout << _coordinateList[i] << " is not part of model." << endl;
_coordinateList.remove(i);
}
else{
++i;
}
}
}
int nq = _coordinateList.getSize();
Storage* store;
for(int i=0;i<nq;i++) {
string name = "MomentArm_" + _coordinateList[i];
store = new Storage(1000,name);
store->setDescription(getDescription());
_storageList.append(store);
}
for(int i=0;i<nq;i++) {
string name = "Moment_" + _coordinateList[i];
store = new Storage(1000,name);
store->setDescription(getDescription());
_storageList.append(store);
}
// POPULATE ACTIVE MOMENT ARM ARRAY
_momentArmStorageArray.setSize(0);
for(int i=0; i<nq; i++) {
int found = qSet.getIndex(_coordinateList[i]);
if(found >=0){
StorageCoordinatePair *pair = new StorageCoordinatePair();
pair->q = &qSet[found];
pair->momentArmStore = _storageList[i];
pair->momentStore = _storageList[i+nq];
_momentArmStorageArray.append(pair);
}
}
}
// EVERYTHING ELSE
//_storageList.setMemoryOwner(false);
_pennationAngleStore = new Storage(1000,"PennationAngle");
_pennationAngleStore->setDescription(getDescription());
_storageList.append(_pennationAngleStore );
_lengthStore = new Storage(1000,"Length");
_lengthStore->setDescription(getDescription());
_storageList.append(_lengthStore );
_fiberLengthStore = new Storage(1000,"FiberLength");
_fiberLengthStore->setDescription(getDescription());
_storageList.append(_fiberLengthStore );
_normalizedFiberLengthStore = new Storage(1000,"NormalizedFiberLength");
_normalizedFiberLengthStore->setDescription(getDescription());
_storageList.append(_normalizedFiberLengthStore );
_tendonLengthStore = new Storage(1000,"TendonLength");
_tendonLengthStore->setDescription(getDescription());
_storageList.append(_tendonLengthStore );
_fiberVelocityStore = new Storage(1000,"FiberVelocity");
_fiberVelocityStore->setDescription(getDescription());
_storageList.append(_fiberVelocityStore );
_normFiberVelocityStore = new Storage(1000,"NormFiberVelocity");
_normFiberVelocityStore->setDescription(getDescription());
_storageList.append(_normFiberVelocityStore );
_pennationAngularVelocityStore = new Storage(1000,
"PennationAngularVelocity");
_pennationAngularVelocityStore->setDescription(getDescription());
_storageList.append(_pennationAngularVelocityStore );
_forceStore = new Storage(1000,"TendonForce");
_forceStore->setDescription(getDescription());
_storageList.append(_forceStore );
_fiberForceStore = new Storage(1000,"FiberForce");
_fiberForceStore->setDescription(getDescription());
_storageList.append(_fiberForceStore );
_activeFiberForceStore = new Storage(1000,"ActiveFiberForce");
_activeFiberForceStore->setDescription(getDescription());
_storageList.append(_activeFiberForceStore );
_passiveFiberForceStore = new Storage(1000,"PassiveFiberForce");
_passiveFiberForceStore->setDescription(getDescription());
_storageList.append(_passiveFiberForceStore );
_activeFiberForceAlongTendonStore = new Storage(1000,
"ActiveFiberForceAlongTendon");
_activeFiberForceAlongTendonStore->setDescription(getDescription());
_storageList.append(_activeFiberForceAlongTendonStore );
_passiveFiberForceAlongTendonStore = new Storage(1000,
"PassiveFiberForceAlongTendon");
_passiveFiberForceAlongTendonStore->setDescription(getDescription());
_storageList.append(_passiveFiberForceAlongTendonStore );
_fiberActivePowerStore = new Storage(1000,"FiberActivePower");
_fiberActivePowerStore->setDescription(getDescription());
_storageList.append(_fiberActivePowerStore );
_fiberPassivePowerStore = new Storage(1000,"FiberPassivePower");
_fiberPassivePowerStore->setDescription(getDescription());
_storageList.append(_fiberPassivePowerStore );
_tendonPowerStore = new Storage(1000,"TendonPower");
_tendonPowerStore->setDescription(getDescription());
_storageList.append(_tendonPowerStore );
_musclePowerStore = new Storage(1000,"MuscleActuatorPower");
_musclePowerStore->setDescription(getDescription());
_storageList.append(_musclePowerStore );
// POPULATE MUSCLE LIST FOR "all"
ForceSet& fSet = _model->updForceSet();
_muscleList = _muscleListProp.getValueStrArray();
int nm = _muscleList.getSize();
if((nm==1) && (_muscleList.get(0)=="all")) {
_muscleList.setSize(0);
int nf = fSet.getSize();
for(int i=0;i<nf;i++) {
Muscle *m = dynamic_cast<Muscle*>(&fSet.get(i));
if( m ) _muscleList.append(m->getName());
}
}
// POPULATE ACTIVE MUSCLE ARRAY
Array<string> tmpMuscleList("");
nm = _muscleList.getSize();
_muscleArray.setSize(0);
for(int i=0; i<nm; i++) {
if(fSet.contains(_muscleList[i])) {
Muscle* mus = dynamic_cast<Muscle*>( &fSet.get(_muscleList[i]) );
if(mus){
_muscleArray.append(mus);
tmpMuscleList.append(mus->getName());
}
}
}
_muscleList = tmpMuscleList;
// CONSTRUCT AND SET COLUMN LABELS
constructColumnLabels();
int size = _storageList.getSize();
Storage* store;
for(int i=0;i<size;i++) {
store = _storageList[i];
if(store==NULL) continue;
store->setColumnLabels(getColumnLabels());
}
}
bool SimbodySimmModel::writeMuscle(Muscle& aMuscle, const ForceSet& aActuatorSet, ofstream& aStream)
{
aStream << "beginmuscle " << aMuscle.getName() << endl;
const PathPointSet& pts = aMuscle.getGeometryPath().getPathPointSet();
aStream << "beginpoints" << endl;
for (int i = 0; i < pts.getSize(); i++)
{
PathPoint& pt = pts.get(i);
if (pt.getConcreteClassName()==("ConditionalPathPoint")) {
ConditionalPathPoint* mvp = (ConditionalPathPoint*)(&pt);
Vec3& attachment = mvp->getLocation();
double range[]{ mvp->get_range(0), mvp->get_range(1) };
aStream << attachment[0] << " " << attachment[1] << " " << attachment[2] << " segment " << mvp->getBody().getName();
if (mvp->hasCoordinate()) {
const Coordinate& coord = mvp->getCoordinate();
if (coord.getMotionType() == Coordinate::Rotational)
aStream << " ranges 1 " << coord.getName() << " (" << range[0] * SimTK_RADIAN_TO_DEGREE << ", " << range[1] * SimTK_RADIAN_TO_DEGREE << ")" << endl;
else
aStream << " ranges 1 " << coord.getName() << " (" << range[0] << ", " << range[1] << ")" << endl;
} else {
aStream << " ranges 1 "
<< mvp->getConnector<Coordinate>("coordinate").getConnecteeName()
<< " (0.0, 1.0)" << endl;
}
} else if (pt.getConcreteClassName()==("MovingPathPoint")) {
MovingPathPoint* mpp = (MovingPathPoint*)(&pt);
const Vec3& attachment = mpp->get_location();
if (mpp->hasXCoordinate()) {
aStream << "f" << addMuscleFunction(&mpp->get_x_location(), mpp->getXCoordinate().getMotionType(), Coordinate::Translational) << "(" << mpp->getXCoordinate().getName() << ") ";
} else {
aStream << attachment[0] << " ";
}
if (mpp->hasYCoordinate()) {
aStream << "f" << addMuscleFunction(&mpp->get_y_location(), mpp->getYCoordinate().getMotionType(), Coordinate::Translational) << "(" << mpp->getYCoordinate().getName() << ") ";
} else {
aStream << attachment[1] << " ";
}
if (mpp->hasZCoordinate()) {
aStream << "f" << addMuscleFunction(&mpp->get_z_location(), mpp->getZCoordinate().getMotionType(), Coordinate::Translational) << "(" << mpp->getZCoordinate().getName() << ")";
} else {
aStream << attachment[2];
}
aStream << " segment " << mpp->getBody().getName() << endl;
} else {
Vec3& attachment = pt.getLocation();
aStream << attachment[0] << " " << attachment[1] << " " << attachment[2] << " segment " << pt.getBody().getName() << endl;
}
}
aStream << "endpoints" << endl;
Array<std::string> groupNames;
aActuatorSet.getGroupNamesContaining(aMuscle.getName(),groupNames);
if(groupNames.getSize()) {
aStream << "begingroups" << endl;
for(int i=0; i<groupNames.getSize(); i++)
aStream << " " << groupNames[i];
aStream << endl << "endgroups" << endl;
}
const PathWrapSet& wrapObjects = aMuscle.getGeometryPath().getWrapSet();
for (int i=0; i<wrapObjects.getSize(); i++)
aStream << "wrapobject " << wrapObjects.get(i).getWrapObjectName() << " " <<
(dynamic_cast<WrapEllipsoid*>(&wrapObjects.get(i)) ? (wrapObjects.get(i).getMethodName()+" ") : "") <<
"range " << wrapObjects.get(i).getStartPoint() << " " << wrapObjects.get(i).getEndPoint() << endl;
if (dynamic_cast<Schutte1993Muscle_Deprecated*>(&aMuscle))
{
Schutte1993Muscle_Deprecated *szh = dynamic_cast<Schutte1993Muscle_Deprecated*>(&aMuscle);
aStream << "max_force " << szh->getMaxIsometricForce() << endl;
aStream << "optimal_fiber_length " << szh->getOptimalFiberLength() << endl;
aStream << "tendon_slack_length " << szh->getTendonSlackLength() << endl;
aStream << "pennation_angle " << szh->getPennationAngleAtOptimalFiberLength() * SimTK_RADIAN_TO_DEGREE << endl;
aStream << "max_contraction_velocity " << szh->getMaxContractionVelocity() << endl;
aStream << "timescale " << szh->getTimeScale() << endl;
aStream << "muscle_model 4" << endl;
aStream << "active_force_length_curve f" << addMuscleFunction(&szh->getActiveForceLengthCurve(), Coordinate::Translational, Coordinate::Translational) << endl;
aStream << "passive_force_length_curve f" << addMuscleFunction(&szh->getPassiveForceLengthCurve(), Coordinate::Translational, Coordinate::Translational) << endl;
aStream << "tendon_force_length_curve f" << addMuscleFunction(&szh->getTendonForceLengthCurve(), Coordinate::Translational, Coordinate::Translational) << endl;
}
else if (dynamic_cast<Thelen2003Muscle_Deprecated*>(&aMuscle))
{
Thelen2003Muscle_Deprecated *sdm = dynamic_cast<Thelen2003Muscle_Deprecated*>(&aMuscle);
aStream << "max_force " << sdm->getMaxIsometricForce() << endl;
aStream << "optimal_fiber_length " << sdm->getOptimalFiberLength() << endl;
aStream << "tendon_slack_length " << sdm->getTendonSlackLength() << endl;
aStream << "pennation_angle " << sdm->getPennationAngleAtOptimalFiberLength() * SimTK_RADIAN_TO_DEGREE << endl;
aStream << "activation_time_constant " << sdm->getActivationTimeConstant() << endl;
aStream << "deactivation_time_constant " << sdm->getDeactivationTimeConstant() << endl;
aStream << "Vmax " << sdm->getVmax() << endl;
aStream << "Vmax0 " << sdm->getVmax0() << endl;
aStream << "FmaxTendonStrain " << sdm->getFmaxTendonStrain() << endl;
aStream << "FmaxMuscleStrain " << sdm->getFmaxMuscleStrain() << endl;
aStream << "KshapeActive " << sdm->getKshapeActive() << endl;
aStream << "KshapePassive " << sdm->getKshapePassive() << endl;
aStream << "damping " << sdm->getDamping() << endl;
aStream << "Af " << sdm->getAf() << endl;
aStream << "Flen " << sdm->getFlen() << endl;
aStream << "muscle_model 9" << endl;
}
else if (dynamic_cast<Delp1990Muscle_Deprecated*>(&aMuscle))
{
Delp1990Muscle_Deprecated *szh = dynamic_cast<Delp1990Muscle_Deprecated*>(&aMuscle);
aStream << "max_force " << szh->getMaxIsometricForce() << endl;
aStream << "optimal_fiber_length " << szh->getOptimalFiberLength() << endl;
aStream << "tendon_slack_length " << szh->getTendonSlackLength() << endl;
aStream << "pennation_angle " << szh->getPennationAngleAtOptimalFiberLength() * SimTK_RADIAN_TO_DEGREE << endl;
aStream << "max_contraction_velocity " << szh->getMaxContractionVelocity() << endl;
aStream << "timescale " << szh->getTimeScale() << endl;
aStream << "muscle_model 2" << endl;
if (szh->getActiveForceLengthCurve())
aStream << "active_force_length_curve f" << addMuscleFunction(szh->getActiveForceLengthCurve(), Coordinate::Translational, Coordinate::Translational) << endl;
if (szh->getPassiveForceLengthCurve())
aStream << "passive_force_length_curve f" << addMuscleFunction(szh->getPassiveForceLengthCurve(), Coordinate::Translational, Coordinate::Translational) << endl;
if (szh->getTendonForceLengthCurve())
aStream << "tendon_force_length_curve f" << addMuscleFunction(szh->getTendonForceLengthCurve(), Coordinate::Translational, Coordinate::Translational) << endl;
if (szh->getForceVelocityCurve())
aStream << "force_velocity_curve f" << addMuscleFunction(szh->getForceVelocityCurve(), Coordinate::Translational, Coordinate::Translational) << endl;
}
aStream << "endmuscle" << endl << endl;
return true;
}
void OpenSimContext::copyMuscle(Muscle& from, Muscle& to) {
to = from;
_configState->invalidateAll(SimTK::Stage::Position);
_model->getMultibodySystem().realize(*_configState, SimTK::Stage::Velocity);
to.updGeometryPath().updateGeometry(*_configState);
}
/*==============================================================================
Main test driver to be used on any muscle model (derived from Muscle) so new
cases should be easy to add currently, the test only verifies that the work
done by the muscle corresponds to the change in system energy.
TODO: Test will fail wih prescribe motion until the work done by this
constraint is accounted for.
================================================================================
*/
void simulateMuscle(
const Muscle &aMuscModel,
double startX,
double act0,
const Function *motion, // prescribe motion of free end of muscle
const Function *control, // prescribed excitation signal to the muscle
double integrationAccuracy,
int testType,
double testTolerance,
bool printResults)
{
string prescribed = (motion == NULL) ? "." : " with Prescribed Motion.";
cout << "\n******************************************************" << endl;
cout << "Test " << aMuscModel.getConcreteClassName()
<< " Model" << prescribed << endl;
cout << "******************************************************" << endl;
using SimTK::Vec3;
//==========================================================================
// 0. SIMULATION SETUP: Create the block and ground
//==========================================================================
// Define the initial and final simulation times
double initialTime = 0.0;
double finalTime = 4.0;
//Physical properties of the model
double ballMass = 10;
double ballRadius = 0.05;
double anchorWidth = 0.1;
// Create an OpenSim model
Model model;
double optimalFiberLength = aMuscModel.getOptimalFiberLength();
double pennationAngle = aMuscModel.getPennationAngleAtOptimalFiberLength();
double tendonSlackLength = aMuscModel.getTendonSlackLength();
// Use a copy of the muscle model passed in to add path points later
PathActuator *aMuscle = aMuscModel.clone();
// Get a reference to the model's ground body
Ground& ground = model.updGround();
ground.addDisplayGeometry("box.vtp");
ground.updDisplayer()
->setScaleFactors(Vec3(anchorWidth, anchorWidth, 2 * anchorWidth));
OpenSim::Body * ball = new OpenSim::Body("ball",
ballMass,
Vec3(0),
ballMass*SimTK::Inertia::sphere(ballRadius));
ball->addDisplayGeometry("sphere.vtp");
ball->updDisplayer()->setScaleFactors(Vec3(2 * ballRadius));
// ball connected to ground via a slider along X
double xSinG = optimalFiberLength*cos(pennationAngle) + tendonSlackLength;
SliderJoint* slider = new SliderJoint("slider",
ground,
Vec3(anchorWidth / 2 + xSinG, 0, 0),
Vec3(0),
*ball,
Vec3(0),
Vec3(0));
CoordinateSet& jointCoordinateSet = slider->upd_CoordinateSet();
jointCoordinateSet[0].setName("tx");
jointCoordinateSet[0].setDefaultValue(1.0);
jointCoordinateSet[0].setRangeMin(0);
jointCoordinateSet[0].setRangeMax(1.0);
if (motion != NULL){
jointCoordinateSet[0].setPrescribedFunction(*motion);
jointCoordinateSet[0].setDefaultIsPrescribed(true);
}
// add ball to model
model.addBody(ball);
model.addJoint(slider);
//==========================================================================
// 1. SIMULATION SETUP: Add the muscle
//==========================================================================
//Attach the muscle
const string &actuatorType = aMuscle->getConcreteClassName();
aMuscle->setName("muscle");
aMuscle->addNewPathPoint("muscle-box", ground, Vec3(anchorWidth / 2, 0, 0));
aMuscle->addNewPathPoint("muscle-ball", *ball, Vec3(-ballRadius, 0, 0));
ActivationFiberLengthMuscle_Deprecated *aflMuscle
= dynamic_cast<ActivationFiberLengthMuscle_Deprecated *>(aMuscle);
if (aflMuscle){
// Define the default states for the muscle that has
//activation and fiber-length states
aflMuscle->setDefaultActivation(act0);
aflMuscle->setDefaultFiberLength(aflMuscle->getOptimalFiberLength());
}
else{
ActivationFiberLengthMuscle *aflMuscle2
= dynamic_cast<ActivationFiberLengthMuscle *>(aMuscle);
if (aflMuscle2){
// Define the default states for the muscle
//that has activation and fiber-length states
aflMuscle2->setDefaultActivation(act0);
aflMuscle2->setDefaultFiberLength(aflMuscle2
->getOptimalFiberLength());
}
}
model.addForce(aMuscle);
// Create a prescribed controller that simply
//applies controls as function of time
PrescribedController * muscleController = new PrescribedController();
if (control != NULL){
muscleController->setActuators(model.updActuators());
// Set the indiviudal muscle control functions
//for the prescribed muscle controller
muscleController->prescribeControlForActuator("muscle", control->clone());
// Add the control set controller to the model
model.addController(muscleController);
}
// Set names for muscles / joints.
Array<string> muscNames;
muscNames.append(aMuscle->getName());
Array<string> jointNames;
jointNames.append("slider");
//==========================================================================
// 2. SIMULATION SETUP: Instrument the test with probes
//==========================================================================
Array<string> muscNamesTwice = muscNames;
muscNamesTwice.append(muscNames.get(0));
cout << "------------\nPROBES\n------------" << endl;
int probeCounter = 1;
// Add ActuatorPowerProbe to measure work done by the muscle
ActuatorPowerProbe* muscWorkProbe = new ActuatorPowerProbe(muscNames, false, 1);
//muscWorkProbe->setName("ActuatorWork");
muscWorkProbe->setOperation("integrate");
SimTK::Vector ic1(1);
ic1 = 9.0; // some arbitary initial condition.
muscWorkProbe->setInitialConditions(ic1);
model.addProbe(muscWorkProbe);
model.setup();
cout << probeCounter++ << ") Added ActuatorPowerProbe to measure work done by the muscle" << endl;
// Add ActuatorPowerProbe to measure power generated by the muscle
ActuatorPowerProbe* muscPowerProbe = new ActuatorPowerProbe(*muscWorkProbe); // use copy constructor
muscPowerProbe->setName("ActuatorPower");
muscPowerProbe->setOperation("value");
model.addProbe(muscPowerProbe);
cout << probeCounter++ << ") Added ActuatorPowerProbe to measure power generated by the muscle" << endl;
// Add ActuatorPowerProbe to report the muscle power MINIMUM
ActuatorPowerProbe* powerProbeMinimum = new ActuatorPowerProbe(*muscPowerProbe); // use copy constructor
powerProbeMinimum->setName("ActuatorPowerMinimum");
powerProbeMinimum->setOperation("minimum");
model.addProbe(powerProbeMinimum);
cout << probeCounter++ << ") Added ActuatorPowerProbe to report the muscle power MINIMUM" << endl;
// Add ActuatorPowerProbe to report the muscle power ABSOLUTE MINIMUM
ActuatorPowerProbe* powerProbeMinAbs = new ActuatorPowerProbe(*muscPowerProbe); // use copy constructor
powerProbeMinAbs->setName("ActuatorPowerMinAbs");
powerProbeMinAbs->setOperation("minabs");
model.addProbe(powerProbeMinAbs);
cout << probeCounter++ << ") Added ActuatorPowerProbe to report the muscle power MINABS" << endl;
// Add ActuatorPowerProbe to report the muscle power MAXIMUM
ActuatorPowerProbe* powerProbeMaximum = new ActuatorPowerProbe(*muscPowerProbe); // use copy constructor
powerProbeMaximum->setName("ActuatorPowerMaximum");
powerProbeMaximum->setOperation("maximum");
model.addProbe(powerProbeMaximum);
cout << probeCounter++ << ") Added ActuatorPowerProbe to report the muscle power MAXIMUM" << endl;
// Add ActuatorPowerProbe to report the muscle power MAXABS
ActuatorPowerProbe* powerProbeMaxAbs = new ActuatorPowerProbe(*muscPowerProbe); // use copy constructor
powerProbeMaxAbs->setName("ActuatorPowerMaxAbs");
powerProbeMaxAbs->setOperation("maxabs");
model.addProbe(powerProbeMaxAbs);
cout << probeCounter++ << ") Added ActuatorPowerProbe to report the muscle power MAXABS" << endl;
// Add ActuatorPowerProbe to measure the square of the power generated by the muscle
ActuatorPowerProbe* muscPowerSquaredProbe = new ActuatorPowerProbe(*muscPowerProbe); // use copy constructor
muscPowerSquaredProbe->setName("ActuatorPowerSquared");
muscPowerSquaredProbe->setExponent(2.0);
model.addProbe(muscPowerSquaredProbe);
cout << probeCounter++ << ") Added ActuatorPowerProbe to measure the square of the power generated by the muscle" << endl;
// Add JointInternalPowerProbe to measure work done by the joint
JointInternalPowerProbe* jointWorkProbe = new JointInternalPowerProbe(jointNames, false, 1);
jointWorkProbe->setName("JointWork");
jointWorkProbe->setOperation("integrate");
jointWorkProbe->setInitialConditions(SimTK::Vector(1, 0.0));
model.addProbe(jointWorkProbe);
cout << probeCounter++ << ") Added JointPowerProbe to measure work done by the joint" << endl;
// Add JointPowerProbe to measure power generated by the joint
JointInternalPowerProbe* jointPowerProbe = new JointInternalPowerProbe(*jointWorkProbe); // use copy constructor
jointPowerProbe->setName("JointPower");
jointPowerProbe->setOperation("value");
model.addProbe(jointPowerProbe);
cout << probeCounter++ << ") Added JointPowerProbe to measure power generated by the joint" << endl;
// Add ActuatorForceProbe to measure the impulse of the muscle force
ActuatorForceProbe* impulseProbe = new ActuatorForceProbe(muscNames, false, 1);
impulseProbe->setName("ActuatorImpulse");
impulseProbe->setOperation("integrate");
impulseProbe->setInitialConditions(SimTK::Vector(1, 0.0));
model.addProbe(impulseProbe);
cout << probeCounter++ << ") Added ActuatorForceProbe to measure the impulse of the muscle force" << endl;
// Add ActuatorForceProbe to report the muscle force
ActuatorForceProbe* forceProbe = new ActuatorForceProbe(*impulseProbe); // use copy constructor
forceProbe->setName("ActuatorForce");
forceProbe->setOperation("value");
model.addProbe(forceProbe);
cout << probeCounter++ << ") Added ActuatorForceProbe to report the muscle force" << endl;
// Add ActuatorForceProbe to report the square of the muscle force
ActuatorForceProbe* forceSquaredProbe = new ActuatorForceProbe(*forceProbe); // use copy constructor
forceSquaredProbe->setName("ActuatorForceSquared");
forceSquaredProbe->setExponent(2.0);
model.addProbe(forceSquaredProbe);
cout << probeCounter++ << ") Added ActuatorForceProbe to report the square of the muscle force " << endl;
// Add ActuatorForceProbe to report the square of the muscle force for the same muscle repeated twice
ActuatorForceProbe* forceSquaredProbeTwice = new ActuatorForceProbe(*forceSquaredProbe); // use copy constructor
forceSquaredProbeTwice->setName("ActuatorForceSquared_RepeatedTwice");
forceSquaredProbeTwice->setSumForcesTogether(true);
forceSquaredProbeTwice->setActuatorNames(muscNamesTwice);
model.addProbe(forceSquaredProbeTwice);
cout << probeCounter++ << ") Added ActuatorForceProbe to report the square of the muscle force for the same muscle repeated twice" << endl;
// Add ActuatorForceProbe to report the square of the muscle force for the same muscle repeated twice, SCALED BY 0.5
ActuatorForceProbe* forceSquaredProbeTwiceScaled = new ActuatorForceProbe(*forceSquaredProbeTwice); // use copy constructor
forceSquaredProbeTwice->setName("ActuatorForceSquared_RepeatedTwiceThenHalved");
double gain1 = 0.5;
forceSquaredProbeTwiceScaled->setGain(gain1);
model.addProbe(forceSquaredProbeTwiceScaled);
cout << probeCounter++ << ") Added ActuatorForceProbe to report the square of the muscle force for the same muscle repeated twice, SCALED BY 0.5" << endl;
// Add ActuatorForceProbe to report -3.5X the muscle force
double gain2 = -3.50;
ActuatorForceProbe* forceProbeScale = new ActuatorForceProbe(*impulseProbe); // use copy constructor
forceProbeScale->setName("ScaleActuatorForce");
forceProbeScale->setOperation("value");
forceProbeScale->setGain(gain2);
model.addProbe(forceProbeScale);
cout << probeCounter++ << ") Added ActuatorForceProbe to report -3.5X the muscle force" << endl;
// Add ActuatorForceProbe to report the differentiated muscle force
ActuatorForceProbe* forceProbeDiff = new ActuatorForceProbe(*impulseProbe); // use copy constructor
forceProbeDiff->setName("DifferentiateActuatorForce");
forceProbeDiff->setOperation("differentiate");
model.addProbe(forceProbeDiff);
cout << probeCounter++ << ") Added ActuatorForceProbe to report the differentiated muscle force" << endl;
// Add SystemEnergyProbe to measure the system KE+PE
SystemEnergyProbe* sysEnergyProbe = new SystemEnergyProbe(true, true);
sysEnergyProbe->setName("SystemEnergy");
sysEnergyProbe->setOperation("value");
sysEnergyProbe->setComputeKineticEnergy(true);
sysEnergyProbe->setComputePotentialEnergy(true);
model.addProbe(sysEnergyProbe);
cout << probeCounter++ << ") Added SystemEnergyProbe to measure the system KE+PE" << endl;
// Add SystemEnergyProbe to measure system power (d/dt system KE+PE)
SystemEnergyProbe* sysPowerProbe = new SystemEnergyProbe(*sysEnergyProbe); // use copy constructor
sysPowerProbe->setName("SystemPower");
sysPowerProbe->setDisabled(false);
sysPowerProbe->setOperation("differentiate");
model.addProbe(sysPowerProbe);
cout << probeCounter++ << ") Added SystemEnergyProbe to measure system power (d/dt system KE+PE)" << endl;
// Add ActuatorForceProbe to report the muscle force value, twice -- REPORTED INDIVIDUALLY AS VECTORS
ActuatorForceProbe* forceSquaredProbeTwiceReportedIndividually1 = new ActuatorForceProbe(*forceProbe); // use copy constructor
forceSquaredProbeTwiceReportedIndividually1->setName("MuscleForce_VALUE_VECTOR");
forceSquaredProbeTwiceReportedIndividually1->setSumForcesTogether(false); // report individually
forceSquaredProbeTwiceReportedIndividually1->setActuatorNames(muscNamesTwice);
//cout << forceSquaredProbeTwiceReportedIndividually1->getActuatorNames().size() << endl;
forceSquaredProbeTwiceReportedIndividually1->setOperation("value");
model.addProbe(forceSquaredProbeTwiceReportedIndividually1);
cout << probeCounter++ << ") Added ActuatorForceProbe to report the muscle force value, twice - REPORTED INDIVIDUALLY" << endl;
// Add ActuatorForceProbe to report the differentiated muscle force value, twice -- REPORTED INDIVIDUALLY AS VECTORS
ActuatorForceProbe* forceSquaredProbeTwiceReportedIndividually2 = new ActuatorForceProbe(*forceSquaredProbeTwiceReportedIndividually1); // use copy constructor
forceSquaredProbeTwiceReportedIndividually2->setName("MuscleForce_DIFFERENTIATE_VECTOR");
forceSquaredProbeTwiceReportedIndividually2->setSumForcesTogether(false); // report individually
forceSquaredProbeTwiceReportedIndividually2->setOperation("differentiate");
model.addProbe(forceSquaredProbeTwiceReportedIndividually2);
cout << probeCounter++ << ") Added ActuatorForceProbe to report the differentiated muscle force value, twice - REPORTED INDIVIDUALLY" << endl;
// Add ActuatorForceProbe to report the integrated muscle force value, twice -- REPORTED INDIVIDUALLY AS VECTORS
ActuatorForceProbe* forceSquaredProbeTwiceReportedIndividually3 = new ActuatorForceProbe(*forceSquaredProbeTwiceReportedIndividually1); // use copy constructor
forceSquaredProbeTwiceReportedIndividually3->setName("MuscleForce_INTEGRATE_VECTOR");
forceSquaredProbeTwiceReportedIndividually3->setSumForcesTogether(false); // report individually
forceSquaredProbeTwiceReportedIndividually3->setOperation("integrate");
SimTK::Vector initCondVec(2);
initCondVec(0) = 0;
initCondVec(1) = 10;
forceSquaredProbeTwiceReportedIndividually3->setInitialConditions(initCondVec);
model.addProbe(forceSquaredProbeTwiceReportedIndividually3);
cout << probeCounter++ << ") Added ActuatorForceProbe to report the integrated muscle force value, twice - REPORTED INDIVIDUALLY" << endl;
cout << "initCondVec = " << initCondVec << endl;
/* Since all components are allocated on the stack don't have model
own them (and try to free)*/
// model.disownAllComponents();
model.setName("testProbesModel");
cout << "Saving model... " << endl;
model.print("testProbesModel.osim");
cout << "Re-loading model... " << endl;
Model reloadedModel = Model("testProbesModel.osim");
/* Setup analyses and reporters. */
ProbeReporter* probeReporter = new ProbeReporter(&model);
model.addAnalysis(probeReporter);
ForceReporter* forceReporter = new ForceReporter(&model);
model.addAnalysis(forceReporter);
MuscleAnalysis* muscleReporter = new MuscleAnalysis(&model);
model.addAnalysis(muscleReporter);
model.print("testProbesModel.osim");
model.printBasicInfo(cout);
//==========================================================================
// 3. SIMULATION Initialization
//==========================================================================
// Initialize the system and get the default state
SimTK::State& si = model.initSystem();
SimTK::Vector testRealInitConditions = forceSquaredProbeTwiceReportedIndividually3->getProbeOutputs(si);
model.getMultibodySystem().realize(si, SimTK::Stage::Dynamics);
model.equilibrateMuscles(si);
CoordinateSet& modelCoordinateSet = model.updCoordinateSet();
// Define non-zero (defaults are 0) states for the free joint
// set x-translation value
modelCoordinateSet[0].setValue(si, startX, true);
//Copy the initial state
SimTK::State initialState(si);
// Check muscle is setup correctly
const PathActuator &muscle
= dynamic_cast<const PathActuator&>(model.updActuators().get("muscle"));
double length = muscle.getLength(si);
double trueLength = startX + xSinG - anchorWidth / 2;
ASSERT_EQUAL(length / trueLength, 1.0, testTolerance, __FILE__, __LINE__,
"testMuscles: path failed to initialize to correct length.");
model.getMultibodySystem().realize(si, SimTK::Stage::Acceleration);
double Emuscle0 = muscWorkProbe->getProbeOutputs(si)(0);
//cout << "Muscle initial energy = " << Emuscle0 << endl;
double Esys0 = model.getMultibodySystem().calcEnergy(si);
Esys0 += (Emuscle0 + jointWorkProbe->getProbeOutputs(si)(0));
double PEsys0 = model.getMultibodySystem().calcPotentialEnergy(si);
//cout << "Total initial system energy = " << Esys0 << endl;
//==========================================================================
// 4. SIMULATION Integration
//==========================================================================
// Create the integrator
SimTK::RungeKuttaMersonIntegrator integrator(model.getMultibodySystem());
integrator.setAccuracy(integrationAccuracy);
// Create the manager
Manager manager(model, integrator);
// Integrate from initial time to final time
manager.setInitialTime(initialTime);
manager.setFinalTime(finalTime);
cout << "\nIntegrating from " << initialTime << " to " << finalTime << endl;
// Start timing the simulation
const clock_t start = clock();
// simulate
manager.integrate(si);
// how long did it take?
double comp_time = (double)(clock() - start) / CLOCKS_PER_SEC;
//==========================================================================
// 5. SIMULATION Reporting
//==========================================================================
double realTimeMultiplier = ((finalTime - initialTime) / comp_time);
printf("testMuscles: Realtime Multiplier: %f\n"
" : simulation duration / clock duration\n"
" > 1 : faster than real time\n"
" = 1 : real time\n"
" < 1 : slower than real time\n",
realTimeMultiplier);
/*
ASSERT(comp_time <= (finalTime-initialTime));
printf("testMuscles: PASSED Realtime test\n"
" %s simulation time: %f with accuracy %f\n\n",
actuatorType.c_str(), comp_time , accuracy);
*/
//An analysis only writes to a dir that exists, so create here.
if (printResults == true){
Storage states(manager.getStateStorage());
states.print("testProbes_states.sto");
probeReporter->getProbeStorage().print("testProbes_probes.sto");
forceReporter->getForceStorage().print("testProbes_forces.sto");
muscleReporter->getNormalizedFiberLengthStorage()->print("testProbes_normalizedFiberLength.sto");
cout << "\nDone with printing results..." << endl;
}
double muscleWork = muscWorkProbe->getProbeOutputs(si)(0);
cout << "Muscle work = " << muscleWork << endl;
// Test the resetting of probes
cout << "Resetting muscle work probe..." << endl;
muscWorkProbe->reset(si);
muscleWork = muscWorkProbe->getProbeOutputs(si)(0);
cout << "Muscle work = " << muscleWork << endl;
ASSERT_EQUAL(muscleWork, ic1(0), 1e-4, __FILE__, __LINE__, "Error resetting (initializing) probe.");
//==========================================================================
// 6. SIMULATION Tests
//==========================================================================
model.getMultibodySystem().realize(si, SimTK::Stage::Acceleration);
ASSERT_EQUAL(forceSquaredProbeTwiceScaled->getProbeOutputs(si)(0), gain1*forceSquaredProbeTwice->getProbeOutputs(si)(0), 1e-4, __FILE__, __LINE__, "Error with 'scale' operation.");
ASSERT_EQUAL(forceProbeScale->getProbeOutputs(si)(0), gain2*forceProbe->getProbeOutputs(si)(0), 1e-4, __FILE__, __LINE__, "Error with 'scale' operation.");
ASSERT_EQUAL(forceSquaredProbe->getProbeOutputs(si)(0), forceSquaredProbeTwiceScaled->getProbeOutputs(si)(0), 1e-4, __FILE__, __LINE__, "forceSquaredProbeTwiceScaled != forceSquaredProbe.");
ASSERT_EQUAL(forceSquaredProbe->getProbeOutputs(si)(0), pow(forceProbe->getProbeOutputs(si)(0), 2), 1e-4, __FILE__, __LINE__, "Error with forceSquaredProbe probe.");
ASSERT_EQUAL(forceSquaredProbeTwice->getProbeOutputs(si)(0), 2 * pow(forceProbe->getProbeOutputs(si)(0), 2), 1e-4, __FILE__, __LINE__, "Error with forceSquaredProbeTwice probe.");
for (int i = 0; i<initCondVec.size(); ++i) {
stringstream myError;
//myError << "Initial condition[" << i << "] for vector integration is not being correctly applied." << endl;
//ASSERT_EQUAL(testRealInitConditions(i), initCondVec(i), 1e-4, __FILE__, __LINE__, myError.str());
//if (testRealInitConditions(i) != initCondVec(i))
// cout << "WARNING: Initial condition[" << i << "] for vector integration is not being correctly applied.\nThis is actually an error, but I have made it into a warning for now so that the test passes..." << endl;
}
}
double OpenSimContext::getMuscleLength(Muscle& m) {
return m.getLength(*_configState);
}
// Muscles
double OpenSimContext::getActivation(Muscle& m) {
// realize to dynamics as required by new muscle models before asking for activation
_model->getMultibodySystem().realize(*_configState, SimTK::Stage::Dynamics);
return m.getActivation(*_configState);
}
/**
* Record the results.
*/
int StaticOptimization::
record(const SimTK::State& s)
{
if(!_modelWorkingCopy) return -1;
// Set model to whatever defaults have been updated to from the last iteration
SimTK::State& sWorkingCopy = _modelWorkingCopy->updWorkingState();
sWorkingCopy.setTime(s.getTime());
_modelWorkingCopy->initStateWithoutRecreatingSystem(sWorkingCopy);
// update Q's and U's
sWorkingCopy.setQ(s.getQ());
sWorkingCopy.setU(s.getU());
_modelWorkingCopy->getMultibodySystem().realize(sWorkingCopy, SimTK::Stage::Velocity);
//_modelWorkingCopy->equilibrateMuscles(sWorkingCopy);
const Set<Actuator>& acts = _modelWorkingCopy->getActuators();
int na = acts.getSize();
int nacc = _accelerationIndices.getSize();
// IPOPT
_numericalDerivativeStepSize = 0.0001;
_optimizerAlgorithm = "ipopt";
_printLevel = 0;
//_optimizationConvergenceTolerance = 1e-004;
//_maxIterations = 2000;
// Optimization target
_modelWorkingCopy->setAllControllersEnabled(false);
StaticOptimizationTarget target(sWorkingCopy,_modelWorkingCopy,na,nacc,_useMusclePhysiology);
target.setStatesStore(_statesStore);
target.setStatesSplineSet(_statesSplineSet);
target.setActivationExponent(_activationExponent);
target.setDX(_numericalDerivativeStepSize);
// Pick optimizer algorithm
SimTK::OptimizerAlgorithm algorithm = SimTK::InteriorPoint;
//SimTK::OptimizerAlgorithm algorithm = SimTK::CFSQP;
// Optimizer
SimTK::Optimizer *optimizer = new SimTK::Optimizer(target, algorithm);
// Optimizer options
//cout<<"\nSetting optimizer print level to "<<_printLevel<<".\n";
optimizer->setDiagnosticsLevel(_printLevel);
//cout<<"Setting optimizer convergence criterion to "<<_convergenceCriterion<<".\n";
optimizer->setConvergenceTolerance(_convergenceCriterion);
//cout<<"Setting optimizer maximum iterations to "<<_maximumIterations<<".\n";
optimizer->setMaxIterations(_maximumIterations);
optimizer->useNumericalGradient(false);
optimizer->useNumericalJacobian(false);
if(algorithm == SimTK::InteriorPoint) {
// Some IPOPT-specific settings
optimizer->setLimitedMemoryHistory(500); // works well for our small systems
optimizer->setAdvancedBoolOption("warm_start",true);
optimizer->setAdvancedRealOption("obj_scaling_factor",1);
optimizer->setAdvancedRealOption("nlp_scaling_max_gradient",1);
}
// Parameter bounds
SimTK::Vector lowerBounds(na), upperBounds(na);
for(int i=0,j=0;i<acts.getSize();i++) {
Actuator& act = acts.get(i);
lowerBounds(j) = act.getMinControl();
upperBounds(j) = act.getMaxControl();
j++;
}
target.setParameterLimits(lowerBounds, upperBounds);
_parameters = 0; // Set initial guess to zeros
// Static optimization
_modelWorkingCopy->getMultibodySystem().realize(sWorkingCopy,SimTK::Stage::Velocity);
target.prepareToOptimize(sWorkingCopy, &_parameters[0]);
//LARGE_INTEGER start;
//LARGE_INTEGER stop;
//LARGE_INTEGER frequency;
//QueryPerformanceFrequency(&frequency);
//QueryPerformanceCounter(&start);
try {
target.setCurrentState( &sWorkingCopy );
optimizer->optimize(_parameters);
}
catch (const SimTK::Exception::Base& ex) {
cout << ex.getMessage() << endl;
cout << "OPTIMIZATION FAILED..." << endl;
cout << endl;
cout << "StaticOptimization.record: WARN- The optimizer could not find a solution at time = " << s.getTime() << endl;
cout << endl;
double tolBounds = 1e-1;
bool weakModel = false;
string msgWeak = "The model appears too weak for static optimization.\nTry increasing the strength and/or range of the following force(s):\n";
for(int a=0;a<na;a++) {
Actuator* act = dynamic_cast<Actuator*>(&_forceSet->get(a));
if( act ) {
Muscle* mus = dynamic_cast<Muscle*>(&_forceSet->get(a));
if(mus==NULL) {
if(_parameters(a) < (lowerBounds(a)+tolBounds)) {
msgWeak += " ";
msgWeak += act->getName();
msgWeak += " approaching lower bound of ";
ostringstream oLower;
oLower << lowerBounds(a);
msgWeak += oLower.str();
msgWeak += "\n";
weakModel = true;
} else if(_parameters(a) > (upperBounds(a)-tolBounds)) {
msgWeak += " ";
msgWeak += act->getName();
msgWeak += " approaching upper bound of ";
ostringstream oUpper;
oUpper << upperBounds(a);
msgWeak += oUpper.str();
msgWeak += "\n";
weakModel = true;
}
} else {
if(_parameters(a) > (upperBounds(a)-tolBounds)) {
msgWeak += " ";
msgWeak += mus->getName();
msgWeak += " approaching upper bound of ";
ostringstream o;
o << upperBounds(a);
msgWeak += o.str();
msgWeak += "\n";
weakModel = true;
}
}
}
}
if(weakModel) cout << msgWeak << endl;
if(!weakModel) {
double tolConstraints = 1e-6;
bool incompleteModel = false;
string msgIncomplete = "The model appears unsuitable for static optimization.\nTry appending the model with additional force(s) or locking joint(s) to reduce the following acceleration constraint violation(s):\n";
SimTK::Vector constraints;
target.constraintFunc(_parameters,true,constraints);
const CoordinateSet& coordSet = _modelWorkingCopy->getCoordinateSet();
for(int acc=0;acc<nacc;acc++) {
if(fabs(constraints(acc)) > tolConstraints) {
const Coordinate& coord = coordSet.get(_accelerationIndices[acc]);
msgIncomplete += " ";
msgIncomplete += coord.getName();
msgIncomplete += ": constraint violation = ";
ostringstream o;
o << constraints(acc);
msgIncomplete += o.str();
msgIncomplete += "\n";
incompleteModel = true;
}
}
_forceReporter->step(sWorkingCopy, 1);
if(incompleteModel) cout << msgIncomplete << endl;
}
}
//QueryPerformanceCounter(&stop);
//double duration = (double)(stop.QuadPart-start.QuadPart)/(double)frequency.QuadPart;
//cout << "optimizer time = " << (duration*1.0e3) << " milliseconds" << endl;
target.printPerformance(sWorkingCopy, &_parameters[0]);
//update defaults for use in the next step
const Set<Actuator>& actuators = _modelWorkingCopy->getActuators();
for(int k=0; k < actuators.getSize(); ++k){
ActivationFiberLengthMuscle *mus = dynamic_cast<ActivationFiberLengthMuscle*>(&actuators[k]);
if(mus){
mus->setDefaultActivation(_parameters[k]);
// Don't send up red flags when the def
mus->setObjectIsUpToDateWithProperties();
}
}
_activationStorage->append(sWorkingCopy.getTime(),na,&_parameters[0]);
SimTK::Vector forces(na);
target.getActuation(const_cast<SimTK::State&>(sWorkingCopy), _parameters,forces);
_forceReporter->step(sWorkingCopy, 1);
return 0;
}
/**
* This function is called at every time step for every actuator.
*
* @param s Current state of the system
* @param controls Controls being calculated
*/
void computeControls(const SimTK::State& s, SimTK::Vector &controls) const
{
// Get the current time in the simulation.
double t = s.getTime();
// Read the mass of the block.
double blockMass = getModel().getBodySet().get( "block" ).getMass();
// Get pointers to each of the muscles in the model.
Muscle* leftMuscle = dynamic_cast<Muscle*> ( &getActuatorSet().get(0) );
Muscle* rightMuscle = dynamic_cast<Muscle*> ( &getActuatorSet().get(1) );
// Compute the desired position of the block in the tug-of-war
// model.
double zdes = desiredModelZPosition(t);
// Compute the desired velocity of the block in the tug-of-war
// model.
double zdesv = desiredModelZVelocity(t);
// Compute the desired acceleration of the block in the tug-
// of-war model.
double zdesa = desiredModelZAcceleration(t);
// Get the z translation coordinate in the model.
const Coordinate& zCoord = _model->getCoordinateSet().
get( "blockToGround_zTranslation" );
// Get the current position of the block in the tug-of-war
// model.
double z = zCoord.getValue(s);
// Get the current velocity of the block in the tug-of-war
// model.
double zv = zCoord.getSpeedValue(s);
// Compute the correction to the desired acceleration arising
// from the deviation of the block's current position from its
// desired position (this deviation is the "position error").
double pErrTerm = kp * ( zdes - z );
// Compute the total desired velocity based on the initial
// desired velocity plus the position error term we
// computed above.
double vErrTerm = kv * ( zdesv - zv );
// Compute the total desired acceleration based on the initial
// desired acceleration plus the position error term we
// computed above.
double desAcc = zdesa + vErrTerm + pErrTerm;
// Compute the desired force on the block as the mass of the
// block times the total desired acceleration of the block.
double desFrc = desAcc * blockMass;
// Get the maximum isometric force for the left muscle.
double FoptL = leftMuscle->getMaxIsometricForce();
// Get the maximum isometric force for the right muscle.
double FoptR = rightMuscle->getMaxIsometricForce();
// If desired force is in direction of one muscle's pull
// direction, then set that muscle's control based on desired
// force. Otherwise, set the muscle's control to zero.
double leftControl = 0.0, rightControl = 0.0;
if( desFrc < 0 ) {
leftControl = abs( desFrc ) / FoptL;
rightControl = 0.0;
}
else if( desFrc > 0 ) {
leftControl = 0.0;
rightControl = abs( desFrc ) / FoptR;
}
// Don't allow any control value to be greater than one.
if( leftControl > 1.0 ) leftControl = 1.0;
if( rightControl > 1.0 ) rightControl = 1.0;
// Thelen muscle has only one control
Vector muscleControl(1, leftControl);
// Add in the controls computed for this muscle to the set of all model controls
leftMuscle->addInControls(muscleControl, controls);
// Specify control for other actuator (muscle) controlled by this controller
muscleControl[0] = rightControl;
rightMuscle->addInControls(muscleControl, controls);
}
// Overwrite base class update
//----------------------------------------------------------------------------------------------------------------------
void ArmMuscledViz::update()
{
// Let simple viz do its rendering first
//ArmViz::update();
glPushAttrib(GL_LIGHTING);
glDisable(GL_LIGHTING);
glDisable(GL_DEPTH_TEST);
// Get shoulder and elbow TMs and other useful data
const float shdAngle = m_arm->getJointAngle(JT_shoulder);
const float elbAngle = m_arm->getJointAngle(JT_elbow);
const ci::Vec3f elbPos = Vec3f(m_arm->getElbowPos());
//const ci::Vec3f effPos = Vec3f(m_arm->getEffectorPos());
const ci::Vec3f shdPos = Vec3f(0,0,0);
const double elbRad = m_arm->getJointRadius(JT_elbow);
const double shdRad = m_arm->getJointRadius(JT_shoulder);
ci::Matrix44f elbLimTM;
elbLimTM.setToIdentity();
elbLimTM.rotate(Vec3f(0.0f, 0.0f, 1.0f), shdAngle);
elbLimTM.setTranslate(elbPos);
ci::Matrix44f elbTM = elbLimTM;
elbTM.rotate(Vec3f(0,0,1), elbAngle);
ci::Matrix44f shdTM;
shdTM.setToIdentity();
shdTM.rotate(Vec3f(0.0f, 0.0f, 1.0f), shdAngle);
shdTM.setTranslate(shdPos);
// Overall position and orientation
glPushMatrix();
glMultMatrixf(*m_pTM);
glLineWidth(1.0);
glColor3f(0,0,0);
// Desired kinematic state
if(m_drawDesiredState)
{
Pos p1, p2;
double desElbAngle = m_arm->getDesiredJointAngle(JT_elbow);
double desShdAngle = m_arm->getDesiredJointAngle(JT_shoulder);
m_arm->forwardKinematics(desElbAngle, desShdAngle, p1, p2);
Vec3f desShdPos(0,0,0);
Vec3f desElbPos(p1);
Vec3f desEffPos(p2);
ci::gl::color(m_colors.desired);
// Draw bones
glEnable(GL_LINE_STIPPLE);
glLineStipple(2, 0xAAAA);
ci::gl::drawLine(desShdPos, desElbPos);
ci::gl::drawLine(desElbPos, desEffPos);
glDisable(GL_LINE_STIPPLE);
// Points indicating joints
glPointSize(4.0);
glBegin(GL_POINTS);
glVertex3f(desShdPos.x, desShdPos.y, 0.0);
glVertex3f(desElbPos.x, desElbPos.y, 0.0);
glVertex3f(desEffPos.x, desEffPos.y, 0.0);
glEnd();
}
// Draw elbow joint and bone
glPushMatrix();
glMultMatrixf(elbTM);
// bone triangle
ci::gl::color(m_colors.boneFill);
ci::Vec3f lt = -elbRad * ci::Vec3f(0,1,0);
ci::Vec3f rt = elbRad * ci::Vec3f(0,1,0);
ci::Vec3f bt = m_arm->getLength(JT_elbow) * ci::Vec3f(1,0,0);
drawTriangle(rt, lt, bt);
ci::gl::color(m_colors.boneOutline);
drawTriangle(rt, lt, bt, GL_LINE);
ci::gl::drawLine(ci::Vec2f(&bt.x), elbRad * ci::Vec2f(1,0));
// joint disk
ci::gl::color(m_colors.jointFill);
ci::gl::drawSolidCircle(ci::Vec2f(0,0), elbRad, 32);
ci::gl::color(m_colors.jointOutline);
ci::gl::drawStrokedCircle(ci::Vec2f(0,0), elbRad, 32);
glPopMatrix();
// Draw elbow limits
glPushMatrix();
glMultMatrixf(elbLimTM);
float limMin = radiansToDegrees(m_arm->getJointLimitLower(JT_elbow));
float limMax = radiansToDegrees(m_arm->getJointLimitUpper(JT_elbow));
ci::gl::color(m_colors.limitsFill);
drawPartialDisk(elbRad, elbRad + 0.01, 16, 1, 90 - limMin, -(limMax - limMin));
glPopMatrix();
// Draw shoulder joint and bone
glColor3f(0,0,0);
glPushMatrix();
glMultMatrixf(shdTM);
// bone triangle
lt = -shdRad * ci::Vec3f(0,1,0);
rt = shdRad * ci::Vec3f(0,1,0);
bt = m_arm->getLength(JT_shoulder) * ci::Vec3f(1,0,0);
ci::gl::color(m_colors.boneFill);
drawTriangle(rt, lt, bt);
ci::gl::color(m_colors.boneOutline);
drawTriangle(rt, lt, bt, GL_LINE);
ci::gl::drawLine(m_arm->getLength(JT_shoulder) * ci::Vec2f(1,0), shdRad * ci::Vec2f(1,0));
// joint disk
ci::gl::color(m_colors.jointFill);
ci::gl::drawSolidCircle(ci::Vec2f(0,0), shdRad, 32);
ci::gl::color(m_colors.jointOutline);
ci::gl::drawStrokedCircle(ci::Vec2f(0,0), shdRad, 32);
glPopMatrix();
// Draw shoulder limits
limMin = m_arm->getJointLimitLower(JT_shoulder) * RAD_TO_DEG;
limMax = m_arm->getJointLimitUpper(JT_shoulder) * RAD_TO_DEG;
ci::gl::color(m_colors.limitsFill);
drawPartialDisk(shdRad, shdRad + 0.01, 16, 1, 90 - limMin, -(limMax - limMin));
glLineWidth(1.0);
// Trajectory
if(m_drawOverlays)
{
glEnableClientState(GL_VERTEX_ARRAY);
glEnableClientState(GL_COLOR_ARRAY);
const std::deque<Pos>& effTrajectory = m_arm->getTrajectory();
int numPoints = effTrajectory.size();
float lineVerts[numPoints*2];
float colors[numPoints*4];
glVertexPointer(2, GL_FLOAT, 0, lineVerts); // 2d positions
glColorPointer(4, GL_FLOAT, 0, colors); // 4d colors
for(size_t i = 0; i < numPoints; i++)
{
lineVerts[i*2 + 0] = effTrajectory[i].x;
lineVerts[i*2 + 1] = effTrajectory[i].y;
float a = 0.5 * (float)i / (float)numPoints;
colors[i*4 + 0] = m_colors.trajectory[0];
colors[i*4 + 1] = m_colors.trajectory[1];
colors[i*4 + 2] = m_colors.trajectory[2];
colors[i*4 + 3] = a;
}
glLineWidth(2.0);
glDrawArrays( GL_LINE_STRIP, 0, numPoints);
glDisableClientState(GL_VERTEX_ARRAY);
glDisableClientState(GL_COLOR_ARRAY);
glLineWidth(1.0);
}
// Draw muscles
for(size_t i = 0; i < m_arm->getNumMuscles(); ++i)
{
Muscle* muscle = m_arm->getMuscle(i);
double act = muscle->getActivation();
// Muscle line width dependent on activation
float lineWidth = act * 32.0;
glLineWidth(lineWidth);
// Muscle colour dependent on length
float l = muscle->getNormalisedLength() - 1;
l = clamp(l, -1.0f, 1.0f);
ci::ColorA col = m_colors.midMuscle;
if(l >= 0)
{
col += (m_colors.longMuscle - m_colors.midMuscle) * l;
}
else
{
col -= (m_colors.shortMuscle - m_colors.midMuscle) * l;
}
ci::gl::color(col);
// Draw origin and insertion points
Vec3f origin = Vec3f(muscle->getOriginWorld());
Vec3f insertion = Vec3f(muscle->getInsertionWorld());
glPointSize(4.0);
glBegin(GL_POINTS);
glVertex3f(origin.x, origin.y, 0);
glVertex3f(insertion.x, insertion.y, 0);
glEnd();
if(muscle->isMonoArticulate())
{
MuscleMonoWrap* m = ((MuscleMonoWrap*)muscle);
if(!m->m_muscleWraps)
{
//ci::Vec2f dir = m->getInsertionWorld() - m->getOriginWorld();
//dir.normalize();
// Only hold in the non-wrapping case. Otherwise we need to do a proper projection (dot product).
//ci::Vec2f closestPoint = m->getOriginWorld() + dir * m->m_originCapsuleDist;
//ci::Vec2f maVec = closestPoint - m_arm->getElbowPos();
//float ma = maVec.length();
//const bool muscleWraps = ma < r && m_arm->getJointAngle(JT_elbow) < PI_OVER_TWO;
//ci::gl::drawLine(ci::Vec3f(closestPoint), ci::Vec3f(m_arm->getElbowPos()));
ci::gl::drawLine(origin, insertion);
// Indicate optimal length
//ci::Vec3f l0Pos = origin + m->getOptimalLength() * (insertion - origin).normalized();
//drawPoint(l0Pos, 4.0);
}
else
{
// Upstream segment
ci::Vec2f pathDir = (m->m_joint == JT_elbow) ? m_arm->getElbowPos() : ci::Vec2f(1, 0);
float rotDir = m->m_isFlexor ? 1.0 : -1.0;
pathDir.rotate(rotDir * (PI_OVER_TWO - m->m_originCapsuleAngle));
pathDir.normalize();
ci::Vec2f pathEnd = m->getOriginWorld() + (pathDir * m->m_originCapsuleDist);
ci::gl::drawLine(origin, ci::Vec3f(pathEnd));
// Downstream segment
pathDir = (m->m_joint == JT_elbow) ? (m_arm->getElbowPos() - m_arm->getEffectorPos()) : ( -m_arm->getElbowPos());
pathDir.rotate(-rotDir * (PI_OVER_TWO - m->m_insertCapsuleAngle));
pathDir.normalize();
pathEnd = m->getInsertionWorld() + (pathDir * m->m_insertCapsuleDist);
ci::gl::drawLine(insertion, ci::Vec3f(pathEnd));
}
} // is mono
else
{
glPushAttrib (GL_LINE_BIT);
glEnable(GL_LINE_STIPPLE);
glLineStipple (1, 0xAAAA);
MuscleBiWrap* m = ((MuscleBiWrap*)muscle);
if (m->wrapsElbow() && m->wrapsShoulder())
{
//glColor3f(0,0,1);
// Upstream segment: always shoulder
ci::Vec2f pathDir = ci::Vec2f(1, 0);
float rotDir = m->m_isFlexor ? 1.0 : -1.0;
pathDir.rotate(rotDir * (PI_OVER_TWO - m->m_originCapsuleAngle));
pathDir.normalize();
ci::Vec2f pathEnd = m->getOriginWorld() + (pathDir * m->m_originCapsuleDist);
ci::gl::drawLine(origin, ci::Vec3f(pathEnd));
// Downstream segment: always elbow.
pathDir = m_arm->getElbowPos() - m_arm->getEffectorPos();
pathDir.rotate(-rotDir * (PI_OVER_TWO - m->m_insertCapsuleAngle));
pathDir.normalize();
pathEnd = m->getInsertionWorld() + (pathDir * m->m_insertCapsuleDist);
ci::gl::drawLine(insertion, ci::Vec3f(pathEnd));
// Middle segment
pathDir = m_arm->getElbowPos();
pathDir.rotate(rotDir * m->m_gammaAngle);
pathDir.normalize();
pathDir = pathDir * m->m_momentArms[JT_shoulder];
ci::Vec2f pathStart = pathDir;
pathDir = m_arm->getEffectorPos() - m_arm->getElbowPos();
pathDir.rotate(rotDir * (m->m_insertCapsuleAngle + m->getWrapAngle(JT_elbow)));
pathDir.normalize();
pathDir = pathDir * m_arm->getJointRadius(JT_elbow);
pathEnd = m_arm->getElbowPos() + pathDir;
ci::gl::drawLine(ci::Vec3f(pathStart), ci::Vec3f(pathEnd));
}
else if (m->wrapsElbow())
{
//glColor3f(0,1,0);
// Downstream segment: always elbow. Upstream segment from origin to capsule
ci::Vec2f pathDir = ci::Vec2f(m_arm->getElbowPos() - m_arm->getEffectorPos());
float rotDir = m->m_isFlexor ? 1.0 : -1.0;
pathDir.rotate(-rotDir * (PI_OVER_TWO - m->m_insertCapsuleAngle));
pathDir.normalize();
ci::Vec2f pathEnd = m->getInsertionWorld() + (pathDir * m->m_insertCapsuleDist);
ci::gl::drawLine(insertion, ci::Vec3f(pathEnd));
pathDir = m_arm->getEffectorPos() - m_arm->getElbowPos();
pathDir.rotate(rotDir * (m->m_insertCapsuleAngle + m->getWrapAngle(JT_elbow)));
pathDir.normalize();
pathDir = pathDir * m_arm->getJointRadius(JT_elbow);
pathEnd = m_arm->getElbowPos() + pathDir;
ci::gl::drawLine(origin, ci::Vec3f(pathEnd));
}
else if (m->wrapsShoulder())
{
//glColor3f(1,0,0);
// Upstream segment: always shoulder
ci::Vec2f pathDir = ci::Vec2f(1, 0);
float rotDir = m->m_isFlexor ? 1.0 : -1.0;
pathDir.rotate(rotDir * (PI_OVER_TWO - m->m_originCapsuleAngle));
pathDir.normalize();
ci::Vec2f pathEnd = m->getOriginWorld() + (pathDir * m->m_originCapsuleDist);
ci::gl::drawLine(origin, ci::Vec3f(pathEnd));
//Downstream segment: from capsule to insertion
pathDir = m_arm->getElbowPos();
pathDir.rotate(rotDir * m->m_gammaAngle);
pathDir.normalize();
pathDir = pathDir * m->m_momentArms[JT_shoulder];
ci::Vec2f pathStart = pathDir;
ci::gl::drawLine(ci::Vec3f(pathStart), insertion);
}
else
{
// No wrap
//glColor3f(1,1,1);
ci::gl::drawLine(origin, insertion);
}
glPopAttrib();
}
} // for all muscles
glLineWidth(1.0);
glPopMatrix();
glEnable(GL_DEPTH_TEST);
glPopAttrib();
}
