Beispiel #1
0
/// 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++;
    }
}
Beispiel #2
0
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);
}
Beispiel #4
0
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;
}
Beispiel #5
0
/// 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;
};
Beispiel #8
0
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;
}
Beispiel #9
0
/// 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;
}
Beispiel #10
0
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();
}
Beispiel #11
0
/// 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;
}
Beispiel #12
0
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;
}
Beispiel #13
0
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);
}
Beispiel #14
0
/// 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]));
  }
}