//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(); }