bool SimulationModel::addSliderJoint(const unsigned int rbIndex1, const unsigned int rbIndex2, const Eigen::Vector3f &pos, const Eigen::Vector3f &axis)
{
SliderJoint *joint = new SliderJoint();
const bool res = joint->initConstraint(*this, rbIndex1, rbIndex2, pos, axis);
if (res)
{
m_constraints.push_back(joint);
m_groupsInitialized = false;
}
return res;
}
/* ....................................................................... */
void
SliderServoMotor::operator()(ODEObject* object)
{
SliderJoint* slider = object->asSliderJoint() ;
PS_ASSERT1( slider != NULL ) ;
World* world = object->getWorld() ;
PS_ASSERT1( world != NULL ) ;
PIDController* pid = getPIDController() ;
ooReal vel = 0.0 ;
if( pid ) {
vel = pid->solve( m_position - slider->getPosition(), world->getCurrentStepSize() ) ;
} else {
vel = m_position - slider->getPosition() ;
}
if( m_max_vel >= 0.0 ) {
vel = osg::clampTo( vel, -m_max_vel, m_max_vel ) ;
}
slider->setParam( dParamFMax, m_force ) ;
slider->setParam( dParamVel, vel ) ;
traverse(object) ;
}
//=============================================================================
// Simbody Model building.
//=============================================================================
//_____________________________________________________________________________
void SliderJoint::addToSystem(SimTK::MultibodySystem& system) const
{
const SimTK::Vec3& orientation = get_orientation();
const SimTK::Vec3& location = get_location();
// CHILD TRANSFORM
Rotation rotation(BodyRotationSequence, orientation[0],XAxis, orientation[1],YAxis, orientation[2],ZAxis);
SimTK::Transform childTransform(rotation, location);
const SimTK::Vec3& orientationInParent = get_orientation_in_parent();
const SimTK::Vec3& locationInParent = get_location_in_parent();
// PARENT TRANSFORM
Rotation parentRotation(BodyRotationSequence, orientationInParent[0],XAxis, orientationInParent[1],YAxis, orientationInParent[2],ZAxis);
SimTK::Transform parentTransform(parentRotation, locationInParent);
SliderJoint* mutableThis = const_cast<SliderJoint*>(this);
mutableThis->createMobilizedBody(parentTransform, childTransform);
// TODO: Joints require super class to be called last.
Super::addToSystem(system);
}
void testMcKibbenActuator()
{
double pressure = 5 * 10e5; // 5 bars
double num_turns = 1.5; // 1.5 turns
double B = 277.1 * 10e-4; // 277.1 mm
using namespace SimTK;
std::clock_t startTime = std::clock();
double mass = 1;
double ball_radius = 10e-6;
Model *model = new Model;
model->setGravity(Vec3(0));
Ground& ground = model->updGround();
McKibbenActuator *actuator = new McKibbenActuator("mckibben", num_turns, B);
OpenSim::Body* ball = new OpenSim::Body("ball", mass ,Vec3(0), mass*SimTK::Inertia::sphere(0.1));
ball->scale(Vec3(ball_radius), false);
actuator->addNewPathPoint("mck_ground", ground, Vec3(0));
actuator->addNewPathPoint("mck_ball", *ball, Vec3(ball_radius));
Vec3 locationInParent(0, ball_radius, 0), orientationInParent(0), locationInBody(0), orientationInBody(0);
SliderJoint *ballToGround = new SliderJoint("ballToGround", ground, locationInParent, orientationInParent, *ball, locationInBody, orientationInBody);
ballToGround->updCoordinate().setName("ball_d");
ballToGround->updCoordinate().setPrescribedFunction(LinearFunction(20 * 10e-4, 0.5 * 264.1 * 10e-4));
ballToGround->updCoordinate().set_prescribed(true);
model->addBody(ball);
model->addJoint(ballToGround);
model->addForce(actuator);
PrescribedController* controller = new PrescribedController();
controller->addActuator(*actuator);
controller->prescribeControlForActuator("mckibben", new Constant(pressure));
model->addController(controller);
ForceReporter* reporter = new ForceReporter(model);
model->addAnalysis(reporter);
SimTK::State& si = model->initSystem();
model->getMultibodySystem().realize(si, Stage::Position);
double final_t = 10.0;
double nsteps = 10;
double dt = final_t / nsteps;
RungeKuttaMersonIntegrator integrator(model->getMultibodySystem());
integrator.setAccuracy(1e-7);
Manager manager(*model, integrator);
manager.setInitialTime(0.0);
for (int i = 1; i <= nsteps; i++){
manager.setFinalTime(dt*i);
manager.integrate(si);
model->getMultibodySystem().realize(si, Stage::Velocity);
Vec3 pos;
model->updSimbodyEngine().getPosition(si, *ball, Vec3(0), pos);
double applied = actuator->computeActuation(si);;
double theoretical = (pressure / (4* pow(num_turns,2) * SimTK::Pi)) * (3*pow(pos(0), 2) - pow(B, 2));
ASSERT_EQUAL(applied, theoretical, 10.0);
manager.setInitialTime(dt*i);
}
std::cout << " ******** Test McKibbenActuator time = ********" <<
1.e3*(std::clock() - startTime) / CLOCKS_PER_SEC << "ms\n" << endl;
}
void testClutchedPathSpring()
{
using namespace SimTK;
// start timing
std::clock_t startTime = std::clock();
double mass = 1;
double stiffness = 100;
double dissipation = 0.3;
double start_h = 0.5;
//double ball_radius = 0.25;
//double omega = sqrt(stiffness/mass);
// Setup OpenSim model
Model* model = new Model;
model->setName("ClutchedPathSpringModel");
model->setGravity(gravity_vec);
//OpenSim bodies
const Ground* ground = &model->getGround();
// body that acts as the pulley that the path wraps over
OpenSim::Body* pulleyBody =
new OpenSim::Body("PulleyBody", mass ,Vec3(0), mass*Inertia::brick(0.1, 0.1, 0.1));
// body the path spring is connected to at both ends
OpenSim::Body* block =
new OpenSim::Body("block", mass ,Vec3(0), mass*Inertia::brick(0.2, 0.1, 0.1));
block->attachGeometry(new Brick(Vec3(0.2, 0.1, 0.1)));
block->scale(Vec3(0.2, 0.1, 0.1), false);
//double dh = mass*gravity_vec(1)/stiffness;
WrapCylinder* pulley = new WrapCylinder();
pulley->set_radius(0.1);
pulley->set_length(0.05);
// Add the wrap object to the body, which takes ownership of it
pulleyBody->addWrapObject(pulley);
// Add joints
WeldJoint* weld =
new WeldJoint("weld", *ground, Vec3(0, 1.0, 0), Vec3(0), *pulleyBody, Vec3(0), Vec3(0));
SliderJoint* slider =
new SliderJoint("slider", *ground, Vec3(0), Vec3(0,0,Pi/2),*block, Vec3(0), Vec3(0,0,Pi/2));
double positionRange[2] = {-10, 10};
// Rename coordinates for a slider joint
slider->updCoordinate().setName("block_h");
slider->updCoordinate().setRange(positionRange);
model->addBody(pulleyBody);
model->addJoint(weld);
model->addBody(block);
model->addJoint(slider);
ClutchedPathSpring* spring =
new ClutchedPathSpring("clutch_spring", stiffness, dissipation, 0.01);
spring->updGeometryPath().appendNewPathPoint("origin", *block, Vec3(-0.1, 0.0 ,0.0));
int N = 10;
for(int i=1; i<N; ++i){
double angle = i*Pi/N;
double x = 0.1*cos(angle);
double y = 0.1*sin(angle);
spring->updGeometryPath().appendNewPathPoint("", *pulleyBody, Vec3(-x, y ,0.0));
}
spring->updGeometryPath().appendNewPathPoint("insertion", *block, Vec3(0.1, 0.0 ,0.0));
// BUG in defining wrapping API requires that the Force containing the GeometryPath be
// connected to the model before the wrap can be added
model->addForce(spring);
PrescribedController* controller = new PrescribedController();
controller->addActuator(*spring);
// Control greater than 1 or less than 0 should be treated as 1 and 0 respectively.
double timePts[4] = {0.0, 5.0, 6.0, 10.0};
double clutchOnPts[4] = {1.5, -2.0, 0.5, 0.5};
PiecewiseConstantFunction* controlfunc =
new PiecewiseConstantFunction(4, timePts, clutchOnPts);
controller->prescribeControlForActuator("clutch_spring", controlfunc);
model->addController(controller);
model->print("ClutchedPathSpringModel.osim");
//Test deserialization
delete model;
model = new Model("ClutchedPathSpringModel.osim");
// Create the force reporter
ForceReporter* reporter = new ForceReporter(model);
model->addAnalysis(reporter);
model->setUseVisualizer(false);
SimTK::State& state = model->initSystem();
CoordinateSet& coords = model->updCoordinateSet();
coords[0].setValue(state, start_h);
model->getMultibodySystem().realize(state, Stage::Position );
//==========================================================================
// Compute the force and torque at the specified times.
RungeKuttaMersonIntegrator integrator(model->getMultibodySystem() );
integrator.setAccuracy(integ_accuracy);
Manager manager(*model, integrator);
manager.setWriteToStorage(true);
manager.setInitialTime(0.0);
double final_t = 4.99999;
manager.setFinalTime(final_t);
manager.integrate(state);
// tension is dynamics dependent because controls must be computed
model->getMultibodySystem().realize(state, Stage::Dynamics);
spring = dynamic_cast<ClutchedPathSpring*>(
&model->updForceSet().get("clutch_spring"));
// Now check that the force reported by spring
double model_force = spring->getTension(state);
double stretch0 = spring->getStretch(state);
// the tension should be half the weight of the block
double analytical_force = -0.5*(gravity_vec(1))*mass;
cout << "Tension is: " << model_force << " and should be: " << analytical_force << endl;
// error if the block does not reach equilibrium since spring is clamped
ASSERT_EQUAL(model_force, analytical_force, 10*integ_accuracy);
// unclamp and continue integrating
manager.setInitialTime(final_t);
final_t = 5.99999;
manager.setFinalTime(final_t);
manager.integrate(state);
// tension is dynamics dependent because controls must be computed
model->getMultibodySystem().realize(state, Stage::Dynamics);
// tension should go to zero quickly
model_force = spring->getTension(state);
cout << "Tension is: " << model_force << " and should be: 0.0" << endl;
// is unclamped and block should be in free-fall
ASSERT_EQUAL(model_force, 0.0, 10*integ_accuracy);
// spring is reclamped at 7s so keep integrating
manager.setInitialTime(final_t);
final_t = 10.0;
manager.setFinalTime(final_t);
manager.integrate(state);
// tension is dynamics dependent because controls must be computed
model->getMultibodySystem().realize(state, Stage::Dynamics);
// tension should build to support the block again
model_force = spring->getTension(state);
double stretch1 = spring->getStretch(state);
cout << "Tension is: " << model_force << " and should be: "<< analytical_force << endl;
// is unclamped and block should be in free-fall
ASSERT_EQUAL(model_force, analytical_force, 10*integ_accuracy);
cout << "Steady stretch at control = 1.0 is " << stretch0 << " m." << endl;
cout << "Steady stretch at control = 0.5 is " << stretch1 << " m." << endl;
ASSERT_EQUAL(2*stretch0, stretch1, 10*integ_accuracy);
manager.getStateStorage().print("clutched_path_spring_states.sto");
model->getControllerSet().printControlStorage("clutched_path_spring_controls.sto");
// Save the forces
reporter->getForceStorage().print("clutched_path_spring_forces.mot");
model->disownAllComponents();
cout << " ********** Test clutched spring time = ********** " <<
1.e3*(std::clock()-startTime)/CLOCKS_PER_SEC << "ms\n" << endl;
}
/*==============================================================================
Main test driver to be used on any muscle model (derived from Muscle) so new
cases should be easy to add currently, the test only verifies that the work
done by the muscle corresponds to the change in system energy.
TODO: Test will fail wih prescribe motion until the work done by this
constraint is accounted for.
================================================================================
*/
void simulateMuscle(
const Muscle &aMuscModel,
double startX,
double act0,
const Function *motion, // prescribe motion of free end of muscle
const Function *control, // prescribed excitation signal to the muscle
double integrationAccuracy,
int testType,
double testTolerance,
bool printResults)
{
string prescribed = (motion == NULL) ? "." : " with Prescribed Motion.";
cout << "\n******************************************************" << endl;
cout << "Test " << aMuscModel.getConcreteClassName()
<< " Model" << prescribed << endl;
cout << "******************************************************" << endl;
using SimTK::Vec3;
//==========================================================================
// 0. SIMULATION SETUP: Create the block and ground
//==========================================================================
// Define the initial and final simulation times
double initialTime = 0.0;
double finalTime = 4.0;
//Physical properties of the model
double ballMass = 10;
double ballRadius = 0.05;
double anchorWidth = 0.1;
// Create an OpenSim model
Model model;
double optimalFiberLength = aMuscModel.getOptimalFiberLength();
double pennationAngle = aMuscModel.getPennationAngleAtOptimalFiberLength();
double tendonSlackLength = aMuscModel.getTendonSlackLength();
// Use a copy of the muscle model passed in to add path points later
PathActuator *aMuscle = aMuscModel.clone();
// Get a reference to the model's ground body
Body& ground = model.getGroundBody();
ground.addDisplayGeometry("box.vtp");
ground.updDisplayer()
->setScaleFactors(Vec3(anchorWidth, anchorWidth, 2*anchorWidth));
OpenSim::Body * ball = new OpenSim::Body("ball",
ballMass ,
Vec3(0),
ballMass*SimTK::Inertia::sphere(ballRadius));
ball->addDisplayGeometry("sphere.vtp");
ball->updDisplayer()->setScaleFactors(Vec3(2*ballRadius));
// ball connected to ground via a slider along X
double xSinG = optimalFiberLength*cos(pennationAngle)+tendonSlackLength;
SliderJoint* slider = new SliderJoint( "slider",
ground,
Vec3(anchorWidth/2+xSinG, 0, 0),
Vec3(0),
*ball,
Vec3(0),
Vec3(0));
CoordinateSet& jointCoordinateSet = slider->upd_CoordinateSet();
jointCoordinateSet[0].setName("tx");
jointCoordinateSet[0].setDefaultValue(1.0);
jointCoordinateSet[0].setRangeMin(0);
jointCoordinateSet[0].setRangeMax(1.0);
if(motion != NULL){
jointCoordinateSet[0].setPrescribedFunction(*motion);
jointCoordinateSet[0].setDefaultIsPrescribed(true);
}
// add ball to model
model.addBody(ball);
model.addJoint(slider);
//==========================================================================
// 1. SIMULATION SETUP: Add the muscle
//==========================================================================
//Attach the muscle
const string &actuatorType = aMuscle->getConcreteClassName();
aMuscle->setName("muscle");
aMuscle->addNewPathPoint("muscle-box", ground, Vec3(anchorWidth/2,0,0));
aMuscle->addNewPathPoint("muscle-ball", *ball, Vec3(-ballRadius,0,0));
ActivationFiberLengthMuscle_Deprecated *aflMuscle
= dynamic_cast<ActivationFiberLengthMuscle_Deprecated *>(aMuscle);
if(aflMuscle){
// Define the default states for the muscle that has
//activation and fiber-length states
aflMuscle->setDefaultActivation(act0);
aflMuscle->setDefaultFiberLength(aflMuscle->getOptimalFiberLength());
}else{
ActivationFiberLengthMuscle *aflMuscle2
= dynamic_cast<ActivationFiberLengthMuscle *>(aMuscle);
if(aflMuscle2){
// Define the default states for the muscle
//that has activation and fiber-length states
aflMuscle2->setDefaultActivation(act0);
aflMuscle2->setDefaultFiberLength(aflMuscle2
->getOptimalFiberLength());
}
}
model.addForce(aMuscle);
// Create a prescribed controller that simply
//applies controls as function of time
PrescribedController * muscleController = new PrescribedController();
if(control != NULL){
muscleController->setActuators(model.updActuators());
// Set the indiviudal muscle control functions
//for the prescribed muscle controller
muscleController->prescribeControlForActuator("muscle",control->clone());
// Add the control set controller to the model
model.addController(muscleController);
}
// Set names for muscles / joints.
Array<string> muscNames;
muscNames.append(aMuscle->getName());
Array<string> jointNames;
jointNames.append("slider");
//==========================================================================
// 2. SIMULATION SETUP: Instrument the test with probes
//==========================================================================
Array<string> muscNamesTwice = muscNames;
muscNamesTwice.append(muscNames.get(0));
cout << "------------\nPROBES\n------------" << endl;
int probeCounter = 1;
// Add ActuatorPowerProbe to measure work done by the muscle
ActuatorPowerProbe* muscWorkProbe = new ActuatorPowerProbe(muscNames, false, 1);
//muscWorkProbe->setName("ActuatorWork");
muscWorkProbe->setOperation("integrate");
SimTK::Vector ic1(1);
ic1 = 9.0; // some arbitary initial condition.
muscWorkProbe->setInitialConditions(ic1);
model.addProbe(muscWorkProbe);
model.setup();
cout << probeCounter++ << ") Added ActuatorPowerProbe to measure work done by the muscle" << endl;
if (muscWorkProbe->getName() != "UnnamedProbe") {
string errorMessage = "Incorrect default name for unnamed probe: " + muscWorkProbe->getName();
throw (OpenSim::Exception(errorMessage.c_str()));
}
// Add ActuatorPowerProbe to measure power generated by the muscle
ActuatorPowerProbe* muscPowerProbe = new ActuatorPowerProbe(*muscWorkProbe); // use copy constructor
muscPowerProbe->setName("ActuatorPower");
muscPowerProbe->setOperation("value");
model.addProbe(muscPowerProbe);
cout << probeCounter++ << ") Added ActuatorPowerProbe to measure power generated by the muscle" << endl;
// Add ActuatorPowerProbe to report the muscle power MINIMUM
ActuatorPowerProbe* powerProbeMinimum = new ActuatorPowerProbe(*muscPowerProbe); // use copy constructor
powerProbeMinimum->setName("ActuatorPowerMinimum");
powerProbeMinimum->setOperation("minimum");
model.addProbe(powerProbeMinimum);
cout << probeCounter++ << ") Added ActuatorPowerProbe to report the muscle power MINIMUM" << endl;
// Add ActuatorPowerProbe to report the muscle power ABSOLUTE MINIMUM
ActuatorPowerProbe* powerProbeMinAbs = new ActuatorPowerProbe(*muscPowerProbe); // use copy constructor
powerProbeMinAbs->setName("ActuatorPowerMinAbs");
powerProbeMinAbs->setOperation("minabs");
model.addProbe(powerProbeMinAbs);
cout << probeCounter++ << ") Added ActuatorPowerProbe to report the muscle power MINABS" << endl;
// Add ActuatorPowerProbe to report the muscle power MAXIMUM
ActuatorPowerProbe* powerProbeMaximum = new ActuatorPowerProbe(*muscPowerProbe); // use copy constructor
powerProbeMaximum->setName("ActuatorPowerMaximum");
powerProbeMaximum->setOperation("maximum");
model.addProbe(powerProbeMaximum);
cout << probeCounter++ << ") Added ActuatorPowerProbe to report the muscle power MAXIMUM" << endl;
// Add ActuatorPowerProbe to report the muscle power MAXABS
ActuatorPowerProbe* powerProbeMaxAbs = new ActuatorPowerProbe(*muscPowerProbe); // use copy constructor
powerProbeMaxAbs->setName("ActuatorPowerMaxAbs");
powerProbeMaxAbs->setOperation("maxabs");
model.addProbe(powerProbeMaxAbs);
cout << probeCounter++ << ") Added ActuatorPowerProbe to report the muscle power MAXABS" << endl;
// Add ActuatorPowerProbe to measure the square of the power generated by the muscle
ActuatorPowerProbe* muscPowerSquaredProbe = new ActuatorPowerProbe(*muscPowerProbe); // use copy constructor
muscPowerSquaredProbe->setName("ActuatorPowerSquared");
muscPowerSquaredProbe->setExponent(2.0);
model.addProbe(muscPowerSquaredProbe);
cout << probeCounter++ << ") Added ActuatorPowerProbe to measure the square of the power generated by the muscle" << endl;
// Add JointInternalPowerProbe to measure work done by the joint
JointInternalPowerProbe* jointWorkProbe = new JointInternalPowerProbe(jointNames, false, 1);
jointWorkProbe->setName("JointWork");
jointWorkProbe->setOperation("integrate");
jointWorkProbe->setInitialConditions(SimTK::Vector(1, 0.0));
model.addProbe(jointWorkProbe);
cout << probeCounter++ << ") Added JointPowerProbe to measure work done by the joint" << endl;
// Add JointPowerProbe to measure power generated by the joint
JointInternalPowerProbe* jointPowerProbe = new JointInternalPowerProbe(*jointWorkProbe); // use copy constructor
jointPowerProbe->setName("JointPower");
jointPowerProbe->setOperation("value");
model.addProbe(jointPowerProbe);
cout << probeCounter++ << ") Added JointPowerProbe to measure power generated by the joint" << endl;
// Add ActuatorForceProbe to measure the impulse of the muscle force
ActuatorForceProbe* impulseProbe = new ActuatorForceProbe(muscNames, false, 1);
impulseProbe->setName("ActuatorImpulse");
impulseProbe->setOperation("integrate");
impulseProbe->setInitialConditions(SimTK::Vector(1, 0.0));
model.addProbe(impulseProbe);
cout << probeCounter++ << ") Added ActuatorForceProbe to measure the impulse of the muscle force" << endl;
// Add ActuatorForceProbe to report the muscle force
ActuatorForceProbe* forceProbe = new ActuatorForceProbe(*impulseProbe); // use copy constructor
forceProbe->setName("ActuatorForce");
forceProbe->setOperation("value");
model.addProbe(forceProbe);
cout << probeCounter++ << ") Added ActuatorForceProbe to report the muscle force" << endl;
// Add ActuatorForceProbe to report the square of the muscle force
ActuatorForceProbe* forceSquaredProbe = new ActuatorForceProbe(*forceProbe); // use copy constructor
forceSquaredProbe->setName("ActuatorForceSquared");
forceSquaredProbe->setExponent(2.0);
model.addProbe(forceSquaredProbe);
cout << probeCounter++ << ") Added ActuatorForceProbe to report the square of the muscle force " << endl;
// Add ActuatorForceProbe to report the square of the muscle force for the same muscle repeated twice
ActuatorForceProbe* forceSquaredProbeTwice = new ActuatorForceProbe(*forceSquaredProbe); // use copy constructor
forceSquaredProbeTwice->setName("ActuatorForceSquared_RepeatedTwice");
forceSquaredProbeTwice->setSumForcesTogether(true);
forceSquaredProbeTwice->setActuatorNames(muscNamesTwice);
model.addProbe(forceSquaredProbeTwice);
cout << probeCounter++ << ") Added ActuatorForceProbe to report the square of the muscle force for the same muscle repeated twice" << endl;
// Add ActuatorForceProbe to report the square of the muscle force for the same muscle repeated twice, SCALED BY 0.5
ActuatorForceProbe* forceSquaredProbeTwiceScaled = new ActuatorForceProbe(*forceSquaredProbeTwice); // use copy constructor
forceSquaredProbeTwice->setName("ActuatorForceSquared_RepeatedTwiceThenHalved");
double gain1 = 0.5;
forceSquaredProbeTwiceScaled->setGain(gain1);
model.addProbe(forceSquaredProbeTwiceScaled);
cout << probeCounter++ << ") Added ActuatorForceProbe to report the square of the muscle force for the same muscle repeated twice, SCALED BY 0.5" << endl;
// Add ActuatorForceProbe to report -3.5X the muscle force
double gain2 = -3.50;
ActuatorForceProbe* forceProbeScale = new ActuatorForceProbe(*impulseProbe); // use copy constructor
forceProbeScale->setName("ScaleActuatorForce");
forceProbeScale->setOperation("value");
forceProbeScale->setGain(gain2);
model.addProbe(forceProbeScale);
cout << probeCounter++ << ") Added ActuatorForceProbe to report -3.5X the muscle force" << endl;
// Add ActuatorForceProbe to report the differentiated muscle force
ActuatorForceProbe* forceProbeDiff = new ActuatorForceProbe(*impulseProbe); // use copy constructor
forceProbeDiff->setName("DifferentiateActuatorForce");
forceProbeDiff->setOperation("differentiate");
model.addProbe(forceProbeDiff);
cout << probeCounter++ << ") Added ActuatorForceProbe to report the differentiated muscle force" << endl;
// Add SystemEnergyProbe to measure the system KE+PE
SystemEnergyProbe* sysEnergyProbe = new SystemEnergyProbe(true, true);
sysEnergyProbe->setName("SystemEnergy");
sysEnergyProbe->setOperation("value");
sysEnergyProbe->setComputeKineticEnergy(true);
sysEnergyProbe->setComputePotentialEnergy(true);
model.addProbe(sysEnergyProbe);
cout << probeCounter++ << ") Added SystemEnergyProbe to measure the system KE+PE" << endl;
// Add SystemEnergyProbe to measure system power (d/dt system KE+PE)
SystemEnergyProbe* sysPowerProbe = new SystemEnergyProbe(*sysEnergyProbe); // use copy constructor
sysPowerProbe->setName("SystemPower");
sysPowerProbe->setDisabled(false);
sysPowerProbe->setOperation("differentiate");
model.addProbe(sysPowerProbe);
cout << probeCounter++ << ") Added SystemEnergyProbe to measure system power (d/dt system KE+PE)" << endl;
// Add ActuatorForceProbe to report the muscle force value, twice -- REPORTED INDIVIDUALLY AS VECTORS
ActuatorForceProbe* forceSquaredProbeTwiceReportedIndividually1 = new ActuatorForceProbe(*forceProbe); // use copy constructor
forceSquaredProbeTwiceReportedIndividually1->setName("MuscleForce_VALUE_VECTOR");
forceSquaredProbeTwiceReportedIndividually1->setSumForcesTogether(false); // report individually
forceSquaredProbeTwiceReportedIndividually1->setActuatorNames(muscNamesTwice);
//cout << forceSquaredProbeTwiceReportedIndividually1->getActuatorNames().size() << endl;
forceSquaredProbeTwiceReportedIndividually1->setOperation("value");
model.addProbe(forceSquaredProbeTwiceReportedIndividually1);
cout << probeCounter++ << ") Added ActuatorForceProbe to report the muscle force value, twice - REPORTED INDIVIDUALLY" << endl;
// Add ActuatorForceProbe to report the differentiated muscle force value, twice -- REPORTED INDIVIDUALLY AS VECTORS
ActuatorForceProbe* forceSquaredProbeTwiceReportedIndividually2 = new ActuatorForceProbe(*forceSquaredProbeTwiceReportedIndividually1); // use copy constructor
forceSquaredProbeTwiceReportedIndividually2->setName("MuscleForce_DIFFERENTIATE_VECTOR");
forceSquaredProbeTwiceReportedIndividually2->setSumForcesTogether(false); // report individually
forceSquaredProbeTwiceReportedIndividually2->setOperation("differentiate");
model.addProbe(forceSquaredProbeTwiceReportedIndividually2);
cout << probeCounter++ << ") Added ActuatorForceProbe to report the differentiated muscle force value, twice - REPORTED INDIVIDUALLY" << endl;
// Add ActuatorForceProbe to report the integrated muscle force value, twice -- REPORTED INDIVIDUALLY AS VECTORS
ActuatorForceProbe* forceSquaredProbeTwiceReportedIndividually3 = new ActuatorForceProbe(*forceSquaredProbeTwiceReportedIndividually1); // use copy constructor
forceSquaredProbeTwiceReportedIndividually3->setName("MuscleForce_INTEGRATE_VECTOR");
forceSquaredProbeTwiceReportedIndividually3->setSumForcesTogether(false); // report individually
forceSquaredProbeTwiceReportedIndividually3->setOperation("integrate");
SimTK::Vector initCondVec(2);
initCondVec(0) = 0;
initCondVec(1) = 10;
forceSquaredProbeTwiceReportedIndividually3->setInitialConditions(initCondVec);
model.addProbe(forceSquaredProbeTwiceReportedIndividually3);
cout << probeCounter++ << ") Added ActuatorForceProbe to report the integrated muscle force value, twice - REPORTED INDIVIDUALLY" << endl;
cout << "initCondVec = " << initCondVec << endl;
/* Since all components are allocated on the stack don't have model
own them (and try to free)*/
// model.disownAllComponents();
model.setName("testProbesModel");
cout << "Saving model... " << endl;
model.print("testProbesModel.osim");
cout << "Re-loading model... " << endl;
Model reloadedModel = Model("testProbesModel.osim");
/* Setup analyses and reporters. */
ProbeReporter* probeReporter = new ProbeReporter(&model);
model.addAnalysis(probeReporter);
ForceReporter* forceReporter = new ForceReporter(&model);
model.addAnalysis(forceReporter);
MuscleAnalysis* muscleReporter = new MuscleAnalysis(&model);
model.addAnalysis(muscleReporter);
model.print("testProbesModel.osim");
model.printBasicInfo(cout);
//==========================================================================
// 3. SIMULATION Initialization
//==========================================================================
// Initialize the system and get the default state
SimTK::State& si = model.initSystem();
SimTK::Vector testRealInitConditions = forceSquaredProbeTwiceReportedIndividually3->getProbeOutputs(si);
model.getMultibodySystem().realize(si,SimTK::Stage::Dynamics);
model.equilibrateMuscles(si);
CoordinateSet& modelCoordinateSet = model.updCoordinateSet();
// Define non-zero (defaults are 0) states for the free joint
// set x-translation value
modelCoordinateSet[0].setValue(si, startX, true);
//Copy the initial state
SimTK::State initialState(si);
// Check muscle is setup correctly
const PathActuator &muscle
= dynamic_cast<const PathActuator&>(model.updActuators().get("muscle"));
double length = muscle.getLength(si);
double trueLength = startX + xSinG - anchorWidth/2;
ASSERT_EQUAL(length/trueLength, 1.0, testTolerance, __FILE__, __LINE__,
"testMuscles: path failed to initialize to correct length." );
model.getMultibodySystem().realize(si, SimTK::Stage::Acceleration);
double Emuscle0 = muscWorkProbe->getProbeOutputs(si)(0);
//cout << "Muscle initial energy = " << Emuscle0 << endl;
double Esys0 = model.getMultibodySystem().calcEnergy(si);
Esys0 += (Emuscle0 + jointWorkProbe->getProbeOutputs(si)(0));
double PEsys0 = model.getMultibodySystem().calcPotentialEnergy(si);
//cout << "Total initial system energy = " << Esys0 << endl;
//==========================================================================
// 4. SIMULATION Integration
//==========================================================================
// Create the integrator
SimTK::RungeKuttaMersonIntegrator integrator(model.getMultibodySystem());
integrator.setAccuracy(integrationAccuracy);
// Create the manager
Manager manager(model, integrator);
// Integrate from initial time to final time
manager.setInitialTime(initialTime);
manager.setFinalTime(finalTime);
cout<<"\nIntegrating from " << initialTime<< " to " << finalTime << endl;
// Start timing the simulation
const clock_t start = clock();
// simulate
manager.integrate(si);
// how long did it take?
double comp_time = (double)(clock()-start)/CLOCKS_PER_SEC;
//==========================================================================
// 5. SIMULATION Reporting
//==========================================================================
double realTimeMultiplier = ((finalTime-initialTime)/comp_time);
printf("testMuscles: Realtime Multiplier: %f\n"
" : simulation duration / clock duration\n"
" > 1 : faster than real time\n"
" = 1 : real time\n"
" < 1 : slower than real time\n",
realTimeMultiplier );
/*
ASSERT(comp_time <= (finalTime-initialTime));
printf("testMuscles: PASSED Realtime test\n"
" %s simulation time: %f with accuracy %f\n\n",
actuatorType.c_str(), comp_time , accuracy);
*/
//An analysis only writes to a dir that exists, so create here.
if(printResults == true){
Storage states(manager.getStateStorage());
states.print("testProbes_states.sto");
probeReporter->getProbeStorage().print("testProbes_probes.sto");
forceReporter->getForceStorage().print("testProbes_forces.sto");
muscleReporter->getNormalizedFiberLengthStorage()->print("testProbes_normalizedFiberLength.sto");
cout << "\nDone with printing results..." << endl;
}
double muscleWork = muscWorkProbe->getProbeOutputs(si)(0);
cout << "Muscle work = " << muscleWork << endl;
// Test the resetting of probes
cout << "Resetting muscle work probe..." << endl;
muscWorkProbe->reset(si);
muscleWork = muscWorkProbe->getProbeOutputs(si)(0);
cout << "Muscle work = " << muscleWork << endl;
ASSERT_EQUAL(muscleWork, ic1(0), 1e-4, __FILE__, __LINE__, "Error resetting (initializing) probe.");
//==========================================================================
// 6. SIMULATION Tests
//==========================================================================
model.getMultibodySystem().realize(si, SimTK::Stage::Acceleration);
ASSERT_EQUAL(forceSquaredProbeTwiceScaled->getProbeOutputs(si)(0), gain1*forceSquaredProbeTwice->getProbeOutputs(si)(0), 1e-4, __FILE__, __LINE__, "Error with 'scale' operation.");
ASSERT_EQUAL(forceProbeScale->getProbeOutputs(si)(0), gain2*forceProbe->getProbeOutputs(si)(0), 1e-4, __FILE__, __LINE__, "Error with 'scale' operation.");
ASSERT_EQUAL(forceSquaredProbe->getProbeOutputs(si)(0), forceSquaredProbeTwiceScaled->getProbeOutputs(si)(0), 1e-4, __FILE__, __LINE__, "forceSquaredProbeTwiceScaled != forceSquaredProbe.");
ASSERT_EQUAL(forceSquaredProbe->getProbeOutputs(si)(0), pow(forceProbe->getProbeOutputs(si)(0), 2), 1e-4, __FILE__, __LINE__, "Error with forceSquaredProbe probe.");
ASSERT_EQUAL(forceSquaredProbeTwice->getProbeOutputs(si)(0), 2*pow(forceProbe->getProbeOutputs(si)(0), 2), 1e-4, __FILE__, __LINE__, "Error with forceSquaredProbeTwice probe.");
for (int i=0; i<initCondVec.size(); ++i) {
stringstream myError;
//myError << "Initial condition[" << i << "] for vector integration is not being correctly applied." << endl;
//ASSERT_EQUAL(testRealInitConditions(i), initCondVec(i), 1e-4, __FILE__, __LINE__, myError.str());
//if (testRealInitConditions(i) != initCondVec(i))
// cout << "WARNING: Initial condition[" << i << "] for vector integration is not being correctly applied.\nThis is actually an error, but I have made it into a warning for now so that the test passes..." << endl;
}
}
Joint* JointInteraction::xmlParseJoint(IrrXMLReader* xml, std::string& jointType)
{
bool finished = false;
Constraint* constraint = NULL;
Joint* joint = JointFactory::create(xml->getAttributeValue("type"));
jointType = xml->getAttributeValue("type");
joint->setWorld(world);
joint->setId(atoi(xml->getAttributeValue("id")));
joint->setJointInteractionClass(this);
if (xml->getAttributeValue("active"))
joint->setActive(atoi(xml->getAttributeValue("active")));
while (!finished && xml && xml->read())
{
switch (xml->getNodeType())
{
case EXN_ELEMENT:
if (!strcmp("entities", xml->getNodeName()))
{
xmlParseEntities(xml, joint);
} // else if
else if (!strcmp("anchor", xml->getNodeName()))
{
if (!jointType.compare("ball") || !jointType.compare("Ball") || !jointType.compare("BALL"))
{
BallJoint* ball = dynamic_cast<BallJoint*>(joint);
ball->setAnchor(atof(xml->getAttributeValue("xPos")), atof(xml->getAttributeValue("yPos")), atof(xml->getAttributeValue("zPos")));
} // if
else if (!jointType.compare("hinge") || !jointType.compare("Hinge") || !jointType.compare("HINGE"))
{
HingeJoint* hinge = dynamic_cast<HingeJoint*>(joint);
hinge->setAnchor(atof(xml->getAttributeValue("xPos")), atof(xml->getAttributeValue("yPos")), atof(xml->getAttributeValue("zPos")));
} // else if
else if (!jointType.compare("universal") || !jointType.compare("Universal") || !jointType.compare("UNIVERSAL"))
{
UniversalJoint* universal = dynamic_cast<UniversalJoint*>(joint);
universal->setAnchor(atof(xml->getAttributeValue("xPos")), atof(xml->getAttributeValue("yPos")), atof(xml->getAttributeValue("zPos")));
} // else if
else if (!jointType.compare("hinge2") || !jointType.compare("Hinge2") || !jointType.compare("HINGE2"))
{
Hinge2Joint* hinge2 = dynamic_cast<Hinge2Joint*>(joint);
hinge2->setAnchor(atof(xml->getAttributeValue("xPos")), atof(xml->getAttributeValue("yPos")), atof(xml->getAttributeValue("zPos")));
} // else if
else
printd(WARNING, "JointInteraction::loadJoints(): WARNING: JOINT-TYPE %s NEEDS NO anchor ELEMENT!!!\n", jointType.c_str());
} // else if
else if (!strcmp("axis", xml->getNodeName()))
{
if (!jointType.compare("hinge") || !jointType.compare("Hinge") || !jointType.compare("HINGE"))
{
HingeJoint* hinge = dynamic_cast<HingeJoint*>(joint);
hinge->setAxis(atof(xml->getAttributeValue("xDir")), atof(xml->getAttributeValue("yDir")), atof(xml->getAttributeValue("zDir")));
} // if
else if (!jointType.compare("slider") || !jointType.compare("Slider") || !jointType.compare("SLIDER"))
{
SliderJoint* slider = dynamic_cast<SliderJoint*>(joint);
slider->setAxis(atof(xml->getAttributeValue("xDir")), atof(xml->getAttributeValue("yDir")), atof(xml->getAttributeValue("zDir")));
} // else if
else if (!jointType.compare("universal") || !jointType.compare("Universal") || !jointType.compare("UNIVERSAL")) {
UniversalJoint* universal = dynamic_cast<UniversalJoint*>(joint);
if (atoi(xml->getAttributeValue("index")) == 1)
universal->setAxis1(atof(xml->getAttributeValue("xDir")), atof(xml->getAttributeValue("yDir")), atof(xml->getAttributeValue("zDir")));
else if (atoi(xml->getAttributeValue("index")) == 2)
universal->setAxis2(atof(xml->getAttributeValue("xDir")), atof(xml->getAttributeValue("yDir")), atof(xml->getAttributeValue("zDir")));
else
printd(WARNING, "JointInteraction::loadJoints(): WARNING: WRONG ATTRIBUTE FOUND IN axis ELEMENT of UniversalJoint!!!\n");
} // else if
else if (!jointType.compare("hinge2") || !jointType.compare("Hinge2") || !jointType.compare("HINGE2")) {
Hinge2Joint* hinge2 = dynamic_cast<Hinge2Joint*>(joint);
if (atoi(xml->getAttributeValue("index")) == 1)
hinge2->setAxis1(atof(xml->getAttributeValue("xDir")), atof(xml->getAttributeValue("yDir")), atof(xml->getAttributeValue("zDir")));
else if (atoi(xml->getAttributeValue("index")) == 2)
hinge2->setAxis2(atof(xml->getAttributeValue("xDir")), atof(xml->getAttributeValue("yDir")), atof(xml->getAttributeValue("zDir")));
else
printd(WARNING, "JointInteraction::loadJoints(): WARNING: WRONG ATTRIBUTE FOUND IN axis ELEMENT of Hinge2Joint!!!\n");
} // else if
else
printd(WARNING, "JointInteraction::loadJoints(): WARNING: JOINT-TYPE %s NEEDS NO axis ELEMENT!!!\n", jointType.c_str());
} // else if
else if (!strcmp("angles", xml->getNodeName()))
{
if (!jointType.compare("hinge") || !jointType.compare("Hinge") || !jointType.compare("HINGE"))
{
HingeJoint* hinge = dynamic_cast<HingeJoint*>(joint);
hinge->setAngles(atof(xml->getAttributeValue("min")), atof(xml->getAttributeValue("max")));
} // if
else
printd(WARNING, "JointInteraction::loadJoints(): WARNING: JOINT-TYPE %s HAS NO angles ELEMENT!!!\n", jointType.c_str());
} // else if
else if (!strcmp("positions", xml->getNodeName()))
{
SliderJoint* slider = dynamic_cast<SliderJoint*>(joint);
slider->setPositions(atof(xml->getAttributeValue("min")), atof(xml->getAttributeValue("max")));
} // else if
else if (!strcmp("activateIF", xml->getNodeName()))
{
constraint = new Constraint(this);
constraint->setActionType(Constraint::ACTIVATEIF);
constraint->setOwner(joint);
xmlParseConstraintAttributes(xml, constraint);
joint->addConstraint(constraint);
constraint = NULL;
} // else if
else if (!strcmp("deactivateIF", xml->getNodeName()))
{
constraint = new Constraint(this);
constraint->setActionType(Constraint::DEACTIVATEIF);
constraint->setOwner(joint);
xmlParseConstraintAttributes(xml, constraint);
joint->addConstraint(constraint);
constraint = NULL;
} // else if
else if (!strcmp("activeIF", xml->getNodeName()))
{
constraint = new Constraint(this);
constraint->setActionType(Constraint::ACTIVEIF);
constraint->setOwner(joint);
xmlParseConstraintAttributes(xml, constraint);
joint->addConstraint(constraint);
constraint = NULL;
} // else if
else if (!strcmp("inactiveIF", xml->getNodeName()))
{
constraint = new Constraint(this);
constraint->setActionType(Constraint::INACTIVEIF);
constraint->setOwner(joint);
xmlParseConstraintAttributes(xml, constraint);
joint->addConstraint(constraint);
constraint = NULL;
} // else if
break;
case EXN_ELEMENT_END:
if (!strcmp("joint", xml->getNodeName()))
finished = true;
break;
default:
break;
} // switch
} // while
assert(xml);
return joint;
} // xmlParseJoint
void GazeboMechanismControl::UpdateChild()
{
assert(joints_.size() == fake_state_->joint_states_.size());
assert(joints_.size() == joints_damping_.size());
//--------------------------------------------------
// Pushes out simulation state
//--------------------------------------------------
// Copies the state from the gazebo joints into the mechanism joints.
for (unsigned int i = 0; i < joints_.size(); ++i)
{
if (!joints_[i])
continue;
fake_state_->joint_states_[i].applied_effort_ = fake_state_->joint_states_[i].commanded_effort_;
switch(joints_[i]->GetType())
{
case Joint::HINGE: {
HingeJoint *hj = (HingeJoint*)joints_[i];
fake_state_->joint_states_[i].position_ = hj->GetAngle();
fake_state_->joint_states_[i].velocity_ = hj->GetAngleRate();
break;
}
case Joint::SLIDER: {
SliderJoint *sj = (SliderJoint*)joints_[i];
fake_state_->joint_states_[i].position_ = sj->GetPosition();
fake_state_->joint_states_[i].velocity_ = sj->GetPositionRate();
break;
}
default:
abort();
}
}
// Reverses the transmissions to propagate the joint position into the actuators.
fake_state_->propagateStateBackwards();
//--------------------------------------------------
// Runs Mechanism Control
//--------------------------------------------------
hw_.current_time_ = Simulator::Instance()->GetSimTime();
try
{
mcn_.update();
}
catch (const char* c)
{
if (strcmp(c,"dividebyzero")==0)
std::cout << "WARNING:pid controller reports divide by zero error" << std::endl;
else
std::cout << "unknown const char* exception: " << c << std::endl;
}
//--------------------------------------------------
// Takes in actuation commands
//--------------------------------------------------
// Reverses the transmissions to propagate the actuator commands into the joints.
fake_state_->propagateEffortBackwards();
// Copies the commands from the mechanism joints into the gazebo joints.
for (unsigned int i = 0; i < joints_.size(); ++i)
{
if (!joints_[i])
continue;
double damping_force;
double effort = fake_state_->joint_states_[i].commanded_effort_;
switch (joints_[i]->GetType())
{
case Joint::HINGE:
damping_force = joints_damping_[i] * ((HingeJoint*)joints_[i])->GetAngleRate();
((HingeJoint*)joints_[i])->SetTorque(effort - damping_force);
break;
case Joint::SLIDER:
damping_force = joints_damping_[i] * ((SliderJoint*)joints_[i])->GetPositionRate();
((SliderJoint*)joints_[i])->SetSliderForce(effort - damping_force);
break;
default:
abort();
}
}
}
