// for adding any components to the model
void CMC::extendAddToSystem( SimTK::MultibodySystem& system) const
{
Super::extendAddToSystem(system);
// add event handler for updating controls for next window
CMC* mutableThis = const_cast<CMC *>(this);
ComputeControlsEventHandler* computeControlsHandler =
new ComputeControlsEventHandler(mutableThis);
system.updDefaultSubsystem().addEventHandler(computeControlsHandler );
const Set<Actuator>& fSet = getActuatorSet();
int nActs = fSet.getSize();
mutableThis->_controlSetIndices.setSize(nActs);
// Create the control set that will hold the controls computed by CMC
mutableThis->_controlSet.setName(_model->getName());
mutableThis->_controlSet.setSize(0);
// Define the control set used to specify control bounds and to hold
// the computed control values from the CMC algorithm
double xmin =0, xmax=0;
std::string actName = "";
for(int i=0; i < nActs; ++i ) {
ScalarActuator* act = dynamic_cast<ScalarActuator*>(&fSet[i]);
//Actuator& act = getActuatorSet().get(i);
ControlLinear *control = new ControlLinear();
control->setName(act->getName() + ".excitation" );
xmin = act->getMinControl();
if (xmin ==-SimTK::Infinity)
xmin =-MAX_CONTROLS_FOR_RRA;
xmax = act->getMaxControl();
if (xmax ==SimTK::Infinity)
xmax =MAX_CONTROLS_FOR_RRA;
Muscle *musc = dynamic_cast<Muscle *>(act);
// if controlling muscles, CMC requires that the control be constant (i.e. piecewise constant or use steps)
// since it uses this assumption to rootsolve for the required controls over the CMC time-window.
if(musc){
control->setUseSteps(true);
if(xmin < MIN_CMC_CONTROL_VALUE){
cout << "CMC::Warning: CMC cannot compute controls for muscles with muscle controls < " << MIN_CMC_CONTROL_VALUE <<".\n" <<
"The minimum control limit for muscle '" << musc->getName() << "' has been reset to " << MIN_CMC_CONTROL_VALUE <<"." <<endl;
xmin = MIN_CMC_CONTROL_VALUE;
}
if(xmax < MAX_CMC_CONTROL_VALUE){
cout << "CMC::Warning: CMC cannot compute controls for muscles with muscle controls > " << MAX_CMC_CONTROL_VALUE <<".\n" <<
"The maximum control limit for muscle '" << musc->getName() << "' has been reset to " << MAX_CMC_CONTROL_VALUE << "." << endl;
xmax = MAX_CMC_CONTROL_VALUE;
}
}
control->setDefaultParameterMin(xmin);
control->setDefaultParameterMax(xmax);
mutableThis->_controlSet.adoptAndAppend(control);
mutableThis->_controlSetIndices.set(i, i);
}
mutableThis->setNumControls(_controlSet.getSize());
}
//==========================================================================================================
void testControlSetControllerOnBlock()
{
using namespace SimTK;
// Create a new OpenSim model
Model osimModel;
osimModel.setName("osimModel");
// Get the ground body
OpenSim::Body& ground = osimModel.getGroundBody();
// Create a 20 kg, 0.1 m^3 block body
double blockMass = 20.0, blockSideLength = 0.1;
Vec3 blockMassCenter(0), groundOrigin(0), blockInGround(0, blockSideLength/2, 0);
Inertia blockIntertia = Inertia::brick(blockSideLength, blockSideLength, blockSideLength);
OpenSim::Body block("block", blockMass, blockMassCenter, blockMass*blockIntertia);
//Create a free joint with 6 degrees-of-freedom
SimTK::Vec3 noRotation(0);
SliderJoint blockToGround("",ground, blockInGround, noRotation, block, blockMassCenter, noRotation);
// Create 6 coordinates (degrees-of-freedom) between the ground and block
CoordinateSet& jointCoordinateSet = blockToGround.upd_CoordinateSet();
double posRange[2] = {-1, 1};
jointCoordinateSet[0].setName("xTranslation");
jointCoordinateSet[0].setMotionType(Coordinate::Translational);
jointCoordinateSet[0].setRange(posRange);
// Add the block body to the model
osimModel.addBody(&block);
// Define a single coordinate actuator.
CoordinateActuator actuator(jointCoordinateSet[0].getName());
actuator.setName("actuator");
// Add the actuator to the model
osimModel.addForce(&actuator);
double initialTime = 0;
double finalTime = 1.0;
// Define the initial and final control values
double controlForce[1] = {100};
// Create two control signals
ControlLinear control;
control.setName("actuator");
// Create a control set and add the controls to the set
ControlSet actuatorControls;
actuatorControls.adoptAndAppend(&control);
actuatorControls.setMemoryOwner(false);
actuatorControls.setControlValues(initialTime, controlForce);
actuatorControls.setControlValues(finalTime, controlForce);
// Create a control set controller that simply applies controls from a ControlSet
ControlSetController actuatorController;
// Make a copy and set it on the ControlSetController as it takes ownership of the
// ControlSet passed in
actuatorController.setControlSet((ControlSet*)Object::SafeCopy(&actuatorControls));
// add the controller to the model
osimModel.addController(&actuatorController);
// Initialize the system and get the state representing the state system
SimTK::State& si = osimModel.initSystem();
// Specify zero slider joint kinematic states
CoordinateSet &coordinates = osimModel.updCoordinateSet();
coordinates[0].setValue(si, 0.0); // x translation
coordinates[0].setSpeedValue(si, 0.0); // x speed
// Create the integrator and manager for the simulation.
double accuracy = 1.0e-3;
SimTK::RungeKuttaMersonIntegrator integrator(osimModel.getMultibodySystem());
integrator.setAccuracy(accuracy);
Manager manager(osimModel, integrator);
// Integrate from initial time to final time
manager.setInitialTime(initialTime);
manager.setFinalTime(finalTime);
std::cout<<"\n\nIntegrating from "<<initialTime<<" to "<<finalTime<<std::endl;
manager.integrate(si);
si.getQ().dump("Final position:");
double x_err = fabs(coordinates[0].getValue(si) - 0.5*(controlForce[0]/blockMass)*finalTime*finalTime);
ASSERT(x_err <= accuracy, __FILE__, __LINE__, "ControlSetControllerOnBlock failed to produce the expected motion.");
// Save the simulation results
Storage states(manager.getStateStorage());
states.print("block_push.sto");
osimModel.disownAllComponents();
}// end of testControlSetControllerOnBlock()
