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;
}
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;
}
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 ...
}
// ...
}
/**
* 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;
}
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;
}
