/**
 * 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;
}
Пример #3
0
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();
}
Пример #4
0
/**
 * 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();
}
Пример #6
0
void HuntCrossleyForce::setViscousFriction(double friction) {
    if (get_contact_parameters().getSize()==0)
        updContactParametersSet().adoptAndAppend(
                new HuntCrossleyForce::ContactParameters());
    upd_contact_parameters()[0].setViscousFriction(friction); 
};
Пример #7
0
double HuntCrossleyForce::getViscousFriction()   { 
    if (get_contact_parameters().getSize()==0)
        updContactParametersSet().adoptAndAppend(
                new HuntCrossleyForce::ContactParameters());
    return get_contact_parameters().get(0).getViscousFriction(); 
};
Пример #8
0
void HuntCrossleyForce::setStiffness(double stiffness) {
    if (get_contact_parameters().getSize()==0)
        updContactParametersSet().adoptAndAppend(
                new HuntCrossleyForce::ContactParameters());
    upd_contact_parameters()[0].setStiffness(stiffness); 
};
Пример #9
0
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); 
};