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&) { } }
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()); // } }
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) ); }
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) ); }
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; }
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 ... } // ... }
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); } }
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)) } }
// 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; }
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; }