/// Creates a sparse vector from a dense vector
SparseVectorN::SparseVectorN(const VectorN& x)
{
// setup the vector size
_size = x.size();
// determine the nonzero elements
_nelm = 0;
shared_array<bool> nz_elms(new bool[x.size()]);
for (unsigned i=0; i< x.size(); i++)
if (std::fabs(x[i]) < NEAR_ZERO)
{
_nelm++;
nz_elms[i] = true;
}
else
nz_elms[i] = false;
// declare memory
_indices = shared_array<unsigned>(new unsigned[_nelm]);
_data = shared_array<Real>(new Real[_nelm]);
// setup data
for (unsigned i=0, j=0; i< x.size(); i++)
if (nz_elms[i])
{
_indices[j] = i;
_data[j] = x[i];
j++;
}
}
void DenseVectorN::from(const VectorN &source)
{
int length = source.length();
x.resize({length}, 0.0);
Offsets offsets = source.getNonZeroOffsets();
for (Offset offset : offsets)
{
x[offset.first] = offset.second;
}
}
void
PrismaticJoint::articulation(const Task&, Context& context,
const ContinousStateValueVector& states,
PortValueList& portValues) const
{
VectorN jointForce;
if (mForcePort.empty())
jointForce.clear();
else
jointForce = portValues[mForcePort];
context.applyJointForce(jointForce);
}
double DenseVectorN::dot(const VectorN& vec) const
{
if (vec.getType() == DENSE)
{
return ddot((const DenseVectorN&)vec);
}
auto acc = 0.;
for (auto offset : vec.getNonZeroOffsets())
{
acc += x[offset.first] * offset.second;
}
return acc;
}
/// Determines (and sets) the value of Q from the axis and the inboard link and outboard link transforms
void ScrewJoint::determine_q(VectorN& q)
{
// get the inboard and outboard link pointers
RigidBodyPtr inboard = get_inboard_link();
RigidBodyPtr outboard = get_outboard_link();
// verify that the inboard and outboard links are set
if (!inboard || !outboard)
{
std::cerr << "ScrewJoint::determine_Q() called on NULL inboard and/or outboard links!" << std::endl;
assert(false);
return;
}
// if axis is not defined, can't use this method
if (std::fabs(_u.norm() - 1.0) > NEAR_ZERO)
{
std::cerr << "ScrewJoint::determine_Q() warning: some axes undefined; aborting..." << std::endl;
return;
}
// get the attachment points on the link (global coords)
Vector3d p1 = get_position_global(false);
Vector3d p2 = get_position_global(true);
// get the joint axis in the global frame
Vector3d ug = inboard->get_transform().mult_vector(_u);
// now, we'll project p2 onto the axis ug; points will be setup so that
// ug passes through origin on inboard
q.resize(num_dof());
q[DOF_1] = ug.dot(p2-p1)/_pitch;
}
void CLinearSolverLU<T>::SolveForwardBackward(MatrixNxN<T>& A, VectorN<T>& b, VectorN<T>& x, int iperm[])
{
int i,j,k,n;
n=A.rows();
for(i=0;i<n;i++)
{
j = iperm[i];
x(i)=b(j);
}
for(i=1;i<n;i++)
{
T sum=0.0;
for(k=0;k<i;k++)
sum+=A(i,k)*x(k);
x(i)=(x(i)-sum);
}
//x(n-1) is already done, because of:
x(n-1)=x(n-1)/A(n-1,n-1);
for(i=n-2;i>=0;i--)
{
T sum = 0.0;
for(k=i+1;k<n;k++)
sum += A(i,k)*x(k);
x(i)=(x(i)-sum)/A(i,i);
}
x.OutputVector();
}
// minimzer scaling modifier
void Minimizer_AlglibLBFGS::set_minimizer_x_scalings(const VectorN& scale_x) {
alglib::real_1d_array scalings(real_1d_array_with_size(scale_x.size()));
VectorN_TO_real_1d_array(scale_x, scalings);
alglib::minlbfgssetscale(m_minimizer_state, scalings);
alglib::minlbfgssetprecscale(m_minimizer_state);
m_scales = scale_x;
};
void make_vector(T dummy){
VectorN<T, 10> A;
VectorN<T, 10> B;
int i;
for(i = 0; i < A.dimension(); i++){
A(i, (T)1);
}
for(i = 0; i < A.dimension(); i++){
B(i, (T)1);
}
for(i = 0; i < A.dimension(); i++){
assert(A(i) == (T)1);
}
for(i = 0; i < B.dimension(); i++){
assert(B(i) == (T)1);
}
B = B + A;
for(i = 0; i < B.dimension(); i++){
assert(B(i) == (T)2);
}
B = B - A;
for(i = 0; i < B.dimension(); i++){
assert(B(i) == (T)1);
}
B = B * A;
for(i = 0; i < B.dimension(); i++){
assert(B(i) == (T)1);
}
B = -A;
for(i = 0; i < B.dimension(); i++){
assert(B(i) == (T)-1);
}
T ll = compute_norm(B);
assertFloat((T)ll, (T)sqrt(10));
cout << "test passed" << endl;
}
/// runs the simulator and updates all transforms
bool step(void* arg)
{
// get the simulator pointer
boost::shared_ptr<Simulator> s = *(boost::shared_ptr<Simulator>*) arg;
// get the generalized coordinates for all bodies in alphabetical order
std::vector<DynamicBodyPtr> bodies = s->get_dynamic_bodies();
std::sort(bodies.begin(), bodies.end(), compbody);
VectorN q;
outfile << s->current_time;
for (unsigned i=0; i< bodies.size(); i++)
{
bodies[i]->get_generalized_coordinates(DynamicBody::eRodrigues, q);
for (unsigned j=0; j< q.size(); j++)
outfile << " " << q[j];
}
outfile << std::endl;
// output the iteration #
if (OUTPUT_ITER_NUM)
std::cout << "iteration: " << ITER << " simulation time: " << s->current_time << std::endl;
if (Log<OutputToFile>::reporting_level > 0)
FILE_LOG(Log<OutputToFile>::reporting_level) << "iteration: " << ITER << " simulation time: " << s->current_time << std::endl;
// step the simulator and update visualization
clock_t pre_sim_t = clock();
s->step(STEP_SIZE);
clock_t post_sim_t = clock();
Real total_t = (post_sim_t - pre_sim_t) / (Real) CLOCKS_PER_SEC;
TOTAL_TIME += total_t;
// output the iteration / stepping rate
if (OUTPUT_SIM_RATE)
std::cout << "time to compute last iteration: " << total_t << " (" << TOTAL_TIME / ITER << "s/iter, " << TOTAL_TIME / s->current_time << "s/step)" << std::endl;
// update the iteration #
ITER++;
// check that maximum number of iterations or maximum time not exceeded
if (ITER >= MAX_ITER || s->current_time > MAX_TIME)
return false;
return true;
}
double getError(const vector<SamplePoint> &samples, const VectorN &curCoeff) {
double errSum = 0.0;
for (auto& s : samples) {
double hx = curCoeff.dotProduct(s.features);
errSum += (hx - s.value) * (hx - s.value);
}
return errSum / samples.size();
}
/// Computes the dot product between a sparse vector and a dense vector
Real SparseVectorN::dot(const VectorN& x) const
{
Real result = 0;
if (x.size() != _size)
throw MissizeException();
for (unsigned i=0; i< _nelm; i++)
result += x[_indices[i]] * _data[i];
return result;
}
VectorN getErrorGradients(const vector<SamplePoint> &samples, const VectorN &curCoeff) {
VectorN result {(unsigned) curCoeff.vals.size()};
for (unsigned i = 0; i < result.vals.size(); i++) {
// compute d'err/d'theta(i)
double derr = 0.0;
for (auto& s : samples) {
double hx = curCoeff.dotProduct(s.features);
derr += 2.0 * s.features.vals[i] * (hx - s.value);
}
result.vals[i] = derr / samples.size();
}
return result;
}
bool PathLCPSolver::solve_lcp(const MatrixN& MM, const VectorN& q, VectorN& z, double tol)
{
const Real INF = std::numeric_limits<Real>::max();
int i, j, k;
int variables;
shared_array<int> m_i, m_j;
shared_array<double> dq, lb, ub, m_ij, x_end;
MCP_Termination termination;
Output_Printf(Output_Log | Output_Status | Output_Listing, "%s: Standalone-C Link\n", Path_Version());
// We need a sparse representation of the matrix
assert(q.size() == MM.rows());
assert(q.size() == z.size());
// Copy q to dq
variables = q.size();
dq = shared_array<double>(new double[variables+1]);
for (i = 0;i < variables; i++)
dq[i] = q[i];
// Copy q to dq
variables = q.size();
dq = shared_array<double>(new double[variables+1]);
for (i = 0;i < variables; i++)
dq[i] = q[i];
int num_non_zeroes = 0;
for (i = 0;i < variables; i++)
for (j = 0;j < variables; j++)
if (std::fabs(MM(i,j)) > NEAR_ZERO)
num_non_zeroes++;
m_i = shared_array<int>(new int[num_non_zeroes + 1]);
m_j = shared_array<int>(new int[num_non_zeroes + 1]);
m_ij = shared_array<double>(new double[num_non_zeroes + 1]);
k = 0;
for (i = 0;i < variables; i++)
{
for (j = 0;j < variables; j++)
{
if (std::fabs(MM(i,j)) > NEAR_ZERO)
{
m_i[k] = i+1;
m_j[k] = j+1;
m_ij[k] = MM(i,j);
k++;
}
}
}
lb = shared_array<double>(new double[variables+1]);
ub = shared_array<double>(new double[variables+1]);
for (i = 0;i < variables; i++)
{
lb[i] = 0.0;
ub[i] = INF;
}
x_end = shared_array<double>(new double[variables+1]);
SimpleLCP(variables, num_non_zeroes, m_i.get(), m_j.get(), m_ij.get(), dq.get(), lb.get(), ub.get(), &termination, x_end.get(), tol);
// We now copy x_end into z
z.resize(variables);
for (i = 0;i < variables; i++)
z[i] = x_end[i];
return (termination == MCP_Solved);
}
/// Determines (and sets) the value of Q from the axes and the inboard link and outboard link transforms
void SphericalJoint::determine_q(VectorN& q)
{
const unsigned X = 0, Y = 1, Z = 2;
// get the inboard and outboard links
RigidBodyPtr inboard = get_inboard_link();
RigidBodyPtr outboard = get_outboard_link();
// verify that the inboard and outboard links are set
if (!inboard || !outboard)
throw NullPointerException("SphericalJoint::determine_q() called on NULL inboard and/or outboard links!");
// if any of the axes are not defined, can't use this method
if (std::fabs(_u[0].norm_sq() - 1.0) > NEAR_ZERO ||
std::fabs(_u[1].norm_sq() - 1.0) > NEAR_ZERO ||
std::fabs(_u[2].norm_sq() - 1.0) > NEAR_ZERO)
return;
// set proper size for q
q.resize(num_dof());
// get the link transforms
Matrix3 R_inboard, R_outboard;
inboard->get_transform().get_rotation(&R_inboard);
outboard->get_transform().get_rotation(&R_outboard);
// determine the joint transformation
Matrix3 R_local = R_inboard.transpose_mult(R_outboard);
// back out the transformation to z-axis
Matrix3 RU = _R.transpose_mult(R_local * _R);
// determine cos and sin values for q1, q2, and q3
Real s2 = RU(X,Z);
Real c2 = std::cos(std::asin(s2));
Real s1, c1, s3, c3;
if (std::fabs(c2) > NEAR_ZERO)
{
s1 = -RU(Y,Z)/c2;
c1 = RU(Z,Z)/c2;
s3 = -RU(X,Y)/c2;
c3 = RU(X,X)/c2;
assert(!std::isnan(s1));
assert(!std::isnan(c1));
assert(!std::isnan(s3));
assert(!std::isnan(c3));
}
else
{
// singular, we can pick any value for s1, c1, s3, c3 as long as the
// following conditions are satisfied
// c1*s3 + s1*c3*s2 = RU(Y,X)
// c1*c3 - s1*s3*s2 = RU(Y,Y)
// s1*s3 - c1*c3*s2 = RU(Z,X)
// s1*c3 + c1*s3*s2 = RU(Z,Y)
// so, we'll set q1 to zero (arbitrarily) and obtain
s1 = 0;
c1 = 1;
s3 = RU(Y,X);
c3 = RU(Y,Y);
}
// now determine q; only q2 can be determined without ambiguity
if (std::fabs(s1) < NEAR_ZERO)
q[DOF_2] = std::atan2(RU(X,Z), RU(Z,Z)/c1);
else
q[DOF_2] = std::atan2(RU(X,Z), -RU(Y,Z)/s1);
assert(!std::isnan(q[DOF_2]));
// if cos(q2) is not singular, proceed easily from here..
if (std::fabs(c2) > NEAR_ZERO)
{
q[DOF_1] = std::atan2(-RU(Y,Z)/c2, RU(Z,Z)/c2);
q[DOF_3] = std::atan2(-RU(X,Y)/c2, RU(X,X)/c2);
assert(!std::isnan(q[DOF_1]));
assert(!std::isnan(q[DOF_3]));
}
else
{
if (std::fabs(c1) > NEAR_ZERO)
q[DOF_3] = std::atan2((RU(Y,X) - s1*s2*c3)/c1, (RU(Y,Y) + s1*s2*s3)/c1);
else
q[DOF_3] = std::atan2((RU(Z,X) + c1*s2*c3)/s1, (RU(Z,Y) - c1*s2*s3)/s1);
if (std::fabs(c3) > NEAR_ZERO)
q[DOF_1] = std::atan2((RU(Y,X) - c1*s3)/(s2*c3), (-RU(Y,X) + s1*s3)/(s2*c3));
else
q[DOF_1] = std::atan2((-RU(Y,Y) + c1*c3)/(s2*s3), (RU(Z,Y) - s1*c3)/(s2*s3));
assert(!std::isnan(q[DOF_1]));
assert(!std::isnan(q[DOF_3]));
}
}