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 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 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();
}
//=============================================================================
// 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);
}