Ejemplo n.º 1
0
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();
}
Ejemplo n.º 3
0
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();
}
Ejemplo n.º 5
0
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);
}
Ejemplo n.º 6
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();
}
Ejemplo n.º 7
0
// 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());
}
Ejemplo n.º 8
0
//=============================================================================
// 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);
}