Example #1
0
/**
 * Append actuators from an actuator set to this set.  Copies of the actuators are not made.
 *
 * This method overrides the method in Set<Force> so that several
 * internal variables of the actuator set can be updated.
 *
 * @param aForceSet The set of actuators to be appended.
 * @param aAllowDuplicateNames If true, all actuators will be appended; If false, don't append actuators whose
 * name already exists in this model's actuator set.
 * @return True if successful; false otherwise.
 */
bool ForceSet::append(ForceSet &aForceSet, bool aAllowDuplicateNames)
{
    bool success = true;
    for(int i=0;i<aForceSet.getSize() && success;i++) {
        bool nameExists = false;
        if(!aAllowDuplicateNames) {
            std::string name = aForceSet.get(i).getName();
            for(int j=0;j<getSize();j++) {
                if(get(j).getName() == name) {
                    nameExists = true;
                    break;
                }
            }
        }
        if(!nameExists) {
            if(!ModelComponentSet<Force>::adoptAndAppend(&aForceSet.get(i))) 
                success = false;
        }
    }

    if(success) {
        updateActuators();
        updateMuscles();
    }

    return(success);
}
Example #2
0
bool testForceSet()
{
  using namespace se3;
  typedef Eigen::Matrix<double,4,4> Matrix4;
  typedef SE3::Matrix6 Matrix6;
  typedef SE3::Vector3 Vector3;
  typedef Force::Vector6 Vector6;

  SE3 amb = SE3::Random();
  SE3 bmc = SE3::Random();
  SE3 amc = amb*bmc;

  ForceSet F(12);
  ForceSet F2(Eigen::Matrix<double,3,2>::Zero(),Eigen::Matrix<double,3,2>::Zero());
  F.block(10,2) = F2;
  assert( F.matrix().col(10).norm() == 0.0 );
  assert( isnan(F.matrix()(0,9)) );

  std::cout << "F10 = " << F2.matrix() << std::endl;
  std::cout << "F = " << F.matrix() << std::endl;

  ForceSet F3(Eigen::Matrix<double,3,12>::Random(),Eigen::Matrix<double,3,12>::Random());
  ForceSet F4 = amb.act(F3);
  SE3::Matrix6 aXb= amb;
  assert( (aXb.transpose().inverse()*F3.matrix()).isApprox(F4.matrix()) );

  ForceSet bF = bmc.act(F3);
  ForceSet aF = amb.act(bF); 
  ForceSet aF2 = amc.act(F3);
  assert( aF.matrix().isApprox( aF2.matrix() ) );

  ForceSet F36 = amb.act(F3.block(3,6));
  assert( (aXb.transpose().inverse()*F3.matrix().block(0,3,6,6)).isApprox(F36.matrix()) );
  
  ForceSet F36full(12); F36full.block(3,6) = amb.act(F3.block(3,6)); 
  assert( (aXb.transpose().inverse()*F3.matrix().block(0,3,6,6))
	  .isApprox(F36full.matrix().block(0,3,6,6)) );

  return true;
}
/**
 * This method is called at the beginning of an analysis so that any
 * necessary initializations may be performed.
 *
 * This method should be overridden in the child class.  It is
 * included here so that the child class will not have to implement it if it
 * is not necessary.
 *
 * @param s state of system
 *
 * @return -1 on error, 0 otherwise.
 */
int InverseDynamics::begin(const SimTK::State& s )
{
    if(!proceed()) return(0);

    SimTK::Matrix massMatrix;
    _model->getMatterSubsystem().calcM(s, massMatrix);

    //massMatrix.dump("mass matrix is:");
    // Check that m is full rank
    try {
        SimTK::FactorLU lu(massMatrix);
    } catch (const SimTK::Exception::Base&) {
        throw Exception("InverseDynamics: ERROR- mass matrix is singular  ",__FILE__,__LINE__);
    }
    // enable the line below when simmath is fixed
    //bool singularMassMatrix = lu.isSingular();

    //cout << "Mass matrix is " << (singularMassMatrix?"singular":"Non-singular");

    // Make a working copy of the model
    delete _modelWorkingCopy;
    _modelWorkingCopy = _model->clone();
    _modelWorkingCopy->updAnalysisSet().setSize(0);

    // Replace model force set with only generalized forces
    if(_model) {
        SimTK::State& sWorkingCopyTemp = _modelWorkingCopy->initSystem();
        // Update the _forceSet we'll be computing inverse dynamics for
        if(_ownsForceSet) delete _forceSet;
        if(_useModelForceSet) {
            // Set pointer to model's internal force set
            _forceSet = &_modelWorkingCopy->updForceSet();
            _numCoordinateActuators = _modelWorkingCopy->getActuators().getSize();
        } else {
            ForceSet& as = _modelWorkingCopy->updForceSet();
            // Keep a copy of forces that are not muscles to restore them back.
            ForceSet* saveForces = as.clone();
            // Generate an force set consisting of a coordinate actuator for every unconstrained degree of freedom
            _forceSet = CoordinateActuator::CreateForceSetOfCoordinateActuatorsForModel(sWorkingCopyTemp,*_modelWorkingCopy,1,false);
            _numCoordinateActuators = _forceSet->getSize();
            // Copy whatever forces that are not muscles back into the model
            
            for(int i=0; i<saveForces->getSize(); i++){
                // const Force& f=saveForces->get(i);
                if ((dynamic_cast<const Muscle*>(&saveForces->get(i)))==NULL)
                    as.append(saveForces->get(i).clone());
            }
        }
        _modelWorkingCopy->setAllControllersEnabled(false);
        _ownsForceSet = false;

        SimTK::State& sWorkingCopy = _modelWorkingCopy->initSystem();

        // Gather indices into speed set corresponding to the unconstrained degrees of freedom (for which we will set acceleration constraints)
        _accelerationIndices.setSize(0);
        auto coordinates = _modelWorkingCopy->getCoordinatesInMultibodyTreeOrder();
        for(size_t i=0u; i<coordinates.size(); ++i) {
            const Coordinate& coord = *coordinates[i];
            if(!coord.isConstrained(sWorkingCopy)) {
                _accelerationIndices.append(static_cast<int>(i));
            }
        }

        _dydt.setSize(_modelWorkingCopy->getNumCoordinates() + _modelWorkingCopy->getNumSpeeds());

        int nf = _numCoordinateActuators;
        int nacc = _accelerationIndices.getSize();

        if(nf < nacc) 
            throw(Exception("InverseDynamics: ERROR- over-constrained system -- need at least as many forces as there are degrees of freedom.\n"));

        // Realize to velocity in case there are any velocity dependent forces
        _modelWorkingCopy->getMultibodySystem().realize(sWorkingCopy, SimTK::Stage::Velocity);

        _constraintMatrix.resize(nacc,nf);
        _constraintVector.resize(nacc);

        _performanceMatrix.resize(nf,nf);
        _performanceMatrix = 0;
        for(int i=0,j=0; i<nf; i++) {
            ScalarActuator* act = dynamic_cast<ScalarActuator*>(&_forceSet->get(i));
            if( act ) {
                act->setActuation(sWorkingCopy, 1);
                _performanceMatrix(j,j) = act->getStress(sWorkingCopy);
                j++;
             }
        }

        _performanceVector.resize(nf);
        _performanceVector = 0;

        int lwork = nf + nf + nacc;
        _lapackWork.resize(lwork);
    }

    _statesSplineSet=GCVSplineSet(5,_statesStore);

    // DESCRIPTION AND LABELS
    constructDescription();
    constructColumnLabels();

    deleteStorage();
    allocateStorage();

    // RESET STORAGE
    _storage->reset(s.getTime());

    // RECORD
    int status = 0;
    if(_storage->getSize()<=0) {
        status = record(s);
    }

    return(status);
}
/**
 * This method is called at the beginning of an analysis so that any
 * necessary initializations may be performed.
 *
 * This method is meant to be called at the beginning of an integration 
 *
 * @param s Current state .
 *
 * @return -1 on error, 0 otherwise.
 */
int StaticOptimization::
begin(SimTK::State& s )
{
    if(!proceed()) return(0);

    // Make a working copy of the model
    delete _modelWorkingCopy;
    _modelWorkingCopy = _model->clone();
    _modelWorkingCopy->initSystem();

    // Replace model force set with only generalized forces
    if(_model) {
        SimTK::State& sWorkingCopyTemp = _modelWorkingCopy->updWorkingState();
        // Update the _forceSet we'll be computing inverse dynamics for
        if(_ownsForceSet) delete _forceSet;
        if(_useModelForceSet) {
            // Set pointer to model's internal force set
            _forceSet = &_modelWorkingCopy->updForceSet();
            _ownsForceSet = false;
        } else {
            ForceSet& as = _modelWorkingCopy->updForceSet();
            // Keep a copy of forces that are not muscles to restore them back.
            ForceSet* saveForces = as.clone();
            // Generate an force set consisting of a coordinate actuator for every unconstrained degree of freedom
            _forceSet = CoordinateActuator::CreateForceSetOfCoordinateActuatorsForModel(sWorkingCopyTemp,*_modelWorkingCopy,1,false);
            _ownsForceSet = false;
            _modelWorkingCopy->setAllControllersEnabled(false);
            _numCoordinateActuators = _forceSet->getSize();
            // Copy whatever forces that are not muscles back into the model
            
            for(int i=0; i<saveForces->getSize(); i++){
                const Force& f=saveForces->get(i);
                if ((dynamic_cast<const Muscle*>(&saveForces->get(i)))==NULL)
                    as.append(saveForces->get(i).clone());
            }
        }

        SimTK::State& sWorkingCopy = _modelWorkingCopy->initSystem();
        // Set modeling options for Actuators to be overriden
        for(int i=0; i<_forceSet->getSize(); i++) {
            ScalarActuator* act = dynamic_cast<ScalarActuator*>(&_forceSet->get(i));
            if( act ) {
                act->overrideActuation(sWorkingCopy, true);
            }
        }

        sWorkingCopy.setTime(s.getTime());
        sWorkingCopy.setQ(s.getQ());
        sWorkingCopy.setU(s.getU());
        sWorkingCopy.setZ(s.getZ());
        _modelWorkingCopy->getMultibodySystem().realize(s,SimTK::Stage::Velocity);
        _modelWorkingCopy->equilibrateMuscles(sWorkingCopy);
        // Gather indices into speed set corresponding to the unconstrained degrees of freedom 
        // (for which we will set acceleration constraints)
        _accelerationIndices.setSize(0);
        const CoordinateSet& coordSet = _model->getCoordinateSet();
        for(int i=0; i<coordSet.getSize(); i++) {
            const Coordinate& coord = coordSet.get(i);
            if(!coord.isConstrained(sWorkingCopy)) {
                Array<int> inds = _statesStore->
                    getColumnIndicesForIdentifier(coord.getName()) ;
                _accelerationIndices.append(inds[0]);
            }
        }

        int na = _forceSet->getSize();
        int nacc = _accelerationIndices.getSize();

        if(na < nacc) 
            throw(Exception("StaticOptimization: ERROR- over-constrained "
                "system -- need at least as many forces as there are degrees of freedom.\n") );

        _forceReporter.reset(new ForceReporter(_modelWorkingCopy));
        _forceReporter->begin(sWorkingCopy);
        _forceReporter->updForceStorage().reset();

        _parameters.resize(_modelWorkingCopy->getNumControls());
        _parameters = 0;
    }

    _statesSplineSet=GCVSplineSet(5,_statesStore);

    // DESCRIPTION AND LABELS
    constructDescription();
    constructColumnLabels();

    deleteStorage();
    allocateStorage();

    // RESET STORAGE
    _activationStorage->reset(s.getTime());
    _forceReporter->updForceStorage().reset(s.getTime());

    // RECORD
    int status = 0;
    if(_activationStorage->getSize()<=0) {
        status = record(s);
        const Set<Actuator>& fs = _modelWorkingCopy->getActuators();
        for(int k=0;k<fs.getSize();k++) {
            ScalarActuator* act = dynamic_cast<ScalarActuator *>(&fs[k]);
            if (act){
                cout << "Bounds for " << act->getName() << ": "
                    << act->getMinControl() << " to "
                    << act->getMaxControl() << endl;
            }
            else{
                std::string msg = getConcreteClassName();
                msg += "::can only process scalar Actuator types.";
                throw Exception(msg);
            }
        }
    }

    return(status);
}
bool SimbodySimmModel::writeMuscle(Muscle& aMuscle, const ForceSet& aActuatorSet, ofstream& aStream)
{
    aStream << "beginmuscle " << aMuscle.getName() << endl;

    const PathPointSet& pts = aMuscle.getGeometryPath().getPathPointSet();

    aStream << "beginpoints" << endl;
    for (int i = 0; i < pts.getSize(); i++)
    {
        PathPoint& pt = pts.get(i);
        if (pt.getConcreteClassName()==("ConditionalPathPoint")) {
            ConditionalPathPoint* mvp = (ConditionalPathPoint*)(&pt);
            Vec3& attachment = mvp->getLocation();
            double range[]{ mvp->get_range(0), mvp->get_range(1) };
            aStream << attachment[0] << " " << attachment[1] << " " << attachment[2] << " segment " << mvp->getBody().getName();
            
            if (mvp->hasCoordinate()) {
                const Coordinate& coord = mvp->getCoordinate();
                if (coord.getMotionType() == Coordinate::Rotational)
                    aStream << " ranges 1 " << coord.getName() << " (" << range[0] * SimTK_RADIAN_TO_DEGREE << ", " << range[1] * SimTK_RADIAN_TO_DEGREE << ")" << endl;
                else
                    aStream << " ranges 1 " << coord.getName() << " (" << range[0] << ", " << range[1] << ")" << endl;
            } else {
                aStream << " ranges 1 " 
                    << mvp->getConnector<Coordinate>("coordinate").getConnecteeName()
                    << " (0.0, 1.0)" << endl;
            }
        } else if (pt.getConcreteClassName()==("MovingPathPoint")) {
            MovingPathPoint* mpp = (MovingPathPoint*)(&pt);
            const Vec3& attachment = mpp->get_location();
            if (mpp->hasXCoordinate()) {
                aStream << "f" << addMuscleFunction(&mpp->get_x_location(), mpp->getXCoordinate().getMotionType(), Coordinate::Translational) << "(" << mpp->getXCoordinate().getName() << ") ";
            } else {
                aStream << attachment[0] << " ";
            }
            if (mpp->hasYCoordinate()) {
                aStream << "f" << addMuscleFunction(&mpp->get_y_location(), mpp->getYCoordinate().getMotionType(), Coordinate::Translational) << "(" << mpp->getYCoordinate().getName() << ") ";
            } else {
                aStream << attachment[1] << " ";
            }
            if (mpp->hasZCoordinate()) {
                aStream << "f" << addMuscleFunction(&mpp->get_z_location(), mpp->getZCoordinate().getMotionType(), Coordinate::Translational) << "(" << mpp->getZCoordinate().getName() << ")";
            } else {
                aStream << attachment[2];
            }
            aStream << " segment " << mpp->getBody().getName() << endl;
        } else {
            Vec3& attachment = pt.getLocation();
            aStream << attachment[0] << " " << attachment[1] << " " << attachment[2] << " segment " << pt.getBody().getName() << endl;
        }
    }
    aStream << "endpoints" << endl;

    Array<std::string> groupNames;
    aActuatorSet.getGroupNamesContaining(aMuscle.getName(),groupNames);
    if(groupNames.getSize()) {
        aStream << "begingroups" << endl;
        for(int i=0; i<groupNames.getSize(); i++)
            aStream << " " << groupNames[i];
        aStream << endl << "endgroups" << endl;
    }

    const PathWrapSet& wrapObjects = aMuscle.getGeometryPath().getWrapSet();
    for (int i=0; i<wrapObjects.getSize(); i++)
        aStream << "wrapobject " << wrapObjects.get(i).getWrapObjectName() << " " <<
        (dynamic_cast<WrapEllipsoid*>(&wrapObjects.get(i)) ? (wrapObjects.get(i).getMethodName()+" ") : "") <<
        "range " << wrapObjects.get(i).getStartPoint() << " " << wrapObjects.get(i).getEndPoint() << endl;

    if (dynamic_cast<Schutte1993Muscle_Deprecated*>(&aMuscle))
    {
        Schutte1993Muscle_Deprecated *szh = dynamic_cast<Schutte1993Muscle_Deprecated*>(&aMuscle);

        aStream << "max_force " << szh->getMaxIsometricForce() << endl;
        aStream << "optimal_fiber_length " << szh->getOptimalFiberLength() << endl;
        aStream << "tendon_slack_length " << szh->getTendonSlackLength() << endl;
        aStream << "pennation_angle " << szh->getPennationAngleAtOptimalFiberLength() * SimTK_RADIAN_TO_DEGREE << endl;
        aStream << "max_contraction_velocity " << szh->getMaxContractionVelocity() << endl;
        aStream << "timescale " << szh->getTimeScale() << endl;
        aStream << "muscle_model 4" << endl;

        aStream << "active_force_length_curve f" << addMuscleFunction(&szh->getActiveForceLengthCurve(), Coordinate::Translational, Coordinate::Translational) << endl;

        aStream << "passive_force_length_curve f" << addMuscleFunction(&szh->getPassiveForceLengthCurve(), Coordinate::Translational, Coordinate::Translational) << endl;

        aStream << "tendon_force_length_curve f" << addMuscleFunction(&szh->getTendonForceLengthCurve(), Coordinate::Translational, Coordinate::Translational) << endl;
    }
    else if (dynamic_cast<Thelen2003Muscle_Deprecated*>(&aMuscle))
    {
        Thelen2003Muscle_Deprecated *sdm = dynamic_cast<Thelen2003Muscle_Deprecated*>(&aMuscle);

        aStream << "max_force " << sdm->getMaxIsometricForce() << endl;
        aStream << "optimal_fiber_length " << sdm->getOptimalFiberLength() << endl;
        aStream << "tendon_slack_length " << sdm->getTendonSlackLength() << endl;
        aStream << "pennation_angle " << sdm->getPennationAngleAtOptimalFiberLength() * SimTK_RADIAN_TO_DEGREE << endl;
        aStream << "activation_time_constant " << sdm->getActivationTimeConstant() << endl;
        aStream << "deactivation_time_constant " << sdm->getDeactivationTimeConstant() << endl;
        aStream << "Vmax " << sdm->getVmax() << endl;
        aStream << "Vmax0 " << sdm->getVmax0() << endl;
        aStream << "FmaxTendonStrain " << sdm->getFmaxTendonStrain() << endl;
        aStream << "FmaxMuscleStrain " << sdm->getFmaxMuscleStrain() << endl;
        aStream << "KshapeActive " << sdm->getKshapeActive() << endl;
        aStream << "KshapePassive " << sdm->getKshapePassive() << endl;
        aStream << "damping " << sdm->getDamping() << endl;
        aStream << "Af " << sdm->getAf() << endl;
        aStream << "Flen " << sdm->getFlen() << endl;
        aStream << "muscle_model 9" << endl;
    }
    else if (dynamic_cast<Delp1990Muscle_Deprecated*>(&aMuscle))
    {
        Delp1990Muscle_Deprecated *szh = dynamic_cast<Delp1990Muscle_Deprecated*>(&aMuscle);

        aStream << "max_force " << szh->getMaxIsometricForce() << endl;
        aStream << "optimal_fiber_length " << szh->getOptimalFiberLength() << endl;
        aStream << "tendon_slack_length " << szh->getTendonSlackLength() << endl;
        aStream << "pennation_angle " << szh->getPennationAngleAtOptimalFiberLength() * SimTK_RADIAN_TO_DEGREE << endl;
        aStream << "max_contraction_velocity " << szh->getMaxContractionVelocity() << endl;
        aStream << "timescale " << szh->getTimeScale() << endl;
        aStream << "muscle_model 2" << endl;

        if (szh->getActiveForceLengthCurve())
            aStream << "active_force_length_curve f" << addMuscleFunction(szh->getActiveForceLengthCurve(), Coordinate::Translational, Coordinate::Translational) << endl;

        if (szh->getPassiveForceLengthCurve())
            aStream << "passive_force_length_curve f" << addMuscleFunction(szh->getPassiveForceLengthCurve(), Coordinate::Translational, Coordinate::Translational) << endl;

        if (szh->getTendonForceLengthCurve())
            aStream << "tendon_force_length_curve f" << addMuscleFunction(szh->getTendonForceLengthCurve(), Coordinate::Translational, Coordinate::Translational) << endl;

        if (szh->getForceVelocityCurve())
            aStream << "force_velocity_curve f" << addMuscleFunction(szh->getForceVelocityCurve(), Coordinate::Translational, Coordinate::Translational) << endl;
    }

    aStream << "endmuscle" << endl << endl;
    return true;
}