/**
* Provide the value(s) to be reported that correspond to the labels
*/
OpenSim::Array<double> ElasticFoundationForce::getRecordValues(const SimTK::State& state) const
{
OpenSim::Array<double> values(1);
const ContactParametersSet& contactParametersSet =
get_contact_parameters();
const SimTK::ElasticFoundationForce& simtkForce =
(SimTK::ElasticFoundationForce &)(_model->getForceSubsystem().getForce(_index));
SimTK::Vector_<SimTK::SpatialVec> bodyForces(0);
SimTK::Vector_<SimTK::Vec3> particleForces(0);
SimTK::Vector mobilityForces(0);
//get the net force added to the system contributed by the Spring
simtkForce.calcForceContribution(state, bodyForces, particleForces, mobilityForces);
for (int i = 0; i < contactParametersSet.getSize(); ++i)
{
ContactParameters& params = contactParametersSet.get(i);
for (int j = 0; j < params.getGeometry().size(); ++j)
{
ContactGeometry& geom = _model->updContactGeometrySet().get(params.getGeometry()[j]);
std::string bodyName = geom.getBodyName();
SimTK::Vec3 forces = bodyForces(_model->getBodySet().get(bodyName).getIndex())[1];
SimTK::Vec3 torques = bodyForces(_model->getBodySet().get(bodyName).getIndex())[0];
values.append(3, &forces[0]);
values.append(3, &torques[0]);
}
}
return values;
}
/**
* Provide names of the quantities (column labels) of the force value(s) reported
*
*/
OpenSim::Array<std::string> ElasticFoundationForce::getRecordLabels() const
{
OpenSim::Array<std::string> labels("");
const ContactParametersSet& contactParametersSet =
get_contact_parameters();
for (int i = 0; i < contactParametersSet.getSize(); ++i)
{
ContactParameters& params = contactParametersSet.get(i);
for (int j = 0; j < params.getGeometry().size(); ++j)
{
ContactGeometry& geom = _model->updContactGeometrySet().get(params.getGeometry()[j]);
std::string bodyName = geom.getBodyName();
labels.append(getName()+"."+bodyName+".force.X");
labels.append(getName()+"."+bodyName+".force.Y");
labels.append(getName()+"."+bodyName+".force.Z");
labels.append(getName()+"."+bodyName+".torque.X");
labels.append(getName()+"."+bodyName+".torque.Y");
labels.append(getName()+"."+bodyName+".torque.Z");
}
}
return labels;
}
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();
}
/**
* Provide the value(s) to be reported that correspond to the labels
*/
OpenSim::Array<double> HuntCrossleyForce::
getRecordValues(const SimTK::State& state) const
{
OpenSim::Array<double> values(1);
const ContactParametersSet& contactParametersSet =
get_contact_parameters();
const auto& forceSubsys = _model->getForceSubsystem();
const SimTK::Force& abstractForce = forceSubsys.getForce(_index);
const auto& simtkForce = (SimTK::HuntCrossleyForce &)(abstractForce);
SimTK::Vector_<SimTK::SpatialVec> bodyForces(0);
SimTK::Vector_<SimTK::Vec3> particleForces(0);
SimTK::Vector mobilityForces(0);
//get the net force added to the system contributed by the Spring
simtkForce.calcForceContribution(state, bodyForces, particleForces,
mobilityForces);
for (int i = 0; i < contactParametersSet.getSize(); ++i)
{
ContactParameters& params = contactParametersSet.get(i);
for (int j = 0; j < params.getGeometry().size(); ++j)
{
const ContactGeometry& geom =
getModel().getComponent<ContactGeometry>(params.getGeometry()[j]);
const auto& mbi = geom.getFrame().getMobilizedBodyIndex();
const auto& thisBodyForce = bodyForces(mbi);
SimTK::Vec3 forces = thisBodyForce[1];
SimTK::Vec3 torques = thisBodyForce[0];
values.append(3, &forces[0]);
values.append(3, &torques[0]);
}
}
return values;
}
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 HuntCrossleyForce::setViscousFriction(double friction) {
if (get_contact_parameters().getSize()==0)
updContactParametersSet().adoptAndAppend(
new HuntCrossleyForce::ContactParameters());
upd_contact_parameters()[0].setViscousFriction(friction);
};
double HuntCrossleyForce::getViscousFriction() {
if (get_contact_parameters().getSize()==0)
updContactParametersSet().adoptAndAppend(
new HuntCrossleyForce::ContactParameters());
return get_contact_parameters().get(0).getViscousFriction();
};
void HuntCrossleyForce::setStiffness(double stiffness) {
if (get_contact_parameters().getSize()==0)
updContactParametersSet().adoptAndAppend(
new HuntCrossleyForce::ContactParameters());
upd_contact_parameters()[0].setStiffness(stiffness);
};
const HuntCrossleyForce::ContactParametersSet& HuntCrossleyForce::
getContactParametersSet()
{
return get_contact_parameters();
}
const ElasticFoundationForce::ContactParametersSet& ElasticFoundationForce::getContactParametersSet()
{
return get_contact_parameters();
}
void ElasticFoundationForce::addGeometry(const std::string& name)
{
if (get_contact_parameters().getSize()==0)
updContactParametersSet().adoptAndAppend(new ElasticFoundationForce::ContactParameters());
upd_contact_parameters()[0].addGeometry(name);
}
void ElasticFoundationForce::setViscousFriction(double friction) {
if (get_contact_parameters().getSize()==0)
updContactParametersSet().adoptAndAppend(new ElasticFoundationForce::ContactParameters());
upd_contact_parameters()[0].setViscousFriction(friction);
};
double ElasticFoundationForce::getViscousFriction() {
if (get_contact_parameters().getSize()==0)
updContactParametersSet().adoptAndAppend(new ElasticFoundationForce::ContactParameters());
return get_contact_parameters().get(0).getViscousFriction();
};
void ElasticFoundationForce::setStiffness(double stiffness) {
if (get_contact_parameters().getSize()==0)
updContactParametersSet().adoptAndAppend(new ElasticFoundationForce::ContactParameters());
upd_contact_parameters()[0].setStiffness(stiffness);
};
