/**
* Compute all constraints given x.
*/
void ActuatorForceTargetFast::
computeConstraintVector(SimTK::State& s, const Vector &x,Vector &c) const
{
CMC_TaskSet& taskSet = _controller->updTaskSet();
const Set<Actuator>& fSet = _controller->getActuatorSet();
int nf = fSet.getSize();
// Now override the actuator forces with computed active force
// (from static optimization) but also include the passive force
// contribution of muscles when applying forces to the model
for(int i=0;i<nf;i++) {
ScalarActuator* act = dynamic_cast<ScalarActuator*>(&fSet[i]);
act->overrideActuation(s, true);
act->setOverrideActuation(s, x[i]);
}
_controller->getModel().getMultibodySystem().realize(s, SimTK::Stage::Acceleration );
taskSet.computeAccelerations(s);
Array<double> &w = taskSet.getWeights();
Array<double> &aDes = taskSet.getDesiredAccelerations();
Array<double> &a = taskSet.getAccelerations();
// CONSTRAINTS
for(int i=0; i<getNumConstraints(); i++)
c[i]=w[i]*(aDes[i]-a[i]);
// reset the actuator control
for(int i=0;i<fSet.getSize();i++) {
ScalarActuator* act = dynamic_cast<ScalarActuator*>(&fSet[i]);
act->overrideActuation(s, false);
}
_controller->getModel().getMultibodySystem().realizeModel(s);
}
//=============================================================================
// 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
}
//=============================================================================
// ANALYSIS
//=============================================================================
//
void InverseDynamics::
computeAcceleration(SimTK::State& s, double *aF,double *rAccel) const
{
for(int i=0,j=0; i<_forceSet->getSize(); i++) {
ScalarActuator* act = dynamic_cast<ScalarActuator*>(&_forceSet->get(i));
if( act ) {
act->setOverrideActuation(s, aF[j++]);
}
}
// NEED TO APPLY OTHER FORCES (e.g. Prescribed) FROM ORIGINAL MODEL
_modelWorkingCopy->getMultibodySystem().realize(s,SimTK::Stage::Acceleration);
SimTK::Vector udot = _modelWorkingCopy->getMatterSubsystem().getUDot(s);
for(int i=0; i<_accelerationIndices.getSize(); i++)
rAccel[i] = udot[_accelerationIndices[i]];
}
void ActuatorForceTarget::
computePerformanceVectors(SimTK::State& s, const Vector &aF, Vector &rAccelPerformanceVector, Vector &rForcePerformanceVector)
{
const Set<Actuator> &fSet = _controller->getActuatorSet();
for(int i=0;i<fSet.getSize();i++) {
ScalarActuator* act = dynamic_cast<ScalarActuator*>(&fSet[i]);
act->setOverrideActuation(s, aF[i]);
act->overrideActuation(s, true);
}
_controller->getModel().getMultibodySystem().realize(s, SimTK::Stage::Acceleration );
CMC_TaskSet& taskSet = _controller->updTaskSet();
taskSet.computeAccelerations(s);
Array<double> &w = taskSet.getWeights();
Array<double> &aDes = taskSet.getDesiredAccelerations();
Array<double> &a = taskSet.getAccelerations();
// PERFORMANCE
double sqrtStressTermWeight = sqrt(_stressTermWeight);
for(int i=0;i<fSet.getSize();i++) {
ScalarActuator* act = dynamic_cast<ScalarActuator*>(&fSet[i]);
rForcePerformanceVector[i] = sqrtStressTermWeight * act->getStress(s);
}
int nacc = aDes.getSize();
for(int i=0;i<nacc;i++) rAccelPerformanceVector[i] = sqrt(w[i]) * (a[i] - aDes[i]);
// reset the actuator control
for(int i=0;i<fSet.getSize();i++) {
ScalarActuator* act = dynamic_cast<ScalarActuator*>(&fSet[i]);
act->overrideActuation(s, false);
}
}
/* Solve for the induced accelerations (udot_f) for a Force in the model
identified by its name. */
const SimTK::Vector& InducedAccelerationsSolver::solve(const SimTK::State& s,
const string& forceName,
bool computeActuatorPotentialOnly,
SimTK::Vector_<SimTK::SpatialVec>* constraintReactions)
{
int nu = _modelCopy.getNumSpeeds();
double aT = s.getTime();
SimTK::State& s_solver = _modelCopy.updWorkingState();
//_modelCopy.initStateWithoutRecreatingSystem(s_solver);
// Just need to set current time and kinematics to determine state of constraints
s_solver.setTime(aT);
s_solver.updQ()=s.getQ();
s_solver.updU()=s.getU();
// Check the external forces and determine if contact constraints should be applied at this time
// and turn constraint on if it should be.
Array<bool> constraintOn = applyContactConstraintAccordingToExternalForces(s_solver);
// Hang on to a state that has the right flags for contact constraints turned on/off
_modelCopy.setPropertiesFromState(s_solver);
// Use this state for the remainder of this step (record)
s_solver = _modelCopy.getMultibodySystem().realizeTopology();
// DO NOT recreate the system, will lose location of constraint
_modelCopy.initStateWithoutRecreatingSystem(s_solver);
//cout << "Solving for contributor: " << _contributors[c] << endl;
// Need to be at the dynamics stage to disable a force
s_solver.setTime(aT);
_modelCopy.getMultibodySystem().realize(s_solver, SimTK::Stage::Dynamics);
if(forceName == "total"){
// Set gravity ON
_modelCopy.getGravityForce().enable(s_solver);
//Use same conditions on constraints
s_solver.updU() = s.getU();
s_solver.updU() = s.getZ();
//Make sure all the actuators are on!
for(int f=0; f<_modelCopy.getActuators().getSize(); f++){
_modelCopy.updActuators().get(f).setDisabled(s_solver, false);
}
// Get to the point where we can evaluate unilateral constraint conditions
_modelCopy.getMultibodySystem().realize(s_solver, SimTK::Stage::Acceleration);
/* *********************************** ERROR CHECKING *******************************
SimTK::Vec3 pcom =_modelCopy.getMultibodySystem().getMatterSubsystem().calcSystemMassCenterLocationInGround(s_solver);
SimTK::Vec3 vcom =_modelCopy.getMultibodySystem().getMatterSubsystem().calcSystemMassCenterVelocityInGround(s_solver);
SimTK::Vec3 acom =_modelCopy.getMultibodySystem().getMatterSubsystem().calcSystemMassCenterAccelerationInGround(s_solver);
SimTK::Matrix M;
_modelCopy.getMultibodySystem().getMatterSubsystem().calcM(s_solver, M);
cout << "mass matrix: " << M << endl;
SimTK::Inertia sysInertia = _modelCopy.getMultibodySystem().getMatterSubsystem().calcSystemCentralInertiaInGround(s_solver);
cout << "system inertia: " << sysInertia << endl;
SimTK::SpatialVec sysMomentum =_modelCopy.getMultibodySystem().getMatterSubsystem().calcSystemMomentumAboutGroundOrigin(s_solver);
cout << "system momentum: " << sysMomentum << endl;
const SimTK::Vector &appliedMobilityForces = _modelCopy.getMultibodySystem().getMobilityForces(s_solver, SimTK::Stage::Dynamics);
appliedMobilityForces.dump("All Applied Mobility Forces");
// Get all applied body forces like those from contact
const SimTK::Vector_<SimTK::SpatialVec>& appliedBodyForces = _modelCopy.getMultibodySystem().getRigidBodyForces(s_solver, SimTK::Stage::Dynamics);
appliedBodyForces.dump("All Applied Body Forces");
SimTK::Vector ucUdot;
SimTK::Vector_<SimTK::SpatialVec> ucA_GB;
_modelCopy.getMultibodySystem().getMatterSubsystem().calcAccelerationIgnoringConstraints(s_solver, appliedMobilityForces, appliedBodyForces, ucUdot, ucA_GB) ;
ucUdot.dump("Udots Ignoring Constraints");
ucA_GB.dump("Body Accelerations");
SimTK::Vector_<SimTK::SpatialVec> constraintBodyForces(_constraintSet.getSize(), SimTK::SpatialVec(SimTK::Vec3(0)));
SimTK::Vector constraintMobilityForces(0);
int nc = _modelCopy.getMultibodySystem().getMatterSubsystem().getNumConstraints();
for (SimTK::ConstraintIndex cx(0); cx < nc; ++cx) {
if (!_modelCopy.getMultibodySystem().getMatterSubsystem().isConstraintDisabled(s_solver, cx)){
cout << "Constraint " << cx << " enabled!" << endl;
}
}
//int nMults = _modelCopy.getMultibodySystem().getMatterSubsystem().getTotalMultAlloc();
for(int i=0; i<constraintOn.getSize(); i++) {
if(constraintOn[i])
_constraintSet[i].calcConstraintForces(s_solver, constraintBodyForces, constraintMobilityForces);
}
constraintBodyForces.dump("Constraint Body Forces");
constraintMobilityForces.dump("Constraint Mobility Forces");
// ******************************* end ERROR CHECKING *******************************/
for(int i=0; i<constraintOn.getSize(); i++) {
_replacementConstraints[i].setDisabled(s_solver, !constraintOn[i]);
// Make sure we stay at Dynamics so each constraint can evaluate its conditions
_modelCopy.getMultibodySystem().realize(s_solver, SimTK::Stage::Acceleration);
}
// This should also push changes to defaults for unilateral conditions
_modelCopy.setPropertiesFromState(s_solver);
}
else if(forceName == "gravity"){
// Set gravity ON
_modelCopy.updForceSubsystem().setForceIsDisabled(s_solver, _modelCopy.getGravityForce().getForceIndex(), false);
// zero velocity
s_solver.setU(SimTK::Vector(nu,0.0));
// disable other forces
for(int f=0; f<_modelCopy.getForceSet().getSize(); f++){
_modelCopy.updForceSet()[f].setDisabled(s_solver, true);
}
}
else if(forceName == "velocity"){
// Set gravity off
_modelCopy.updForceSubsystem().setForceIsDisabled(s_solver, _modelCopy.getGravityForce().getForceIndex(), true);
// non-zero velocity
s_solver.updU() = s.getU();
// zero actuator forces
for(int f=0; f<_modelCopy.getActuators().getSize(); f++){
_modelCopy.updActuators().get(f).setDisabled(s_solver, true);
}
// Set the configuration (gen. coords and speeds) of the model.
_modelCopy.getMultibodySystem().realize(s_solver, SimTK::Stage::Velocity);
}
else{ //The rest are actuators
// Set gravity OFF
_modelCopy.updForceSubsystem().setForceIsDisabled(s_solver, _modelCopy.getGravityForce().getForceIndex(), true);
// zero actuator forces
for(int f=0; f<_modelCopy.getActuators().getSize(); f++){
_modelCopy.updActuators().get(f).setDisabled(s_solver, true);
}
// zero velocity
SimTK::Vector U(nu,0.0);
s_solver.setU(U);
s_solver.updZ() = s.getZ();
// light up the one Force who's contribution we are looking for
int ai = _modelCopy.getForceSet().getIndex(forceName);
if(ai<0){
cout << "Force '"<< forceName << "' not found in model '" <<
_modelCopy.getName() << "'." << endl;
}
Force &force = _modelCopy.getForceSet().get(ai);
force.setDisabled(s_solver, false);
ScalarActuator *actuator = dynamic_cast<ScalarActuator*>(&force);
if(actuator){
if(computeActuatorPotentialOnly){
actuator->overrideActuation(s_solver, true);
actuator->setOverrideActuation(s_solver, 1.0);
}
}
// Set the configuration (gen. coords and speeds) of the model.
_modelCopy.getMultibodySystem().realize(s_solver, SimTK::Stage::Model);
_modelCopy.getMultibodySystem().realize(s_solver, SimTK::Stage::Velocity);
}// End of if to select contributor
// cout << "Constraint 0 is of "<< _constraintSet[0].getConcreteClassName() << " and should be " << constraintOn[0] << " and is actually " << (_constraintSet[0].isDisabled(s_solver) ? "off" : "on") << endl;
// cout << "Constraint 1 is of "<< _constraintSet[1].getConcreteClassName() << " and should be " << constraintOn[1] << " and is actually " << (_constraintSet[1].isDisabled(s_solver) ? "off" : "on") << endl;
// After setting the state of the model and applying forces
// Compute the derivative of the multibody system (speeds and accelerations)
_modelCopy.getMultibodySystem().realize(s_solver, SimTK::Stage::Acceleration);
// Sanity check that constraints hasn't totally changed the configuration of the model
// double error = (s.getQ()-s_solver.getQ()).norm();
// Report reaction forces for debugging
/*
SimTK::Vector_<SimTK::SpatialVec> constraintBodyForces(_constraintSet.getSize());
SimTK::Vector mobilityForces(0);
for(int i=0; i<constraintOn.getSize(); i++) {
if(constraintOn[i])
_constraintSet.get(i).calcConstraintForces(s_solver, constraintBodyForces, mobilityForces);
}*/
return s_solver.getUDot();
}
