示例#1
0
void Force::DiscreteForces::
setAllBodyForces(State& state, const Vector_<SpatialVec>& bodyForcesInG) const {
    if (bodyForcesInG.size()==0) {clearAllBodyForces(state); return;}
    const int numBodies = getImpl().m_matter.getNumBodies();
    SimTK_ERRCHK2_ALWAYS(bodyForcesInG.size() == numBodies,
      "Force::DiscreteForces::setAllBodyForces()",
      "Body force vector bodyForcesInG had wrong size %d; "
      "should have been %d (0th entry is for Ground).",
      bodyForcesInG.size(), numBodies);

    getImpl().updAllBodyForces(state) = bodyForcesInG;
}
示例#2
0
Vector TaskSpace::Jacobian::operator*(const Vector& u) const
{
    Vector_<Vec3> Ju;
    m_tspace->getMatterSubsystem().multiplyByStationJacobian(getState(),
            m_tspace->getMobilizedBodyIndices(), m_tspace->getStations(),
            u, Ju);

    // Convert to a Vector.
    Vector out(3 * Ju.size());
    for (int i = 0; i < Ju.size(); ++i) {
        out[3 * i] = Ju[i][0];
        out[3 * i + 1] = Ju[i][1];
        out[3 * i + 2] = Ju[i][2];
    }

    return out;
}
示例#3
0
T absNorm( Vector_<T>& values, Vector_<T>& expected) {
   T norm = 0;

   for(int i=0;i<values.size(); i++ ) {
       norm += (fabs(values(i)) - fabs(expected(i))) * (fabs(values(i)) - fabs(expected(i))) + 
              (fabs(values(i)) - fabs(expected(i))) * (fabs(values(i)) - fabs(expected(i)));  
   }
   return( sqrt(norm) );
}
示例#4
0
T absNormComplex( Vector_<std::complex<T> >& values, Vector_<std::complex<T> >& expected) {
   T norm = 0;
 
   for(int i=0;i<values.size(); i++ ) {
       norm += (fabs(values(i).real()) - fabs(expected(i).real())) * (fabs(values(i).real()) - fabs(expected(i).real())) + 
             (fabs(values(i).imag()) - fabs(expected(i).imag())) * (fabs(values(i).imag()) - fabs(expected(i).imag()));  
   } 

   return( sqrt(norm) );
}
示例#5
0
void FactorQTZRep<T>::solve( const Vector_<T>& b, Vector_<T> &x ) const {

    SimTK_APIARGCHECK_ALWAYS(isFactored ,"FactorQTZ","solve",
       "No matrix was passed to FactorQTZ. \n"  );

    SimTK_APIARGCHECK2_ALWAYS(b.size()==nRow,"FactorQTZ","solve",
       "number of rows in right hand side=%d does not match number of rows in original matrix=%d \n",
        b.size(), nRow );


    Matrix_<T> m(maxmn,1);

    for(int i=0;i<b.size();i++) {
        m(i,0) = b(i);
    }
    Matrix_<T> r(nCol, 1 );
    doSolve( m, r );
    x.copyAssign(r);
}
示例#6
0
 void calcForce(const State& state, Vector_<SpatialVec>& bodyForces, Vector_<Vec3>& particleForces, 
                Vector& mobilityForces) const 
 {
     SimTK_TEST( f.size() == 0 || f.size() == mobilityForces.size() );
     SimTK_TEST( F.size() == 0 || F.size() == bodyForces.size() );
     if (f.size())
         mobilityForces += f;
     if (F.size())
         bodyForces += F;
 }
示例#7
0
void f_e1(Vector_& v)
{
	// ...
	try { // exceptions here are handled by the handler defined below

		v[v.size()] = 7; // try to access beyond the end of v
	}
	catch (out_of_range) {        // oops: out_of_range error
		// ... handle range error ...
	}
	// ...
}
/**
 * Compute the reaction forces and torques at all the joints in the model.
 * For each joint, what's reported is the force and torque the joint structure
 * applies on the child body at the joint frame attached to the child body.
 * Both vectors are expressed in the ground reference frame.
 *
 * It is necessary to call computeAccelerations() before this method
 * to get valid results.  The cost to calculate joint forces and moments is 114 
 * flops/body.
 *
 * @param rForces Matrix of reaction forces.  The size should be
 * at least NumberOfJoints x 3.
 * @param rTorques Matrix of reaction torques.  The size should be
 * at least NumberOfJoints x 3.
 */
void SimbodyEngine::computeReactions(const SimTK::State& s, Vector_<Vec3>& rForces, Vector_<Vec3>& rTorques) const
{
    // get the number of mobilized bodies in the underlying SimbodyMatterSubsystem
    int nmb = _model->getMatterSubsystem().getNumBodies();
    
    // get the number of bodies in the OpenSim model
    int nj = _model->getNumJoints();

    int nf = rForces.size();
    int ntorq = rTorques.size();

    // there may be more mobilized bodies than joint exposed in the OpenSim model
    // since joints and other components may use (massless) bodies internally
    assert(nmb >= nj);
    assert(nj == nf);
    assert(nf == ntorq);

    SimTK::Vector_<SpatialVec> reactionForces(nj);

    // Systems must be realized to acceleration stage
    _model->getMultibodySystem().realize(s, Stage::Acceleration);
    _model->getMatterSubsystem().calcMobilizerReactionForces(s, reactionForces);


    const JointSet &joints = _model->getJointSet();

    //Separate SimTK SpatialVecs to Forces and Torques  
    // SpatialVec = Vec2<Vec3 torque, Vec3 force>
    for(int i=0; i<nj; i++){
        const SimTK::MobilizedBodyIndex& ix = 
            joints[i].getChildFrame().getMobilizedBodyIndex();
         
        rTorques[i] = reactionForces[ix](0);
        rForces[i] = reactionForces[ix](1);
    }
}
示例#9
0
void verifyRoots(Vector_<Complex>& coefficients, Vector_<Complex>& roots, Real tol=1e-2) {
    for (int i = 0; i < roots.size(); ++i) {
        Complex sum = evalPoly(coefficients, roots[i]);
        ASSERT(equal(0.0, sum, tol))
    }
}
示例#10
0
// Evaluate a complex-coefficient polynomial at the given complex value. Should
// evaluate to zero if this is a root.
Complex evalPoly(Vector_<Complex>& coefficients, const Complex& value) {
    Complex sum = 0.0;
    for (int j = 0; j < coefficients.size(); ++j)
        sum = sum*value+coefficients[j];
    return sum;
}
示例#11
0
int main () {
    try { 
            // Default precision (Real, normally double) test.
        Matrix a(4,4, A);
        Vector b(4, B);
        Vector x_right(4, X);
        Vector x; // should get sized automatically to 4 by solve()

        FactorLU lu(a);  // perform LU factorization 

        lu.solve( b, x );  // solve for x given a right hand side 

        cout << " Real SOLUTION: " << x << "  errnorm=" << (x-x_right).norm() << endl;
        ASSERT((x-x_right).norm() < 10*SignificantReal);

            // float test

        Matrix_<float> af(4,4); for (int i=0; i<4; ++i) for (int j=0; j<4; ++j) af(i,j)=(float)a(i,j);
        Vector_<float> bf(4); for (int i=0; i<4; ++i) bf[i] = (float)b[i];
        Vector_<float> xf_right(4); for (int i=0; i<4; ++i) xf_right[i] = (float)x_right[i];
        Vector_<float> xf; // should get sized automatically to 4 by solve()

        FactorLU luf;
        luf.factor(af);
        luf.solve(bf, xf);

        cout << " float SOLUTION: " << xf << "  errnorm=" << (xf-xf_right).norm() << endl;
        const float SignificantFloat = NTraits<float>::getSignificant();
        ASSERT((xf-xf_right).norm() < 10*SignificantFloat);

        luf.factor(a);
        lu.solve( b, x );  // solve for x given a right hand side 
        cout << " Real SOLUTION: " << x << "  errnorm=" << (x-x_right).norm() << endl;
        ASSERT((x-x_right).norm() < 10*SignificantReal);
        
        Real C[4] = { 1.0,   2.0,
                      1.0,   3.0  };
        Matrix c(2,2, C);
        FactorLU clu(c);
        Matrix invC;
        clu.inverse(invC);
        cout << "Inverse c: " << endl;
        cout << invC[0] << endl;
        cout << invC[1] << endl;
        Real Z[4] = { 0.0,   0.0,
                     0.0,   0.0  };
        Matrix z(2,2, Z);
        FactorLU zlu(z);
        Vector_<double> xz;
        Vector_<double> bz(2);
        bz(1) = bz(0) = 0.0;
        zlu.solve( bz, xz );
        cout << " solve with mat all zeros : " << endl;
        for(int i=0;i<xz.size();i++) printf("%f ", xz(i) );  printf("\n");
   
        try {
            Matrix_<double> z0;
            FactorLU z0lu(z0);
            Vector_<double> bz0(0);
            z0lu.solve( bz0, xz );
            cout << " solve with mat(0,0) : " << endl;
            for(int i=0;i<xz.size();i++) printf("%f ", xz(i) );  printf("\n");
        } catch (const std::exception& e) {
             cout << "(EXPECTED EXCEPTION) NULL matrix test: " 
                 << e.what() << endl;
        }
    } 
    catch (const std::exception& e) {
        std::printf("FAILED: %s\n", e.what());
        return 1;
    }

    return 0;
}