Exemple #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;
}
void testSolveArbitrary() {
    static Random::Uniform uniform(2.0, 7.0);
    static Random::Gaussian random(0.0, 100.0);

    // Test random polynomials of various degrees with real coefficients.

    Vector realCoeff;
    Vector_<Complex> roots;
    for (int i = 0; i < 1000; ++i) {
        int length = uniform.getIntValue();
        realCoeff.resize(length);
        roots.resize(length-1);
        for (int j = 0; j < length; ++j)
            realCoeff[j] = random.getValue();
        PolynomialRootFinder::findRoots(realCoeff, roots);
        verifyRoots(realCoeff, roots);
    }

    // Test random polynomials of various degrees with complex coefficients.

    Vector_<Complex> complexCoeff;
    for (int i = 0; i < 1000; ++i) {
        int length = uniform.getIntValue();
        complexCoeff.resize(length);
        roots.resize(length-1);
        for (int j = 0; j < length; ++j)
            complexCoeff[j] = Complex(random.getValue(), random.getValue());
        PolynomialRootFinder::findRoots(complexCoeff, roots);
        verifyRoots(complexCoeff, roots);
    }

    // Verify that if the leading coefficient is zero, it throws an exception.

    try {
        realCoeff[0] = 0.0;
        PolynomialRootFinder::findRoots(realCoeff, roots);
        ASSERT(false)
    }
    catch (const PolynomialRootFinder::ZeroLeadingCoefficient&) {
    }
    try {
        complexCoeff[0] = 0.0;
        PolynomialRootFinder::findRoots(complexCoeff, roots);
        ASSERT(false)
    }
    catch (const PolynomialRootFinder::ZeroLeadingCoefficient&) {
    }
}
Exemple #3
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;
}
    inline static Vector_ rotate(const RotationBase<Rotation_, Usage_>& rotation, const Vector_& vector) {
        return static_cast<Vector_>(rotation.derived().rotate(vector.toImplementation()));
//    if(Usage_ == RotationUsage::ACTIVE){
//
//    } else {
//      return static_cast<Vector_>(eigen_impl::RotationMatrix<typename Rotation_::Scalar, Usage_>(rotation.derived().inverted()).toImplementation()*vector.toImplementation());
//    }
    }
Exemple #5
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) );
}
Exemple #6
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) );
}
Exemple #7
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);
}
 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;
 }
Exemple #9
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 ...
	}
	// ...
}
Exemple #10
0
void Force::calcForceContribution(const State&   state,
                           Vector_<SpatialVec>&  bodyForces,
                           Vector_<Vec3>&        particleForces,
                           Vector&               mobilityForces) const 
{
    const MultibodySystem& mbs = getForceSubsystem().getMultibodySystem();
    const SimbodyMatterSubsystem& matter = mbs.getMatterSubsystem();

    // Resize if necessary.
    bodyForces.resize(matter.getNumBodies());
    particleForces.resize(matter.getNumParticles());
    mobilityForces.resize(matter.getNumMobilities());

    // Set all forces to zero.
    bodyForces.setToZero();
    particleForces.setToZero();
    mobilityForces.setToZero();
    if (isDisabled(state)) return;

    // Add in force element contributions.
    getImpl().calcForce(state,bodyForces,particleForces,mobilityForces);
}
/**
 * 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);
    }
}
Exemple #12
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))
    }
}
Exemple #13
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;
}
bool test_Vector() {
  clock_t ss = clock();
  
  Vector_<double> a = { 1.99, 2.02, 6.7 };
  Vector_<double> b = { -0.1, 7.0, 2.22 };
  Vector_<double> c = { 3.3, -3.3, 0.0 };
  Vector_<double> cmpa = { 1.990, 2.02000, 6.700000};
  Vector_<double> a_b = {2.090, -4.980, 4.4800};
  
  // answer
  Vector_<double> a_mult_b = {-0.1990,  14.1400,  14.8740};
  Vector_<double> a_mult_c = {6.5670, -6.6660,     0};
  Vector_<double> b_mult_c = {-0.3300,  -23.1000,  0};
  
  double a_dot_b = 28.815;
  double a_dot_c = -0.099;
  double b_dot_c = -23.43;
  
  Vector_<double> a_dot2_99 = {5.9501,  6.0398, 20.0330};
  
  Vector_<double> a_cro_b = {-42.4156, -5.0878, 14.1320};
  Vector_<double> b_cro_a = {42.4156, 5.0878, -14.1320};
  Vector_<double> a_cro_c = {22.1100, 22.1100, -13.2330};
  Vector_<double> c_cro_a = {-22.1100, -22.1100, 13.2330};
  Vector_<double> b_cro_c = {7.3260, 7.3260, -22.7700};
  Vector_<double> c_cro_b = {-7.3260, -7.3260, 22.7700};
  
  Vector_<double> na = {0.2735, 0.2777, 0.9209};
  Vector_<double> nb = {-0.0136, 0.9531, 0.3023};
  Vector_<double> nc = {0.7071, -0.7071,  0};
  
  
  assert(fabs(a[0] - 1.99) < 0.0000001);
//  std::cout<<fabs(a[2] - 2.02) <<std::endl;
  assert(fabs(a[1] - 2.02) < 0.0000001);
  assert(fabs(a[2] - 6.7) < 0.0000001);
  // C++ STL bug
//  double sa[] = {6.7*2.22};
//  double sb[] = {14.8740};
//  vector<double> va(sa, sa + sizeof(double));
//  vector<double> vb(sb, sb + sizeof(double));
//  assert(va == vb);
  assert(a == a && b == b && c == c);
  assert(a == cmpa);
//  std::cout<<a.ele_mult(b)[2] - a_mult_b[2]<<std::endl;;
  assert(a.ele_mult(b) == a_mult_b);
  assert(a[1] - 2.02 < 1e-9);
  assert(a.ele_mult(c) == a_mult_c);
  assert(b.ele_mult(c) == b_mult_c);
  assert(a * b - a_dot_b < 1e-9);
  assert(a * c - a_dot_c < 1e-9);
  assert(b * c - b_dot_c < 1e-9);
  
  assert((a - b) == a_b);
  std::cout<<"Vector_ passed Test!"<<std::endl;
  
  
  Vec3d zero = { 0.0, 0.0, 0.0 };
  Vec3d va = { 1.99, 2.02, 6.7 };
  Vec3d v1 = {1.99, 2.02, 6.7 };
  Vec3d v2 = {-0.1, 7.0, 2.22};
  Vec3d v1_2 = {2.09, -4.98, 4.48};
  Vec3d v1p2 = {1.89, 9.02, 8.920000000000000};
  Vec3d nv1 = {0.273526921854589,  0.277650443289583, 0.920919787148616};
  Vec3d nv2 = {-0.013616044753350, 0.953123132734531,  0.302276193524380};
  Vec3d croV12 = {-42.415599999999998,  -5.08780,  14.132};
  double n2 = 7.344276683241175;
  double n1 = 7.2753350;
//  std::cout<<v1.Norm() - n1<<std::endl;
  assert(v1.Norm() - n1 < 1e-6);
  assert(v2.Norm() - n2 < 1e-6);
  assert(v1.Normalization() == nv1);
  assert(v2.Normalization() == nv2);
  
  assert(va * 2.99 == a_dot2_99);
  assert(va * 0.0 == zero );
  assert(-v1 == v1 * -1.0);
  assert(v1 * -1.0 == -1.0 * v1);
  assert(-(-v1) == v1);
  assert(v1 % v2 == croV12);
  
  assert(fabs(v1 * v2 - 28.815) < 1e-7);
  assert(v1 - v2 == v1_2);
  assert(v1 + v2 == v1p2);
  std::cout<<"Vec3d passed Test!"<<std::endl;
  
  clock_t  ed = clock();
  std::cout<<"Vector_ Test Running time: "<<
    (double) (ed - ss) / (double) CLOCKS_PER_SEC <<"s"<<std::endl;
  return true;
}
Exemple #15
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;
}