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