// 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()