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;
}
