/** * This method is called at the beginning of an analysis so that any * necessary initializations may be performed. * * This method is meant to be called at the begining of an integration * * @param s current state of System * * @return -1 on error, 0 otherwise. */ int Kinematics:: begin( SimTK::State& s ) { if(!proceed()) return(0); double time = s.getTime(); // RESET STORAGE _pStore->reset(time); _vStore->reset(time); if (_aStore) _aStore->reset(time); // RECORD int status = 0; if(_pStore->getSize()<=0) { if(_recordAccelerations && s.getSystemStage() < SimTK::Stage::Acceleration ) _model->getMultibodySystem().realize(s, SimTK::Stage::Acceleration); else _model->getMultibodySystem().realize(s, SimTK::Stage::Velocity); status = record(s); } return(status); }
bool RollingOnSurfaceConstraint::setDisabled(SimTK::State& state, bool isDisabled) { // All constraints treated the same as default behavior i.e. at initilization std::vector<bool> shouldBeOn(_numConstraintEquations, !isDisabled); // If dynamics has been realized, then this is an attempt to enable/disable the constraint // during a computation and not an initialization, in which case we must check the // unilateral conditions for each constraint if(state.getSystemStage() > Stage::Dynamics) shouldBeOn = unilateralConditionsSatisfied(state); return setDisabled(state, isDisabled, shouldBeOn); }
/** Solve the inverse dynamics system of equations for generalized coordinate forces, Tau. Applied loads are explicity provided as generalized coordinate forces (MobilityForces) and/or a Vector of Spatial-body forces */ Vector InverseDynamicsSolver::solve(const SimTK::State &s, const SimTK::Vector &udot, const SimTK::Vector &appliedMobilityForces, const SimTK::Vector_<SimTK::SpatialVec>& appliedBodyForces) { //Results of the inverse dynamics for the generalized forces to satisfy accelerations Vector residualMobilityForces; if(s.getSystemStage() < SimTK::Stage::Dynamics) getModel().getMultibodySystem().realize(s,SimTK::Stage::Dynamics); // Perform inverse dynamics getModel().getMultibodySystem().getMatterSubsystem().calcResidualForceIgnoringConstraints(s, appliedMobilityForces, appliedBodyForces, udot, residualMobilityForces); return residualMobilityForces; }
/** * From a potentially partial specification of the generalized coordinates, * form a complete storage of the generalized coordinates (q's) and * generalized speeds (u's). * * @param aQIn Storage containing the q's or a subset of the q's. Rotational * q's should be in degrees. * @param rQComplete Storage containing all the q's. If q's were not * in aQIn, the values are set to 0.0. When a q is constrained, its value * is altered to be consistent with the constraint. The caller is responsible * for deleting the memory associated with this storage. * @param rUComplete Storage containing all the u's. The generalized speeds * are obtained by spline fitting the q's and differentiating the splines. * When a u is constrained, its value is altered to be consistent with the * constraint. The caller is responsible for deleting the memory * associated with this storage. */ void SimbodyEngine:: formCompleteStorages( const SimTK::State& s, const OpenSim::Storage &aQIn, OpenSim::Storage *&rQComplete,OpenSim::Storage *&rUComplete) const { int i; int nq = _model->getNumCoordinates(); int nu = _model->getNumSpeeds(); // Get coordinate file indices Array<string> columnLabels, speedLabels, coordStateNames; columnLabels.append("time"); speedLabels = columnLabels; Array<int> index(-1,nq); const CoordinateSet& coordinateSet = _model->getCoordinateSet(); int sizeCoordSet = coordinateSet.getSize(); for(i=0;i<sizeCoordSet;i++) { Coordinate& coord = coordinateSet.get(i); string prefix = coord.getJoint().getName() + "/" + coord.getName() + "/"; coordStateNames = coord.getStateVariableNames(); columnLabels.append(prefix+coordStateNames[0]); speedLabels.append(prefix+coordStateNames[1]); int fix = aQIn.getStateIndex(coord.getName()); if (fix < 0) { fix = aQIn.getStateIndex(columnLabels[i+1]); } index[i] = fix; if(index[i]<0) { string msg = "Model::formCompleteStorages(): WARNING- Did not find column "; msg += coordStateNames[0]; msg += " in storage object.\n"; cout << msg << endl; } } // Extract Coordinates double time; Array<double> data(0.0); Array<double> q(0.0,nq); Storage *qStore = new Storage(); qStore->setInDegrees(aQIn.isInDegrees()); qStore->setName("GeneralizedCoordinates"); qStore->setColumnLabels(columnLabels); int size = aQIn.getSize(); StateVector *vector; int j; for(i=0;i<size;i++) { vector = aQIn.getStateVector(i); data = vector->getData(); time = vector->getTime(); for(j=0;j<nq;j++) { q[j] = 0.0; if(index[j]<0) continue; q[j] = data[index[j]]; } qStore->append(time,nq,&q[0]); } // Convert to radians if (aQIn.isInDegrees()) convertDegreesToRadians(*qStore); // Compute generalized speeds GCVSplineSet tempQset(5,qStore); Storage *uStore = tempQset.constructStorage(1); // Compute constraints Array<double> qu(0.0,nq+nu); rQComplete = new Storage(); rUComplete = new Storage(); State constrainedState = s; _model->getMultibodySystem().realize(constrainedState, s.getSystemStage()); for(i=0;i<size;i++) { qStore->getTime(i,time); qStore->getData(i,nq,&qu[0]); uStore->getData(i,nq,&qu[nq]); for (int j = 0; j < nq; j++) { Coordinate& coord = coordinateSet.get(j); coord.setValue(constrainedState, qu[j], false); coord.setSpeedValue(constrainedState, qu[nq+j]); } _model->assemble(constrainedState); for (int j = 0; j < nq; j++) { Coordinate& coord = coordinateSet.get(j); qu[j] = coord.getValue(constrainedState); qu[nq+j] = coord.getSpeedValue(constrainedState); } rQComplete->append(time,nq,&qu[0]); rUComplete->append(time,nu,&qu[nq]); } delete qStore; // Compute storage object for simulation // Need to set column labels before converting rad->deg rQComplete->setColumnLabels(columnLabels); rUComplete->setColumnLabels(speedLabels); // Convert back to degrees convertRadiansToDegrees(*rQComplete); convertRadiansToDegrees(*rUComplete); }
/** * Record the MuscleAnalysis quantities. */ int MuscleAnalysis::record(const SimTK::State& s) { if(_model==NULL) return -1; if (!getOn()) return -1; // MAKE SURE ALL ACTUATION QUANTITIES ARE VALID // COMPUTE DERIVATIVES // ---------------------------------- // TIME NORMALIZATION double tReal = s.getTime(); // ---------------------------------- // LOOP THROUGH MUSCLES int nm = _muscleArray.getSize(); double nan = SimTK::NaN; // Angles and lengths Array<double> penang(nan,nm); Array<double> len(nan,nm), tlen(nan,nm); Array<double> fiblen(nan,nm), normfiblen(nan,nm); // Muscle velocity information Array<double> fibVel(nan,nm), normFibVel(nan,nm); Array<double> penAngVel(nan,nm); // Muscle component forces Array<double> force(nan, nm), fibforce(nan, nm); Array<double> actfibforce(nan,nm), passfibforce(nan,nm); Array<double> actfibforcealongten(nan,nm), passfibforcealongten(nan,nm); // Muscle and component powers Array<double> fibActivePower(nan,nm), fibPassivePower(nan,nm), tendonPower(nan,nm), muscPower(nan,nm); double sysMass = _model->getMatterSubsystem().calcSystemMass(s); bool hasMass = sysMass > SimTK::Eps; // Just warn once per instant bool lengthWarning = false; bool forceWarning = false; bool dynamicsWarning = false; for(int i=0; i<nm; ++i) { try{ len[i] = _muscleArray[i]->getLength(s); tlen[i] = _muscleArray[i]->getTendonLength(s); fiblen[i] = _muscleArray[i]->getFiberLength(s); normfiblen[i] = _muscleArray[i]->getNormalizedFiberLength(s); penang[i] = _muscleArray[i]->getPennationAngle(s); } catch (const std::exception& e) { if(!lengthWarning){ cout << "WARNING- MuscleAnalysis::record() unable to evaluate "; cout << "muscle length at time " << s.getTime() << " for reason: "; cout << e.what() << endl; lengthWarning = true; } continue; } try{ // Compute muscle forces that are dependent on Positions, Velocities // so that later quantities are valid and setForce is called _muscleArray[i]->computeActuation(s); force[i] = _muscleArray[i]->getActuation(s); fibforce[i] = _muscleArray[i]->getFiberForce(s); actfibforce[i] = _muscleArray[i]->getActiveFiberForce(s); passfibforce[i] = _muscleArray[i]->getPassiveFiberForce(s); actfibforcealongten[i] = _muscleArray[i]->getActiveFiberForceAlongTendon(s); passfibforcealongten[i] = _muscleArray[i]->getPassiveFiberForceAlongTendon(s); } catch (const std::exception& e) { if(!forceWarning){ cout << "WARNING- MuscleAnalysis::record() unable to evaluate "; cout << "muscle forces at time " << s.getTime() << " for reason: "; cout << e.what() << endl; forceWarning = true; } continue; } } // Cannot compute system dynamics without mass if(hasMass){ // state derivatives (activation rate and fiber velocity) evaluated at dynamics _model->getMultibodySystem().realize(s,SimTK::Stage::Dynamics); for(int i=0; i<nm; ++i) { try{ //Velocities fibVel[i] = _muscleArray[i]->getFiberVelocity(s); normFibVel[i] = _muscleArray[i]->getNormalizedFiberVelocity(s); penAngVel[i] = _muscleArray[i]->getPennationAngularVelocity(s); //Powers fibActivePower[i] = _muscleArray[i]->getFiberActivePower(s); fibPassivePower[i] = _muscleArray[i]->getFiberPassivePower(s); tendonPower[i] = _muscleArray[i]->getTendonPower(s); muscPower[i] = _muscleArray[i]->getMusclePower(s); } catch (const std::exception& e) { if(!dynamicsWarning){ cout << "WARNING- MuscleAnalysis::record() unable to evaluate "; cout << "muscle forces at time " << s.getTime() << " for reason: "; cout << e.what() << endl; dynamicsWarning = true; } continue; } } } else { if(!dynamicsWarning){ cout << "WARNING- MuscleAnalysis::record() unable to evaluate "; cout << "muscle dynamics at time " << s.getTime() << " because "; cout << "model has no mass and system dynamics cannot be computed." << endl; dynamicsWarning = true; } } // APPEND TO STORAGE _pennationAngleStore->append(tReal,penang.getSize(),&penang[0]); _lengthStore->append(tReal,len.getSize(),&len[0]); _fiberLengthStore->append(tReal,fiblen.getSize(),&fiblen[0]); _normalizedFiberLengthStore ->append(tReal,normfiblen.getSize(),&normfiblen[0]); _tendonLengthStore->append(tReal,tlen.getSize(),&tlen[0]); _fiberVelocityStore->append(tReal,fibVel.getSize(),&fibVel[0]); _normFiberVelocityStore->append(tReal,normFibVel.getSize(),&normFibVel[0]); _pennationAngularVelocityStore ->append(tReal,penAngVel.getSize(),&penAngVel[0]); _forceStore->append(tReal,force.getSize(),&force[0]); _fiberForceStore->append(tReal,fibforce.getSize(),&fibforce[0]); _activeFiberForceStore->append(tReal,actfibforce.getSize(),&actfibforce[0]); _passiveFiberForceStore ->append(tReal,passfibforce.getSize(),&passfibforce[0]); _activeFiberForceAlongTendonStore ->append(tReal,actfibforcealongten.getSize(),&actfibforcealongten[0]); _passiveFiberForceAlongTendonStore ->append(tReal,passfibforcealongten.getSize(),&passfibforcealongten[0]); _fiberActivePowerStore ->append(tReal,fibActivePower.getSize(),&fibActivePower[0]); _fiberPassivePowerStore ->append(tReal,fibPassivePower.getSize(),&fibPassivePower[0]); _tendonPowerStore->append(tReal,tendonPower.getSize(),&tendonPower[0]); _musclePowerStore->append(tReal,muscPower.getSize(),&muscPower[0]); if (_computeMoments){ // LOOP OVER ACTIVE MOMENT ARM STORAGE OBJECTS Coordinate *q = NULL; Storage *maStore=NULL, *mStore=NULL; int nq = _momentArmStorageArray.getSize(); Array<double> ma(0.0,nm),m(0.0,nm); for(int i=0; i<nq; i++) { q = _momentArmStorageArray[i]->q; maStore = _momentArmStorageArray[i]->momentArmStore; mStore = _momentArmStorageArray[i]->momentStore; // bool locked = q->getLocked(s); _model->getMultibodySystem().realize(s, s.getSystemStage()); // LOOP OVER MUSCLES for(int j=0; j<nm; j++) { ma[j] = _muscleArray[j]->computeMomentArm(s,*q); m[j] = ma[j] * force[j]; } maStore->append(s.getTime(),nm,&ma[0]); mStore->append(s.getTime(),nm,&m[0]); } } return 0; }
void FunctionBasedBushingForce::generateDecorations( bool fixed, const ModelDisplayHints& hints, const SimTK::State& s, SimTK::Array_<SimTK::DecorativeGeometry>& geometryArray) const { // invoke parent class method Super::generateDecorations(fixed, hints , s, geometryArray); // draw frame1 as red SimTK::Vec3 frame1color(1.0, 0.0, 0.0); // draw frame2 as blue SimTK::Vec3 frame2color(0.0, 0.5, 1.0); // the moment on frame2 will be yellow SimTK::Vec3 moment2color(1.0, 1.0, 0.0); // the force on frame2 will be green SimTK::Vec3 force2color(0.0, 1.0, 0.0); // create decorative frames to be fixed on frame1 and frame2 SimTK::DecorativeFrame decorativeFrame1(0.2); SimTK::DecorativeFrame decorativeFrame2(0.2); // get connected frames const PhysicalFrame& frame1 = getFrame1(); const PhysicalFrame& frame2 = getFrame2(); // attach frame 1 to body 1, translate and rotate it to the location of the bushing decorativeFrame1.setBodyId(frame1.getMobilizedBodyIndex()); decorativeFrame1.setTransform(frame1.findTransformInBaseFrame()); decorativeFrame1.setColor(frame1color); // attach frame 2 to body 2, translate and rotate it to the location of the bushing decorativeFrame2.setBodyId(frame2.getMobilizedBodyIndex()); decorativeFrame2.setTransform(frame2.findTransformInBaseFrame()); decorativeFrame2.setColor(frame2color); geometryArray.push_back(decorativeFrame1); geometryArray.push_back(decorativeFrame2); // if the model is moving and the state is adequately realized, // calculate and draw the bushing forces. if (!fixed && (s.getSystemStage() >= Stage::Dynamics)) { SpatialVec F_GM(Vec3(0.0), Vec3(0.0)); SpatialVec F_GF(Vec3(0.0), Vec3(0.0)); // total bushing force in the internal basis of the deflection (dq) Vec6 f = calcStiffnessForce(s) + calcDampingForce(s); convertInternalForceToForcesOnFrames(s, f, F_GF, F_GM); // location of the bushing on frame2 //SimTK::Vec3 p_b2M_b2 = frame2.findTransformInBaseFrame().p(); SimTK::Vec3 p_GM_G = frame2.getTransformInGround(s).p(); // Add moment on frame2 as line vector starting at bushing location SimTK::Vec3 scaled_M_GM(get_moment_visual_scale()*F_GM[0]); SimTK::Real m_length(scaled_M_GM.norm()); SimTK::Real m_radius(m_length / get_visual_aspect_ratio() / 2.0); SimTK::Transform X_m2cylinder(SimTK::Rotation(SimTK::UnitVec3(scaled_M_GM), SimTK::YAxis), p_GM_G + scaled_M_GM / 2.0); SimTK::DecorativeCylinder frame2Moment(m_radius, m_length / 2.0); frame2Moment.setTransform(X_m2cylinder); frame2Moment.setColor(moment2color); geometryArray.push_back(frame2Moment); // Add force on frame2 as line vector starting at bushing location SimTK::Vec3 scaled_F_GM(get_force_visual_scale()*F_GM[1]); SimTK::Real f_length(scaled_F_GM.norm()); SimTK::Real f_radius(f_length / get_visual_aspect_ratio() / 2.0); SimTK::Transform X_f2cylinder(SimTK::Rotation(SimTK::UnitVec3(scaled_F_GM), SimTK::YAxis), p_GM_G + scaled_F_GM / 2.0); SimTK::DecorativeCylinder frame2Force(f_radius, f_length / 2.0); frame2Force.setTransform(X_f2cylinder); frame2Force.setColor(force2color); geometryArray.push_back(frame2Force); } }
/** * Compute and record the results. * * This method computes the reaction loads at all joints in the model, then * truncates the results to contain only the loads at the requested joints * and finally, if necessary, modifies the loads to be acting on the specified * body and expressed in the specified frame * * @param aT Current time in the simulation. * @param aX Current values of the controls. * @param aY Current values of the states. */ int JointReaction:: record(const SimTK::State& s) { /** if a forces file is specified replace the computed actuation with the forces from storage.*/ SimTK::State s_analysis = s; _model->updMultibodySystem().realize(s_analysis, s.getSystemStage()); if(_useForceStorage){ const Set<Actuator> *actuatorSet = &_model->getActuators(); int nA = actuatorSet->getSize(); Array<double> forces(0,nA); _storeActuation->getDataAtTime(s.getTime(),nA,forces); int storageIndex = -1; for(int actuatorIndex=0;actuatorIndex<nA;actuatorIndex++) { //Actuator* act = dynamic_cast<Actuator*>(&_forceSet->get(actuatorIndex)); std::string actuatorName = actuatorSet->get(actuatorIndex).getName(); storageIndex = _storeActuation->getStateIndex(actuatorName, 0); if(storageIndex == -1){ cout << "The actuator, " << actuatorName << ", was not found in the forces file." << endl; break; } actuatorSet->get(actuatorIndex).overrideForce(s_analysis,true); actuatorSet->get(actuatorIndex).setOverrideForce(s_analysis,forces[storageIndex]); } } // VARIABLES int numBodies = _model->getNumBodies(); /** define 2 variable length vectors of Vec3 vectors to contain calculated * forces and moments for all the bodies in the model */ Vector_<Vec3> allForcesVec(numBodies); Vector_<Vec3> allMomentsVec(numBodies); double Mass = 0.0; //// BodySet and JointSet and ground body index const BodySet& bodySet = _model->getBodySet(); const JointSet& jointSet = _model->getJointSet(); Body &ground = _model->getSimbodyEngine().getGroundBody(); int groundIndex = bodySet.getIndex(ground.getName()); /* Calculate All joint reaction forces and moments. * Applied to child bodies, expressed in ground frame. * computeReactions realizes to the acceleration stage internally * so you don't have to call realize in this analysis.*/ _model->getSimbodyEngine().computeReactions(s_analysis, allForcesVec, allMomentsVec); /* retrieved desired joint reactions, convert to desired bodies, and convert * to desired reference frames*/ int numOutputJoints = _reactionList.getSize(); Vector_<Vec3> forcesVec(numOutputJoints), momentsVec(numOutputJoints), pointsVec(numOutputJoints); for(int i=0; i<numOutputJoints; i++) { JointReactionKey currentKey = _reactionList[i]; const Joint& joint = jointSet.get(currentKey.jointName); Vec3 force = allForcesVec[currentKey.reactionIndex]; Vec3 moment = allMomentsVec[currentKey.reactionIndex]; Body& expressedInBody = bodySet.get(currentKey.inFrameIndex); // find the point of application of the joint load on the child Vec3 childLocation(0,0,0); joint.getLocation(childLocation); // and find it's current location in the ground reference frame Vec3 childLocationInGlobal(0,0,0); _model->getSimbodyEngine().getPosition(s_analysis, joint.getBody(), childLocation,childLocationInGlobal); // set the point of application to the joint location in the child body Vec3 pointOfApplication(0,0,0); // check if the load on the child needs to be converted to an equivalent // load on the parent body. if(currentKey.onBodyIndex != currentKey.reactionIndex){ /*Take reaction load from child and apply on parent*/ force = -force; moment = -moment; Vec3 parentLocation(0,0,0); joint.getLocationInParent(parentLocation); Vec3 parentLocationInGlobal(0,0,0); //_model->getSimbodyEngine().getPosition(s_analysis, joint.getBody(), childLocation,childLocationInGlobal); _model->getSimbodyEngine().getPosition(s_analysis,joint.getParentBody(), parentLocation, parentLocationInGlobal); // define vector from the mobilizer location on the child to the location on the parent Vec3 translation = parentLocationInGlobal - childLocationInGlobal; // find equivalent moment if the load is shifted to the parent loaction moment -= translation % force; // reset the point of application to the joint location in the parent expressed in ground pointOfApplication = parentLocationInGlobal; } else{ // set the point of application to the joint laction in the child expressed in ground pointOfApplication = childLocationInGlobal; } /* express loads in the desired reference frame*/ _model->getSimbodyEngine().transform(s_analysis,ground,force,expressedInBody,force); _model->getSimbodyEngine().transform(s_analysis,ground,moment,expressedInBody,moment); _model->getSimbodyEngine().transformPosition(s_analysis,ground,pointOfApplication,expressedInBody,pointOfApplication); /* place results in the truncated loads vectors*/ forcesVec[i] = force; momentsVec[i] = moment; pointsVec[i] = pointOfApplication; } /* fill out row construction array*/ for(int i=0;i<numOutputJoints;i++) { int I = 9*i; for(int j=0;j<3;j++) { _Loads[I+j] = forcesVec[i][j]; _Loads[I+j+3] = momentsVec[i][j]; _Loads[I+j+6] = pointsVec[i][j]; } } /* Write the reaction data to storage*/ _storeReactionLoads.append(s.getTime(),_Loads.getSize(),&_Loads[0]); return(0); }