/**
* This test verifies the use of BodyActuator for applying spatial forces to a selected
* body. It checks if using a BodyActuator generates equivalent acceleration compared
* to when applying the forces via mobilityForce.
*
* @author Soha Pouya
*/
void testBodyActuator()
{
using namespace SimTK;
// start timing
std::clock_t startTime = std::clock();
// Setup OpenSim model
Model *model = new Model;
// turn off gravity
model->setGravity(Vec3(0));
//OpenSim body 1: Ground
OpenSim::Body& ground = model->getGroundBody();
// OpenSim body 2: A Block
// Geometrical/Inertial properties for the block
double blockMass = 1.0, blockSideLength = 1;
Vec3 blockMassCenter(0);
Inertia blockInertia = blockMass*Inertia::brick(blockSideLength/2,
blockSideLength/2, blockSideLength/2); // for the halves see doxygen for brick
OpenSim::Body *block = new OpenSim::Body("block", blockMass,
blockMassCenter, blockInertia);
// Add display geometry to the block to visualize in the GUI
block->addDisplayGeometry("block.vtp");
Vec3 locationInParent(0, blockSideLength / 2, 0), orientationInParent(0),
locationInBody(0), orientationInBody(0);
FreeJoint *blockToGroundFree = new FreeJoint("blockToGroundBall",
ground, locationInParent, orientationInParent,
*block, locationInBody, orientationInBody);
model->addBody(block);
model->addJoint(blockToGroundFree);
// specify magnitude and direction of applied force and torque
double forceMag = 1.0;
Vec3 forceAxis(1, 1, 1);
Vec3 forceInG = forceMag * forceAxis;
double torqueMag = 1.0;
Vec3 torqueAxis(1, 1, 1);
Vec3 torqueInG = torqueMag*torqueAxis;
// ---------------------------------------------------------------------------
// Use MobilityForces to Apply the given Torques and Forces to the body
// ---------------------------------------------------------------------------
State& state = model->initSystem();
model->getMultibodySystem().realize(state, Stage::Dynamics);
Vector_<SpatialVec>& bodyForces =
model->getMultibodySystem().updRigidBodyForces(state, Stage::Dynamics);
bodyForces.dump("Body Forces before applying 6D spatial force:");
model->getMatterSubsystem().addInBodyTorque(state, block->getIndex(),
torqueInG, bodyForces);
model->getMatterSubsystem().addInStationForce(state, block->getIndex(),
Vec3(0), forceInG, bodyForces);
bodyForces.dump("Body Forces after applying 6D spatial force to the block");
model->getMultibodySystem().realize(state, Stage::Acceleration);
Vector udotBody = state.getUDot();
udotBody.dump("Accelerations due to body forces");
// clear body forces
bodyForces *= 0;
// update mobility forces
Vector& mobilityForces = model->getMultibodySystem()
.updMobilityForces(state, Stage::Dynamics);
// Apply torques as mobility forces of the ball joint
for (int i = 0; i<3; ++i){
mobilityForces[i] = torqueInG[i];
mobilityForces[i+3] = forceInG[i];
}
mobilityForces.dump("Mobility Forces after applying 6D spatial force to the block");
model->getMultibodySystem().realize(state, Stage::Acceleration);
Vector udotMobility = state.getUDot();
udotMobility.dump("Accelerations due to mobility forces");
// First make sure that accelerations are not zero accidentally
ASSERT(udotMobility.norm() != 0.0 || udotBody.norm() != 0.0);
// Then check if they are equal
for (int i = 0; i<udotMobility.size(); ++i){
ASSERT_EQUAL(udotMobility[i], udotBody[i], SimTK::Eps);
}
// clear the mobility forces
mobilityForces = 0;
// ---------------------------------------------------------------------------
// Use a BodyActuator to Apply the same given Torques and Forces to the body
// ---------------------------------------------------------------------------
// Create and add the body actuator to the model
BodyActuator* actuator = new BodyActuator(*block);
actuator->setName("BodyAct");
model->addForce(actuator);
model->print("TestBodyActuatorModel.osim");
model->setUseVisualizer(false);
// get a new system and state to reflect additions to the model
State& state1 = model->initSystem();
// -------------- Provide control signals for bodyActuator ----------
// Get the default control vector of the model
Vector modelControls = model->getDefaultControls();
// Spedicfy a vector of control signals to include desired torques and forces
Vector fixedControls(6);
for (int i = 0; i < 3; i++){
fixedControls(i) = torqueInG(i);
fixedControls(i + 3) = forceInG(i);
}
fixedControls.dump("Spatial forces applied by body Actuator:");
// Add control values and set their values
actuator->addInControls(fixedControls, modelControls);
model->setDefaultControls(modelControls);
// ------------------- Compute Acc and Compare -------------
// Compute the acc due to spatial forces applied by body actuator
model->computeStateVariableDerivatives(state1);
Vector udotBodyActuator = state1.getUDot();
udotBodyActuator.dump("Accelerations due to body actuator");
// First make sure that accelerations are not zero accidentally
ASSERT(udotMobility.norm() != 0.0 || udotBodyActuator.norm() != 0.0);
// Then verify that the BodyActuator also generates the same acceleration
// as the equivalent applied mobility force
for (int i = 0; i<udotBodyActuator.size(); ++i){
ASSERT_EQUAL(udotMobility[i], udotBodyActuator[i], SimTK::Eps);
}
// -------------- Setup integrator and manager -------------------
RungeKuttaMersonIntegrator integrator(model->getMultibodySystem());
integrator.setAccuracy(integ_accuracy);
Manager manager(*model, integrator);
manager.setInitialTime(0.0);
double final_t = 1.00;
manager.setFinalTime(final_t);
manager.integrate(state1);
// ----------------- Test Copying the model -------------------
// Before exiting lets see if copying the actuator works
BodyActuator* copyOfActuator = actuator->clone();
ASSERT(*copyOfActuator == *actuator);
// Check that de/serialization works
Model modelFromFile("TestBodyActuatorModel.osim");
ASSERT(modelFromFile == *model, __FILE__, __LINE__,
"Model from file FAILED to match model in memory.");
std::cout << " ********** Test BodyActuator time = ********** " <<
1.e3*(std::clock() - startTime) / CLOCKS_PER_SEC << "ms\n" << endl;
}
void Force::applyTorque(const SimTK::State &s, const OpenSim::Body &aBody,
const Vec3& aTorque, Vector_<SpatialVec> &bodyForces) const
{
_model->getMatterSubsystem().addInBodyTorque(s, SimTK::MobilizedBodyIndex(aBody.getIndex()),
aTorque, bodyForces);
}
//==============================================================================
// Test Cases
//==============================================================================
void testTorqueActuator()
{
using namespace SimTK;
// start timing
std::clock_t startTime = std::clock();
// Setup OpenSim model
Model *model = new Model;
// turn off gravity
model->setGravity(Vec3(0));
//OpenSim bodies
OpenSim::Body& ground = model->getGroundBody();
//Cylindrical bodies
double r = 0.25, h = 1.0;
double m1 = 1.0, m2 = 2.0;
Inertia j1 = m1*Inertia::cylinderAlongY(r, h);
Inertia j2 = m2*Inertia::cylinderAlongY(r, h);
//OpenSim bodies
OpenSim::Body* bodyA
= new OpenSim::Body("A", m1, Vec3(0), j1);
OpenSim::Body* bodyB
= new OpenSim::Body("B", m2, Vec3(0), j2);
// connect bodyA to ground with 6dofs
FreeJoint* base =
new FreeJoint("base", ground, Vec3(0), Vec3(0), *bodyA, Vec3(0), Vec3(0));
model->addBody(bodyA);
model->addJoint(base);
// connect bodyA to bodyB by a Ball joint
BallJoint* bInA = new BallJoint("bInA", *bodyA, Vec3(0,-h/2, 0), Vec3(0),
*bodyB, Vec3(0, h/2, 0), Vec3(0));
model->addBody(bodyB);
model->addJoint(bInA);
// specify magnitude and direction of applied torque
double torqueMag = 2.1234567890;
Vec3 torqueAxis(1/sqrt(2.0), 0, 1/sqrt(2.0));
Vec3 torqueInG = torqueMag*torqueAxis;
State state = model->initSystem();
model->getMultibodySystem().realize(state, Stage::Dynamics);
Vector_<SpatialVec>& bodyForces =
model->getMultibodySystem().updRigidBodyForces(state, Stage::Dynamics);
bodyForces.dump("Body Forces before applying torque");
model->getMatterSubsystem().addInBodyTorque(state, bodyA->getIndex(),
torqueMag*torqueAxis, bodyForces);
model->getMatterSubsystem().addInBodyTorque(state, bodyB->getIndex(),
-torqueMag*torqueAxis, bodyForces);
bodyForces.dump("Body Forces after applying torque to bodyA and bodyB");
model->getMultibodySystem().realize(state, Stage::Acceleration);
const Vector& udotBody = state.getUDot();
udotBody.dump("Accelerations due to body forces");
// clear body forces
bodyForces *= 0;
// update mobility forces
Vector& mobilityForces = model->getMultibodySystem()
.updMobilityForces(state, Stage::Dynamics);
// Apply torques as mobility forces of the ball joint
for(int i=0; i<3; ++i){
mobilityForces[6+i] = torqueInG[i];
}
model->getMultibodySystem().realize(state, Stage::Acceleration);
const Vector& udotMobility = state.getUDot();
udotMobility.dump("Accelerations due to mobility forces");
// First make sure that accelerations are not zero accidentally
ASSERT(udotMobility.norm() != 0.0 || udotBody.norm() != 0.0);
// Then check if they are equal
for(int i=0; i<udotMobility.size(); ++i){
ASSERT_EQUAL(udotMobility[i], udotBody[i], 1.0e-12);
}
// clear the mobility forces
mobilityForces = 0;
//Now add the actuator to the model and control it to generate the same
//torque as applied directly to the multibody system (above)
// Create and add the torque actuator to the model
TorqueActuator* actuator =
new TorqueActuator(*bodyA, *bodyB, torqueAxis, true);
actuator->setName("torque");
model->addForce(actuator);
// Create and add a controller to control the actuator
PrescribedController* controller = new PrescribedController();
controller->addActuator(*actuator);
// Apply torque about torqueAxis
controller->prescribeControlForActuator("torque", new Constant(torqueMag));
model->addController(controller);
/*
ActuatorPowerProbe* powerProbe = new ActuatorPowerProbe(Array<string>("torque",1),false, 1);
powerProbe->setOperation("integrate");
powerProbe->setInitialConditions(Vector(1, 0.0));
*/
//model->addProbe(powerProbe);
model->print("TestTorqueActuatorModel.osim");
model->setUseVisualizer(false);
// get a new system and state to reflect additions to the model
state = model->initSystem();
model->computeStateVariableDerivatives(state);
const Vector &udotTorqueActuator = state.getUDot();
// First make sure that accelerations are not zero accidentally
ASSERT(udotMobility.norm() != 0.0 || udotTorqueActuator.norm() != 0.0);
// Then verify that the TorqueActuator also generates the same acceleration
// as the equivalent applied mobility force
for(int i=0; i<udotMobility.size(); ++i){
ASSERT_EQUAL(udotMobility[i], udotTorqueActuator[i], 1.0e-12);
}
// determine the initial kinetic energy of the system
double iKE = model->getMatterSubsystem().calcKineticEnergy(state);
RungeKuttaMersonIntegrator integrator(model->getMultibodySystem());
integrator.setAccuracy(integ_accuracy);
Manager manager(*model, integrator);
manager.setInitialTime(0.0);
double final_t = 1.00;
manager.setFinalTime(final_t);
manager.integrate(state);
model->computeStateVariableDerivatives(state);
double fKE = model->getMatterSubsystem().calcKineticEnergy(state);
// Change in system kinetic energy can only be attributable to actuator work
//double actuatorWork = (powerProbe->getProbeOutputs(state))[0];
// test that this is true
//ASSERT_EQUAL(actuatorWork, fKE-iKE, integ_accuracy);
// Before exiting lets see if copying the spring works
TorqueActuator* copyOfActuator = actuator->clone();
ASSERT(*copyOfActuator == *actuator);
// Check that de/serialization works
Model modelFromFile("TestTorqueActuatorModel.osim");
ASSERT(modelFromFile == *model, __FILE__, __LINE__,
"Model from file FAILED to match model in memory.");
std::cout << " ********** Test TorqueActuator time = ********** " <<
1.e3*(std::clock()-startTime)/CLOCKS_PER_SEC << "ms\n" << endl;
}
//-----------------------------------------------------------------------------
// METHODS TO APPLY FORCES AND TORQUES
//-----------------------------------------------------------------------------
void Force::applyForceToPoint(const SimTK::State &s, const OpenSim::Body &aBody, const Vec3& aPoint,
const Vec3& aForce, Vector_<SpatialVec> &bodyForces) const
{
_model->getMatterSubsystem().addInStationForce(s, SimTK::MobilizedBodyIndex(aBody.getIndex()),
aPoint, aForce, bodyForces);
}
