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