void Coordinate::extendAddToSystem(SimTK::MultibodySystem& system) const
{
Super::extendAddToSystem(system);
// Make this modifiable temporarily so we can record information needed
// to later access our pieces of the SimTK::MultibodySystem. That info is
// const after the system has been built.
Coordinate* mutableThis = const_cast<Coordinate *>(this);
// Define the locked value for the constraint as a function.
// The PrescribedMotion will take ownership, but we'll keep a reference
// pointer here to allow for later modification.
std::unique_ptr<ModifiableConstant>
funcOwner(new ModifiableConstant(get_default_value(), 1));
mutableThis->_lockFunction = funcOwner.get();
// The underlying SimTK constraint
SimTK::Constraint::PrescribedMotion
lock(system.updMatterSubsystem(),
funcOwner.release(), // give up ownership
_bodyIndex,
SimTK::MobilizerQIndex(_mobilizerQIndex));
// Save the index so we can access the SimTK::Constraint later
mutableThis->_lockedConstraintIndex = lock.getConstraintIndex();
if(!getProperty_prescribed_function().empty()){
//create prescribed motion constraint automatically
SimTK::Constraint::PrescribedMotion prescribe(
_model->updMatterSubsystem(),
get_prescribed_function().createSimTKFunction(),
_bodyIndex,
SimTK::MobilizerQIndex(_mobilizerQIndex));
mutableThis->_prescribedConstraintIndex = prescribe.getConstraintIndex();
}
else if(get_prescribed()){
// if prescribed is set to true, and there is no prescribed
// function defined, then it cannot be prescribed (set to false).
mutableThis->upd_prescribed() = false;
}
//Now add clamping
addModelingOption("is_clamped", 1);
SimTK::SubsystemIndex sbsix =
getModel().getMatterSubsystem().getMySubsystemIndex();
//Expose coordinate state variable
CoordinateStateVariable* csv
= new CoordinateStateVariable("value", *this,
sbsix, _mobilizerQIndex );
addStateVariable(csv);
//Expose coordinate's speed state variable
SpeedStateVariable* ssv =
new SpeedStateVariable("speed", *this, sbsix, _mobilizerQIndex);
addStateVariable(ssv);
}
void CoordinateCouplerConstraint::addToSystem(SimTK::MultibodySystem& system) const
{
Super::addToSystem(system);
/** List of mobilized body indices established when constraint is set up */
std::vector<SimTK::MobilizedBodyIndex> mob_bodies;
/** List of coordinate (Q) indices corresponding to the respective body */
std::vector<SimTK::MobilizerQIndex> mob_qs;
string errorMessage;
// Look up the bodies and coordinates being coupled by name in the
// model and keep lists of their indices
Array<std::string> independentCoordNames;
for(int i = 0; i < getProperty_independent_coordinate_names().size(); i++) {
independentCoordNames.append(get_independent_coordinate_names(i));
}
for(int i=0; i<independentCoordNames.getSize(); i++){
// Error checking was handled in connectToModel()
const Coordinate& aCoordinate = getModel().getCoordinateSet().get(independentCoordNames[i]);
mob_bodies.push_back(aCoordinate._bodyIndex);
mob_qs.push_back(SimTK::MobilizerQIndex(aCoordinate._mobilizerQIndex));
}
// Last coordinate in the coupler is the dependent coordinate
const Coordinate& aCoordinate = getModel().getCoordinateSet().get(get_dependent_coordinate_name());
mob_bodies.push_back(aCoordinate._bodyIndex);
mob_qs.push_back(SimTK::MobilizerQIndex(aCoordinate._mobilizerQIndex));
if (!mob_qs.size() & (mob_qs.size() != mob_bodies.size())) {
errorMessage = "CoordinateCouplerConstraint:: requires at least one body and coordinate." ;
throw (Exception(errorMessage));
}
// Create and set the underlying coupler constraint function;
const Function& f = get_coupled_coordinates_function();
SimTK::Function *simtkCouplerFunction = new CompoundFunction(f.createSimTKFunction(), get_scale_factor());
// Now create a Simbody Constraint::CoordinateCoupler
SimTK::Constraint::CoordinateCoupler simtkCoordinateCoupler(system.updMatterSubsystem() ,
simtkCouplerFunction,
mob_bodies, mob_qs);
// Beyond the const Component get the index so we can access the SimTK::Constraint later
CoordinateCouplerConstraint* mutableThis = const_cast<CoordinateCouplerConstraint *>(this);
mutableThis->_index = simtkCoordinateCoupler.getConstraintIndex();
}
void integrateSimbodySystem(SimTK::MultibodySystem &system, SimTK::State &state)
{
using namespace SimTK;
// realize simbody system to velocity stage
system.realize(state, Stage::Velocity);
RungeKuttaMersonIntegrator integ(system);
integ.setAccuracy(integ_accuracy);
TimeStepper ts(system, integ);
ts.initialize(state);
ts.stepTo(duration);
state = ts.getState();
}
void ElasticFoundationForce::addToSystem(SimTK::MultibodySystem& system) const
{
Super::addToSystem(system);
const ContactParametersSet& contactParametersSet =
get_contact_parameters();
const double& transitionVelocity = get_transition_velocity();
SimTK::GeneralContactSubsystem& contacts = system.updContactSubsystem();
SimTK::SimbodyMatterSubsystem& matter = system.updMatterSubsystem();
SimTK::ContactSetIndex set = contacts.createContactSet();
SimTK::ElasticFoundationForce force(_model->updForceSubsystem(), contacts, set);
force.setTransitionVelocity(transitionVelocity);
for (int i = 0; i < contactParametersSet.getSize(); ++i)
{
ContactParameters& params = contactParametersSet.get(i);
for (int j = 0; j < params.getGeometry().size(); ++j)
{
if (!_model->updContactGeometrySet().contains(params.getGeometry()[j]))
{
std::string errorMessage = "Invalid ContactGeometry (" + params.getGeometry()[j] + ") specified in ElasticFoundationForce" + getName();
throw (Exception(errorMessage.c_str()));
}
ContactGeometry& geom = _model->updContactGeometrySet().get(params.getGeometry()[j]);
contacts.addBody(set, matter.updMobilizedBody(SimTK::MobilizedBodyIndex(geom.getBody().getIndex())), geom.createSimTKContactGeometry(), geom.getTransform());
if (dynamic_cast<ContactMesh*>(&geom) != NULL)
force.setBodyParameters(SimTK::ContactSurfaceIndex(contacts.getNumBodies(set)-1),
params.getStiffness(), params.getDissipation(),
params.getStaticFriction(), params.getDynamicFriction(), params.getViscousFriction());
}
}
// Beyond the const Component get the index so we can access the SimTK::Force later
ElasticFoundationForce* mutableThis = const_cast<ElasticFoundationForce *>(this);
mutableThis->_index = force.getForceIndex();
}
void Coordinate::extendAddToSystem(SimTK::MultibodySystem& system) const
{
Super::extendAddToSystem(system);
//create lock constraint automatically
// The underlying SimTK constraint
SimTK::Constraint::PrescribedMotion lock(system.updMatterSubsystem(),
_lockFunction,
_bodyIndex,
SimTK::MobilizerQIndex(_mobilizerQIndex));
// Beyond the const Component get the index so we can access the SimTK::Constraint later
Coordinate* mutableThis = const_cast<Coordinate *>(this);
mutableThis->_lockedConstraintIndex = lock.getConstraintIndex();
//mutableThis->_model->addModelComponent(mutableThis);
if(!getProperty_prescribed_function().empty()){
//create prescribed motion constraint automatically
SimTK::Constraint::PrescribedMotion prescribe(
_model->updMatterSubsystem(),
get_prescribed_function().createSimTKFunction(),
_bodyIndex,
SimTK::MobilizerQIndex(_mobilizerQIndex));
mutableThis->_prescribedConstraintIndex = prescribe.getConstraintIndex();
}
else{
// even if prescribed is set to true, if there is no prescribed
// function defined, then it cannot be prescribed.
mutableThis->upd_prescribed() = false;
}
//TODO add clamping
addModelingOption("is_clamped", 1);
SimTK::SubsystemIndex sbsix =
getModel().getMatterSubsystem().getMySubsystemIndex();
//Expose coordinate state variable
CoordinateStateVariable* csv
= new CoordinateStateVariable("value", *this,
sbsix, _mobilizerQIndex );
addStateVariable(csv);
//Expose coordinate's speed state variable
SpeedStateVariable* ssv =
new SpeedStateVariable("speed", *this, sbsix, _mobilizerQIndex);
addStateVariable(ssv);
}
void HuntCrossleyForce::extendAddToSystem(SimTK::MultibodySystem& system) const
{
Super::extendAddToSystem(system);
const ContactParametersSet& contactParametersSet =
get_contact_parameters();
const double& transitionVelocity = get_transition_velocity();
SimTK::GeneralContactSubsystem& contacts = system.updContactSubsystem();
SimTK::ContactSetIndex set = contacts.createContactSet();
SimTK::HuntCrossleyForce force(_model->updForceSubsystem(), contacts, set);
force.setTransitionVelocity(transitionVelocity);
for (int i = 0; i < contactParametersSet.getSize(); ++i)
{
ContactParameters& params = contactParametersSet.get(i);
for (int j = 0; j < params.getGeometry().size(); ++j) {
// get the ContactGeometry from the Model
const ContactGeometry& geom =
getModel().getComponent<ContactGeometry>(params.getGeometry()[j]);
// B: base Frame (Body or Ground)
// F: PhysicalFrame that this ContactGeometry is connected to
// P: the frame defined (relative to F) by the location and
// orientation properties.
const auto& X_BF = geom.getFrame().findTransformInBaseFrame();
const auto& X_FP = geom.getTransform();
const auto X_BP = X_BF * X_FP;
contacts.addBody(set, geom.getFrame().getMobilizedBody(),
geom.createSimTKContactGeometry(), X_BP);
force.setBodyParameters(
SimTK::ContactSurfaceIndex(contacts.getNumBodies(set)-1),
params.getStiffness(), params.getDissipation(),
params.getStaticFriction(), params.getDynamicFriction(),
params.getViscousFriction());
}
}
// Beyond the const Component get the index so we can access the
// SimTK::Force later.
HuntCrossleyForce* mutableThis = const_cast<HuntCrossleyForce *>(this);
mutableThis->_index = force.getForceIndex();
}
// 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());
}
//=============================================================================
// Simbody Model building.
//=============================================================================
//_____________________________________________________________________________
void CustomJoint::addToSystem(SimTK::MultibodySystem& system) const
{
SimTK::MobilizedBody inb;
SimTK::Body outb;
const SimTK::Transform* inbX = &getParentTransform();
const SimTK::Transform* outbX = &getChildTransform();
const OpenSim::Body* mobilized = &getChildBody();
// if the joint is reversed then flip the underlying tree representation
// of inboard and outboard bodies, although the joint direction will be
// preserved, the inboard must exist first.
if (get_reverse()){
inb = system.updMatterSubsystem().updMobilizedBody(
getChildBody().getIndex());
inbX = &getChildTransform();
outb = getParentInternalRigidBody();
outbX = &getParentTransform();
mobilized = &getParentBody();
}
else{
inb = system.updMatterSubsystem().updMobilizedBody(
getParentBody().getIndex());
outb = getChildInternalRigidBody();
}
const CoordinateSet& coords = get_CoordinateSet();
// Some initializations
int numMobilities = coords.getSize(); // Note- should check that all coordinates are used.
std::vector<std::vector<int> > coordinateIndices =
getSpatialTransform().getCoordinateIndices();
std::vector<const SimTK::Function*> functions =
getSpatialTransform().getFunctions();
std::vector<Vec3> axes = getSpatialTransform().getAxes();
SimTK_ASSERT1(numMobilities > 0,
"%s must have 1 or more mobilities (dofs).",
getConcreteClassName().c_str());
SimTK_ASSERT1(numMobilities <= 6,
"%s cannot exceed 6 mobilities (dofs).",
getConcreteClassName().c_str());
assert(functions.size() == 6);
SimTK_ASSERT2(numMobilities <= 6,
"%s::%s must specify functions for complete spatial (6 axes) motion.",
getConcreteClassName().c_str(),
getSpatialTransform().getConcreteClassName().c_str());
assert(coordinateIndices.size() == 6);
SimTK_ASSERT2(axes.size() == 6,
"%s::%s must specify 6 independent axes to span spatial motion.",
getConcreteClassName().c_str(), getSpatialTransform().getConcreteClassName().c_str());
SimTK::MobilizedBody::Direction dir =
SimTK::MobilizedBody::Direction(get_reverse());
SimTK::MobilizedBody::FunctionBased
simtkBody(inb, *inbX,
outb, *outbX,
numMobilities, functions,
coordinateIndices, axes, dir);
int nc = numCoordinates();
SimTK_ASSERT1(nc == numMobilities, "%s list of coordinates does not match number of mobilities.",
getConcreteClassName().c_str());
assignSystemIndicesToBodyAndCoordinates(simtkBody, mobilized, nc, 0);
// TODO: Joints require super class to be called last.
Super::addToSystem(system);
}
