Esempio 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();
}
Esempio n. 3
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);
}
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();
}
Esempio n. 5
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);
}