/**
* 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 beginning of an integration in
* Model::integBeginCallback() and has the same argument list.
*
* This method should be overridden in the child class. It is
* included here so that the child class will not have to implement it if it
* is not necessary.
*
* @param state system State
* @param aStep Step number of the integration.
*
* @return -1 on error, 0 otherwise.
*/
int Actuation::
begin(SimTK::State& s)
{
if (!proceed()) return(0);
// NUMBER OF ACTUATORS
int na = _model->getActuators().getSize();
_na = na;
// WORK ARRAY
if (_fsp != NULL) delete[] _fsp;
_fsp = new double[_na];
// RESET STORAGE
if (_forceStore == NULL)
_forceStore = new Storage();
if (_speedStore == NULL)
_speedStore = new Storage();
if (_powerStore == NULL)
_powerStore = new Storage();
// RESET STORAGE
_forceStore->reset(s.getTime());
_speedStore->reset(s.getTime());
_powerStore->reset(s.getTime());
// RECORD
int status = 0;
if (_forceStore->getSize() <= 0) {
status = record(s);
}
return(status);
}
/**
* 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
*
* This method should be overriden in the child class. It is
* included here so that the child class will not have to implement it if it
* is not necessary.
*
* @param s current system state
* @param aClientData General use pointer for sending in client data.
*
* @return -1 on error, 0 otherwise.
*/
int PointKinematics::
begin( SimTK::State& s)
{
if(!proceed()) return(0);
// RESET STORAGE
_pStore->reset(s.getTime());
_vStore->reset(s.getTime());
_aStore->reset(s.getTime());
int status = record(s);
return(status);
}
/**
* 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);
}
/**
* Record the ForceReporter quantities.
*/
int ForceReporter::record(const SimTK::State& s)
{
if(_model==NULL) return(-1);
// MAKE SURE ALL ForceReporter QUANTITIES ARE VALID
_model->getMultibodySystem().realize(s, SimTK::Stage::Dynamics );
StateVector nextRow = StateVector(s.getTime());
// Model Forces
auto forces = _model->getComponentList<Force>();
for(auto& force : forces) {
// If body force we need to record six values for torque+force
// If muscle we record one scalar
if (force.isDisabled(s)) continue;
Array<double> values = force.getRecordValues(s);
nextRow.getData().append(values);
}
if(_includeConstraintForces){
// Model Constraints
auto constraints = _model->getComponentList<Constraint>();
for (auto& constraint : constraints) {
if (constraint.isDisabled(s)) continue;
Array<double> values = constraint.getRecordValues(s);
nextRow.getData().append(values);
}
}
_forceStore.append(nextRow);
return(0);
}
/**
* initialize storages and analyses
*
* @param s system state before integration
*/
void Manager::initialize(SimTK::State& s, double dt )
{
// skip initializations for CMC's actuator system
if( _writeToStorage && _performAnalyses ) {
double tReal = s.getTime();
// STORE STARTING CONTROLS
if (_model->isControlled()){
_controllerSet->setModel(*_model);
_controllerSet->storeControls(s, 0);
}
// STORE STARTING STATES
if(hasStateStorage()) {
// ONLY IF NO STATES WERE PREVIOUSLY STORED
if(getStateStorage().getSize()==0) {
SimTK::Vector stateValues = _model->getStateVariableValues(s);
getStateStorage().store(0,tReal,stateValues.size(), &stateValues[0]);
}
}
// ANALYSES
AnalysisSet& analysisSet = _model->updAnalysisSet();
analysisSet.begin(s);
}
return;
}
//=============================================================================
// ACCELERATION
//=============================================================================
//
void StaticOptimizationTarget::
computeAcceleration(SimTK::State& s, const SimTK::Vector ¶meters,SimTK::Vector &rAccel) const
{
double time = s.getTime();
const ForceSet& fs = _model->getForceSet();
for(int i=0,j=0;i<fs.getSize();i++) {
ScalarActuator *act = dynamic_cast<ScalarActuator*>(&fs.get(i));
if( act ) {
act->setOverrideActuation(s, parameters[j] * _optimalForce[j]);
j++;
}
}
_model->getMultibodySystem().realize(s,SimTK::Stage::Acceleration);
SimTK::Vector udot = _model->getMatterSubsystem().getUDot(s);
for(int i=0; i<_accelerationIndices.getSize(); i++)
rAccel[i] = udot[_accelerationIndices[i]];
//QueryPerformanceCounter(&stop);
//double duration = (double)(stop.QuadPart-start.QuadPart)/(double)frequency.QuadPart;
//std::cout << "computeAcceleration time = " << (duration*1.0e3) << " milliseconds" << std::endl;
// 1.45 ms
}
SimTK::Vec3 calcSomething(const SimTK::State& state) const {
const_cast<Foo *>(this)->m_ctr++;
m_mutableCtr++;
double t = state.getTime();
return SimTK::Vec3(t, t*t, sqrt(t));
}
/**
* Filter the controls. This method was introduced as a means of attempting
* to reduce the sizes of residuals. Unfortunately, the approach was
* generally unsuccessful because the desired accelerations were not
* achieved.
*
* @param aControlSet Set of controls computed by CMC.
* @param aDT Current time window.
* @param rControls Array of filtered controls.
*/
void CMC::
FilterControls(const SimTK::State& s, const ControlSet &aControlSet,double aDT,
OpenSim::Array<double> &rControls,bool aVerbosePrinting)
{
if(aDT <= SimTK::Zero) {
if(aVerbosePrinting) cout<<"\nCMC.filterControls: aDT is practically 0.0, skipping!\n\n";
return;
}
if(aVerbosePrinting) cout<<"\n\nFiltering controls to limit curvature...\n";
int i;
int size = rControls.getSize();
Array<double> x0(0.0,size),x1(0.0,size),x2(0.0,size);
// SET TIMES
double t0,t1/*,t2*/;
// t2 = s.getTime();
t1 = s.getTime() - aDT;
t0 = t1 - aDT;
// SET CONTROL VALUES
x2 = rControls;
aControlSet.getControlValues(t1,x1);
aControlSet.getControlValues(t0,x0);
// LOOP OVER CONTROLS
double m1,m2;
double curvature;
double thresholdCurvature = 2.0 * 0.05 / (aDT * aDT);
//double thresholdSlopeDiff = 0.2 / aDT;
for(i=0;i<size;i++) {
m2 = (x2[i]-x1[i]) / aDT;
m1 = (x1[i]-x0[i]) / aDT;
curvature = (m2 - m1) / aDT;
curvature = fabs(curvature);
if(curvature<=thresholdCurvature) continue;
// diff = fabs(m2) - fabs(m1);
// cout<<"thresholdSlopeDiff="<<thresholdSlopeDiff<<" slopeDiff="<<diff<<endl;
// if(diff>thresholdSlopeDiff) continue;
// ALTER CONTROL VALUE
rControls[i] = (3.0*x2[i] + 2.0*x1[i] + x0[i]) / 6.0;
// PRINT
if(aVerbosePrinting) cout<<aControlSet[i].getName()<<": old="<<x2[i]<<" new="<<rControls[i]<<endl;
}
if(aVerbosePrinting) cout<<endl<<endl;
}
/**
* 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 beginning of an integration
*
* This method should be overridden in the child class. It is
* included here so that the child class will not have to implement it if it
* is not necessary.
*
* @param s System state
*
* @return -1 on error, 0 otherwise.
*/
int BodyKinematics::
begin(SimTK::State& s )
{
if(!proceed()) return(0);
// RESET STORAGE
_pStore->reset(s.getTime());
_vStore->reset(s.getTime());
_aStore->reset(s.getTime());
// RECORD
int status = 0;
if(_pStore->getSize()<=0) {
status = record(s);
}
return(status);
}
void IKSolverParallel::pushState(const SimTK::State& s) {
GeneralisedCoordinatesData currentData(nCoordinates_);
std::vector<double> q(nCoordinates_);
SimTK::Vector stateQ(s.getQ());
for (unsigned i(0); i < nCoordinates_; ++i)
q[i] = stateQ[i];
currentData.setQ(q);
outputGeneralisedCoordinatesQueue_.push({ s.getTime(), currentData });
}
// compute the control value for an actuator
void PrescribedController::computeControls(const SimTK::State& s, SimTK::Vector& controls) const
{
SimTK::Vector actControls(1, 0.0);
SimTK::Vector time(1, s.getTime());
for(int i=0; i<getActuatorSet().getSize(); i++){
actControls[0] = get_ControlFunctions()[i].calcValue(time);
getActuatorSet()[i].addInControls(actControls, controls);
}
}
void ControllerSet::storeControls( const SimTK::State& s, int step )
{
int size = _actuatorSet->getSize();
if( size > 0 )
{
_controlStore->store( step, s.getTime(), getModel().getNumControls(),
&(getModel().getControls(s)[0]) );
}
}
Array<bool> InducedAccelerationsSolver::
applyContactConstraintAccordingToExternalForces(SimTK::State &s)
{
Array<bool> constraintOn(false, _replacementConstraints.getSize());
double t = s.getTime();
for(int i=0; i<_forcesToReplace.getSize(); i++){
ExternalForce* exf = dynamic_cast<ExternalForce*>(&_forcesToReplace[i]);
SimTK::Vec3 point, force, gpoint;
force = exf->getForceAtTime(t);
// If the applied force is "significant" replace it with a constraint
if (force.norm() > _forceThreshold){
// get the point of contact from applied external force
point = exf->getPointAtTime(t);
// point should be expressed in the "applied to" body for consistency across all constraints
if(exf->getPointExpressedInBodyName() != exf->getAppliedToBodyName()){
int appliedToBodyIndex = getModel().getBodySet().getIndex(exf->getAppliedToBodyName());
if(appliedToBodyIndex < 0){
cout << "External force appliedToBody " << exf->getAppliedToBodyName() << " not found." << endl;
}
int expressedInBodyIndex = getModel().getBodySet().getIndex(exf->getPointExpressedInBodyName());
if(expressedInBodyIndex < 0){
cout << "External force expressedInBody " << exf->getPointExpressedInBodyName() << " not found." << endl;
}
const Body &appliedToBody = getModel().getBodySet().get(appliedToBodyIndex);
const Body &expressedInBody = getModel().getBodySet().get(expressedInBodyIndex);
getModel().getMultibodySystem().realize(s, SimTK::Stage::Velocity);
getModel().getSimbodyEngine().transformPosition(s, expressedInBody, point, appliedToBody, point);
}
_replacementConstraints[i].setContactPointForInducedAccelerations(s, point);
// turn on the constraint
_replacementConstraints[i].setDisabled(s, false);
// return the state of the constraint
constraintOn[i] = true;
}
else{
// turn off the constraint
_replacementConstraints[i].setDisabled(s, true);
// return the state of the constraint
constraintOn[i] = false;
}
}
return constraintOn;
}
void StaticOptimizationTarget::
printPerformance(const SimTK::State& s, double *parameters)
{
double p;
setCurrentState( &s );
objectiveFunc(SimTK::Vector(getNumParameters(),parameters,true),true,p);
SimTK::Vector constraints(getNumConstraints());
constraintFunc(SimTK::Vector(getNumParameters(),parameters,true),true,constraints);
cout << endl;
cout << "time = " << s.getTime() <<" Performance =" << p <<
" Constraint violation = " << sqrt(~constraints*constraints) << endl;
}
// Controller Interface.
// compute the control value for all actuators this Controller is responsible for
void CMC::computeControls(const SimTK::State& s, SimTK::Vector& controls) const
{
SimTK_ASSERT( _controlSet.getSize() == getActuatorSet().getSize() ,
"CMC::computeControls number of controls does not match number of actuators.");
SimTK::Vector actControls(1, 0.0);
for(int i=0; i<getActuatorSet().getSize(); i++){
actControls[0] = _controlSet[_controlSetIndices[i]].getControlValue(s.getTime());
getActuatorSet()[i].addInControls(actControls, controls);
}
// double *val = &controls[0];
}
/**
* Record the kinematics.
*
* @return 0 of success, -1 on error.
*/
int Kinematics::record(const SimTK::State& s)
{
if(_recordAccelerations){
_model->getMultibodySystem().realize(s, SimTK::Stage::Acceleration);
}
else{
_model->getMultibodySystem().realize(s, SimTK::Stage::Velocity);
}
// RECORD RESULTS
const CoordinateSet& cs = _model->getCoordinateSet();
int nvalues = _coordinateIndices.getSize();
for(int i=0;i<nvalues;i++){
_values[i] = cs[_coordinateIndices[i]].getValue(s);
if(getInDegrees() && (cs[_coordinateIndices[i]].getMotionType() == Coordinate::Rotational))
_values[i] *= SimTK_RADIAN_TO_DEGREE;
}
_pStore->append(s.getTime(),nvalues,&_values[0]);
for(int i=0;i<nvalues;i++){
_values[i] = cs[_coordinateIndices[i]].getSpeedValue(s);
if(getInDegrees() && (cs[_coordinateIndices[i]].getMotionType() == Coordinate::Rotational))
_values[i] *= SimTK_RADIAN_TO_DEGREE;
}
_vStore->append(s.getTime(),nvalues,&_values[0]);
if(_recordAccelerations) {
for(int i=0;i<nvalues;i++){
_values[i] = cs[_coordinateIndices[i]].getAccelerationValue(s);
if(getInDegrees() && (cs[_coordinateIndices[i]].getMotionType() == Coordinate::Rotational))
_values[i] *= SimTK_RADIAN_TO_DEGREE;
}
_aStore->append(s.getTime(),nvalues,&_values[0]);
}
return(0);
}
void PrescribedForce::computeForce(const SimTK::State& state,
SimTK::Vector_<SimTK::SpatialVec>& bodyForces,
SimTK::Vector& generalizedForces) const
{
const bool pointIsGlobal = get_pointIsGlobal();
const bool forceIsGlobal = get_forceIsGlobal();
const FunctionSet& forceFunctions = getForceFunctions();
const FunctionSet& pointFunctions = getPointFunctions();
const FunctionSet& torqueFunctions = getTorqueFunctions();
double time = state.getTime();
SimTK::Vector timeAsVector(1, time);
const bool hasForceFunctions = forceFunctions.getSize()==3;
const bool hasPointFunctions = pointFunctions.getSize()==3;
const bool hasTorqueFunctions = torqueFunctions.getSize()==3;
const PhysicalFrame& frame =
getConnector<PhysicalFrame>("frame").getConnectee();
const Ground& gnd = getModel().getGround();
if (hasForceFunctions) {
Vec3 force(forceFunctions[0].calcValue(timeAsVector),
forceFunctions[1].calcValue(timeAsVector),
forceFunctions[2].calcValue(timeAsVector));
if (!forceIsGlobal)
force = frame.expressVectorInAnotherFrame(state, force, gnd);
Vec3 point(0); // Default is body origin.
if (hasPointFunctions) {
// Apply force to a specified point on the body.
point = Vec3(pointFunctions[0].calcValue(timeAsVector),
pointFunctions[1].calcValue(timeAsVector),
pointFunctions[2].calcValue(timeAsVector));
if (pointIsGlobal)
point = gnd.findLocationInAnotherFrame(state, point, frame);
}
applyForceToPoint(state, frame, point, force, bodyForces);
}
if (hasTorqueFunctions){
Vec3 torque(torqueFunctions[0].calcValue(timeAsVector),
torqueFunctions[1].calcValue(timeAsVector),
torqueFunctions[2].calcValue(timeAsVector));
if (!forceIsGlobal)
torque = frame.expressVectorInAnotherFrame(state, torque, gnd);
applyTorque(state, frame, torque, bodyForces);
}
}
// WITNESS FUNCTION
SimTK::Real getValue(const SimTK::State& s) const
{
// keep returning a positive value while the contact settles out
double simTime = s.getTime();
if (simTime < settleTime) {
return 100.0;
}
// total vertical force applied to both feet
Array<double> force_foot_r = _model.getForceSet().get("foot_r").getRecordValues(s);
Array<double> force_foot_l = _model.getForceSet().get("foot_l").getRecordValues(s);
double netGRFVertical = -force_foot_r[1] - force_foot_l[1];
return netGRFVertical - _forceThreshold;
}
/**
* Record the kinematics.
*/
int PointKinematics::
record(const SimTK::State& s)
{
const SimbodyEngine& de = _model->getSimbodyEngine();
// VARIABLES
SimTK::Vec3 vec;
// POSITION
de.getPosition(s, *_body,_point,vec);
if(_relativeToBody){
de.transformPosition(s, de.getGroundBody(), vec, *_relativeToBody, vec);
}
_pStore->append(s.getTime(),vec);
// VELOCITY
de.getVelocity(s, *_body,_point,vec);
if(_relativeToBody){
de.transform(s, de.getGroundBody(), vec, *_relativeToBody, vec);
}
_vStore->append(s.getTime(),vec);
// ACCELERATIONS
_model->getMultibodySystem().realize(s, SimTK::Stage::Acceleration);
de.getAcceleration(s, *_body,_point,vec);
if(_relativeToBody){
de.transform(s, de.getGroundBody(), vec, *_relativeToBody, vec);
}
_aStore->append(s.getTime(),vec);
return(0);
}
/** get the values of the MarkersReference */
void MarkersReference::getValues(const SimTK::State &s, SimTK::Array_<Vec3> &values) const
{
double time = s.getTime();
int before=0, after=0;
// get index for time
_markerData->findFrameRange(time, time, before, after);
if(before > after || after < 0)
throw Exception("MarkersReference: No index corresponding to time of frame.");
else if(after-before > 0){
before = abs(_markerData->getFrame(before).getFrameTime()-time) < abs(_markerData->getFrame(after).getFrameTime()-time) ? before : after;
}
values = _markerData->getFrame(before).getMarkers();
}
/**
* Given SimTK::State object extract all the values necessary to report forces, application location
* frame, etc. used in conjunction with getRecordLabels and should return same size Array
*/
OpenSim::Array<double> PrescribedForce::getRecordValues(const SimTK::State& state) const {
OpenSim::Array<double> values(SimTK::NaN);
const bool pointIsGlobal = get_pointIsGlobal();
const bool forceIsGlobal = get_forceIsGlobal();
const FunctionSet& forceFunctions = getForceFunctions();
const FunctionSet& pointFunctions = getPointFunctions();
const FunctionSet& torqueFunctions = getTorqueFunctions();
const bool appliesForce = forceFunctions.getSize()==3;
const bool pointSpecified = pointFunctions.getSize()==3;
const bool appliesTorque = torqueFunctions.getSize()==3;
// This is bad as it duplicates the code in computeForce we'll cleanup after it works!
const double time = state.getTime();
const SimTK::Vector timeAsVector(1, time);
const PhysicalFrame& frame =
getConnector<PhysicalFrame>("frame").getConnectee();
const Ground& gnd = getModel().getGround();
if (appliesForce) {
Vec3 force = getForceApplied(state);
if (!forceIsGlobal)
force = frame.expressVectorInAnotherFrame(state, force, gnd);
if (!pointSpecified) {
//applyForce(*_body, force);
for (int i=0; i<3; i++) values.append(force[i]);
} else {
Vec3 point = getApplicationPoint(state);
if (pointIsGlobal)
point = gnd.findLocationInAnotherFrame(state, point, frame);
//applyForceToPoint(*_body, point, force);
for (int i=0; i<3; i++) values.append(force[i]);
for (int i=0; i<3; i++) values.append(point[i]);
}
}
if (appliesTorque) {
Vec3 torque = getTorqueApplied(state);
if (!forceIsGlobal)
torque = frame.expressVectorInAnotherFrame(state, torque, gnd);
for (int i=0; i<3; i++) values.append(torque[i]);
//applyTorque(*_body, torque);
}
return values;
};
/**
* Apply the muscle's force at its points of attachment to the bodies.
*/
void Muscle::computeForce(const SimTK::State& s,
SimTK::Vector_<SimTK::SpatialVec>& bodyForces,
SimTK::Vector& generalizedForces) const
{
Super::computeForce(s, bodyForces, generalizedForces); // Calls compute actuation.
// NOTE: Actuation could be negative, in particular during CMC, when the optimizer
// is computing gradients, but in those cases the actuation will be
// overridden and will not be computed by the muscle
if (!isActuationOverridden(s) && (getActuation(s) < -SimTK::SqrtEps)) {
string msg = getConcreteClassName()
+ "::computeForce, muscle "+ getName() + " force < 0";
cout << msg << " at time = " << s.getTime() << endl;
//throw Exception(msg);
}
}
/**
* 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 in
* Model::integBeginCallback() and has the same argument list.
*
* @param aStep Step number of the integration.
* @param aDT Size of the time step that will be attempted.
* @param aT Current time in the integration.
* @param aX Current control values.
* @param aY Current states.
* @param aYP Current pseudo states.
* @param aDYDT Current state derivatives.
*
* @return -1 on error, 0 otherwise.
*/
int AnalysisPlugin_Template::
begin(SimTK::State& s)
{
if(!proceed()) return(0);
// RESET STORAGE
_storePos.reset(s.getTime()); //->reset(s.getTime());
// RECORD
int status = 0;
if(_storePos.getSize()<=0) {
status = record(s);
}
return(status);
}
/**
* 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 reference to the current state
*
* @return -1 on error, 0 otherwise.
*/
int JointReaction::
begin(SimTK::State& s)
{
if(!proceed()) return(0);
// Read forces file here rather than during initialization
setupStorage();
// RESET STORAGE
_storeReactionLoads.reset(s.getTime());
// RECORD
int status = 0;
if(_storeReactionLoads.getSize()<=0) {
status = record(s);
}
return(status);
}
/** Obtain a model configuration that meets the assembly conditions
(desired values and constraints) given a state that satisfies or
is close to satisfying the constraints. Note there can be no change
in the number of constraints or desired coordinates. Desired
coordinate values can and should be updated between repeated calls
to track a desired trajectory of coordinate values. */
void AssemblySolver::track(SimTK::State &s)
{
// move the target locations or angles, etc... just do not change number of goals
// and their type (constrained vs. weighted)
if(_assembler && _assembler->isInitialized()){
updateGoals(s);
}
else{
throw Exception(
"AssemblySolver::track() failed: assemble() must be called first.");
}
/* TODO: Useful to include through debug message/log in the future
printf("UNASSEMBLED(track) CONFIGURATION (normerr=%g, maxerr=%g, cost=%g)\n",
_assembler->calcCurrentErrorNorm(),
max(abs(_assembler->getInternalState().getQErr())),
_assembler->calcCurrentGoal() );
cout << "Model numQs: " << _assembler->getInternalState().getNQ()
<< " Assembler num freeQs: " << _assembler->getNumFreeQs() << endl;
*/
try{
// Now do the assembly and return the updated state.
_assembler->track(s.getTime());
// update the state from the result of the assembler
_assembler->updateFromInternalState(s);
/* TODO: Useful to include through debug message/log in the future
printf("Tracking: t= %f (acc=%g tol=%g normerr=%g, maxerr=%g, cost=%g)\n",
s.getTime(),
_assembler->getAccuracyInUse(), _assembler->getErrorToleranceInUse(),
_assembler->calcCurrentErrorNorm(), max(abs(_assembler->getInternalState().getQErr())),
_assembler->calcCurrentGoal());
*/
}
catch (const std::exception& ex)
{
std::cout << "AssemblySolver::track() attempt Failed: " << ex.what() << std::endl;
throw Exception("AssemblySolver::track() attempt failed.");
}
}
/**
* Compute the controls for Actuator's that this Controller is charge of.
*
*/
void CorrectionController::computeControls(const SimTK::State& s, SimTK::Vector& controls) const
{
// NUMBER OF MODEL COORDINATES, SPEEDS, AND ACTUATORS
// (ALL ARE PROBABLY EQUAL)
int nq = _model->getNumCoordinates();
int nu = _model->getNumSpeeds();
double t = s.getTime();
// GET CURRENT DESIRED COORDINATES AND SPEEDS
// Note: yDesired[0..nq-1] will contain the generalized coordinates
// and yDesired[nq..nq+nu-1] will contain the generalized speeds.
Array<double> yDesired(0.0,nq+nu);
getDesiredStatesStorage().getDataAtTime(t, nq+nu,yDesired);
SimTK::Vector actControls(1, 0.0);
for(int i=0; i< getActuatorSet().getSize(); i++){
CoordinateActuator* act =
dynamic_cast<CoordinateActuator*>(&getActuatorSet().get(i));
SimTK_ASSERT( act, "CorrectionController::computeControls dynamic cast failed");
Coordinate *aCoord = act->getCoordinate();
if( aCoord->isConstrained(s) ) {
actControls = 0.0;
}
else
{
double qval = aCoord->getValue(s);
double uval = aCoord->getSpeedValue(s);
// COMPUTE EXCITATIONS
double oneOverFmax = 1.0 / act->getOptimalForce();
double pErr = qval - yDesired[2*i];
double vErr = uval - yDesired[2*i+1];
double pErrTerm = _kp*oneOverFmax*pErr;
double vErrTerm = _kv*oneOverFmax*vErr;
actControls = -vErrTerm - pErrTerm;
}
getActuatorSet()[i].addInControls(actControls, controls);
}
}
/**
* Compute and record the results.
*
* This method, for the purpose of example, records the position and
* orientation of each body in the model. You will need to customize it
* to perform your analysis.
*
* @param aT Current time in the simulation.
* @param aX Current values of the controls.
* @param aY Current values of the states: includes generalized coords and speeds
*/
int AnalysisPlugin_Template::
record(const SimTK::State& s)
{
// VARIABLES
double dirCos[3][3];
SimTK::Vec3 vec,angVec;
double Mass = 0.0;
// GROUND BODY
const Body& ground = _model->getGroundBody();
// POSITION
const BodySet& bodySet = _model->getBodySet();
for(int i=0;i<_bodyIndices.getSize();i++) {
const Body& body = bodySet.get(_bodyIndices[i]);
SimTK::Vec3 com = body.getMassCenter();
// GET POSITIONS AND EULER ANGLES
_model->getSimbodyEngine().getPosition(s,body,com,vec);
_model->getSimbodyEngine().getDirectionCosines(s,body,dirCos);
_model->getSimbodyEngine().convertDirectionCosinesToAngles(dirCos,
&angVec[0],&angVec[1],&angVec[2]);
// CONVERT TO DEGREES?
if(getInDegrees()) {
angVec *= SimTK_RADIAN_TO_DEGREE;
}
// FILL KINEMATICS ARRAY
int I=6*i;
memcpy(&_bodypos[I],&vec[0],3*sizeof(double));
memcpy(&_bodypos[I+3],&angVec[0],3*sizeof(double));
}
_storePos.append(s.getTime(),_bodypos.getSize(),&_bodypos[0]);
// VELOCITY
// ACCELERATIONS
return(0);
}
/**
* 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 in
* Model::integBeginCallback() and has the same argument list.
*
* @param s SimTK:State
*
* @return -1 on error, 0 otherwise.
*/
int InducedAccelerations::begin(SimTK::State &s)
{
if(!proceed()) return(0);
initialize(s);
// RESET STORAGES
for(int i = 0; i<_storeInducedAccelerations.getSize(); i++){
_storeInducedAccelerations[i]->reset(s.getTime());
}
cout << "Performing Induced Accelerations Analysis" << endl;
// RECORD
int status = 0;
status = record(s);
return(status);
}
/**
* This method is called at the beginning of an analysis so that any
* necessary initializations may be performed.
*
*
* This method should be overridden in the child class. It is
* included here so that the child class will not have to implement it if it
* is not necessary.
*
* @param state system State
* @param aStep Step number of the integration.
*
* @return -1 on error, 0 otherwise.
*/
int ForceReporter::
begin(SimTK::State& s)
{
if(!proceed()) return(0);
tidyForceNames();
// LABELS
constructColumnLabels(s);
// RESET STORAGE
_forceStore.reset(s.getTime());
// RECORD
int status = 0;
if(_forceStore.getSize()<=0) {
status = record(s);
}
return(status);
}
/**
* Record the ForceReporter quantities.
*/
int ForceReporter::record(const SimTK::State& s)
{
if(_model==NULL) return(-1);
// MAKE SURE ALL ForceReporter QUANTITIES ARE VALID
_model->getMultibodySystem().realize(s, SimTK::Stage::Dynamics );
StateVector nextRow = StateVector(s.getTime());
// NUMBER OF Forces
const ForceSet& forces = _model->getForceSet(); // This does not contain gravity
int nf = forces.getSize();
for(int i=0;i<nf;i++) {
// If body force we need to record six values for torque+force
// If muscle we record one scalar
OpenSim::Force& nextForce = (OpenSim::Force&)forces[i];
if (nextForce.isDisabled(s)) continue;
Array<double> values = nextForce.getRecordValues(s);
nextRow.getData().append(values);
}
if(_includeConstraintForces){
// NUMBER OF Constraints
const ConstraintSet& constraints = _model->getConstraintSet(); // This does not contain gravity
int nc = constraints.getSize();
for(int i=0;i<nc;i++) {
OpenSim::Constraint& nextConstraint = (OpenSim::Constraint&)constraints[i];
if (nextConstraint.isDisabled(s)) continue;
Array<double> values = nextConstraint.getRecordValues(s);
nextRow.getData().append(values);
}
}
_forceStore.append(nextRow);
return(0);
}