示例#1
0
    void SpaceCraft::Step(double delta, double rxCommand, double rzCommand, double throttle)
    {
        // Rotation computing
        VectorD vCommand = Rotate(VectorD(rxCommand, 0, rzCommand), this->orientation);

        VectorD result = rmodel.Step(delta, vCommand);

        double norm = sqrt(result.get_norm2());

        QuaternionD q(1, 0, 0, 0);

        if(norm > 0)
        {
            q = QuaternionD(cos(norm * delta),
                            sin(norm * delta) * result.get_x() / norm,
                            sin(norm * delta) * result.get_y() / norm,
                            sin(norm * delta) * result.get_z() / norm);
        }

        this->orientation = q * this->orientation;
        this->orientation = (this->orientation /  sqrt(this->orientation.get_norm2()));

        // Translation computing
        vCommand = Rotate(VectorD(0, 0, throttle), this->orientation);

        this->position = tmodel.Step(delta, vCommand);
    }
示例#2
0
Color MirrorMaterial::shade(const Intersection &intersection, const Scene &scene) const
{
    VectorD vec_incident = -intersection.incident_ray.direction;
    if(intersection.incident_ray.recursion_depth >= scene.options().max_recursion)
    {
        return Color(0.0, 0.0, 0.0);
    }

    VectorD n = intersection.normal;
    // Reflection vector
    VectorD vec_r = 2 * n.dot(vec_incident) * n - vec_incident;
    vec_r.normalize();
    // Reflection ray
    Ray ray_r(intersection.hit_point, vec_r);
    ray_r.recursion_depth = intersection.incident_ray.recursion_depth + 1;
    // Trace reflection ray
    Color reflected_color;
    Intersection reflection_hit = scene.trace(ray_r);
    if(reflection_hit.exists)
    {
        reflected_color = reflection_hit.hit_object->material()->shade(reflection_hit, scene);
    }
    else
    {
        reflected_color = scene.background();
    }

    return reflected_color;
}
bool pinv_damped(const MatrixD &A, MatrixD *invA, Scalar lambda_max, Scalar eps) {

  //A (m x n) usually comes from a redundant task jacobian, therfore we consider m<n
  int m = A.rows() - 1;
  VectorD sigma;  //vector of singular values
  Scalar lambda2;
  int r = 0;

  JacobiSVD<MatrixD> svd_A(A.transpose(), ComputeThinU | ComputeThinV);
  sigma = svd_A.singularValues();
  if (((m > 0) && (sigma(m) > eps)) || ((m == 0) && (A.array().abs() > eps).any())) {
    for (int i = 0; i <= m; i++) {
      sigma(i) = 1.0 / sigma(i);
    }
    (*invA) = svd_A.matrixU() * sigma.asDiagonal() * svd_A.matrixV().transpose();
    return true;
  } else {
    lambda2 = (1 - (sigma(m) / eps) * (sigma(m) / eps)) * lambda_max * lambda_max;
    for (int i = 0; i <= m; i++) {
      if (sigma(i) > EPSQ)
        r++;
      sigma(i) = (sigma(i) / (sigma(i) * sigma(i) + lambda2));
    }
    //only U till the rank
    MatrixD subU = svd_A.matrixU().block(0, 0, A.cols(), r);
    MatrixD subV = svd_A.matrixV().block(0, 0, A.rows(), r);

    (*invA) = subU * sigma.asDiagonal() * subV.transpose();
    return false;
  }

}
bool pinv_QR_Z(const MatrixD &A, const MatrixD &Z0, MatrixD *invA, MatrixD *Z, Scalar lambda_max, Scalar eps) {
  VectorD sigma;  //vector of singular values
  Scalar lambda2;

  MatrixD AZ0t = (A * Z0).transpose();
  HouseholderQR < MatrixD > qr = AZ0t.householderQr();

  int m = A.rows();
  int p = Z0.cols();

  MatrixD Rt = MatrixD::Zero(m, m);
  bool invertible;
  MatrixD hR = (MatrixD) qr.matrixQR();
  MatrixD Y = ((MatrixD) qr.householderQ()).leftCols(m);

  //take the useful part of R
  for (int i = 0; i < m; i++) {
    for (int j = 0; j <= i; j++)
      Rt(i, j) = hR(j, i);
  }

  FullPivLU < MatrixD > invRt(Rt);
  invertible = fabs(invRt.determinant()) > eps;

  if (invertible) {
    *invA = Z0 * Y * invRt.inverse();
    *Z = Z0 * (((MatrixD) qr.householderQ()).rightCols(p - m));
    return true;
  } else {
    MatrixD R = MatrixD::Zero(m, m);
    //take the useful part of R
    for (int i = 0; i < m; i++) {
      for (int j = i; j < m; j++)  // TODO: is starting at i correct?
        R(i, j) = hR(i, j);
    }

    //perform the SVD of R
    JacobiSVD<MatrixD> svd_R(R, ComputeThinU | ComputeThinV);
    sigma = svd_R.singularValues();
    lambda2 = (1 - (sigma(m - 1) / eps) * (sigma(m - 1) / eps)) * lambda_max * lambda_max;
    for (int i = 0; i < m; i++) {
      sigma(i) = sigma(i) / (sigma(i) * sigma(i) + lambda2);
    }
    (*invA) = Z0 * Y * svd_R.matrixU() * sigma.asDiagonal() * svd_R.matrixV().transpose();

    *Z = Z0 * (((MatrixD) qr.householderQ()).rightCols(p - m));
    return false;
  }

}
示例#5
0
// Assemble lower-triangular matrix from log-diagonal and lower triangle and multiply by vector v
// [ exp(q0)        0        0        0 ]
// [      l0  exp(q2)        0        0 ]
// [      l1       l2  exp(q3)        0 ]
// [      l3       l4       l5  exp(q4) ]
VectorD Qtimesv(VectorD const& q, Vector const& l, VectorD const& v)
{
  VectorD ret(v.size());
  for (size_t i = 0; i < size_t(v.size()); ++i) {
    // let li = vectorSlice (Card i) (tri(i - 1)) l
    // let vi = vectorSlice (Card i) 0 v
    // vectorSum (li.*vi) + exp(q[i])*v[i]
    size_t lstart = tri(i - 1);
    double out = exp(q[i]) * v[i];
    for (size_t k = 0; k < i; ++k)
      out += l[lstart + k] * v[k];
    ret[i] = out;
  }
  return ret;
}
示例#6
0
inline
VectorD<T>::VectorD(const VectorD<T>& v1)
        : VectorDBase<T>(v1.get_size())
{
    VectorDBase<T>::assign(v1);
    *this = v1;
}
bool pinv(const MatrixD &A, MatrixD *invA, Scalar eps) {

  //A (m x n) usually comes from a redundant task jacobian, therfore we consider m<n
  int m = A.rows() - 1;
  VectorD sigma;  //vector of singular values

  JacobiSVD<MatrixD> svd_A(A.transpose(), ComputeThinU | ComputeThinV);
  sigma = svd_A.singularValues();
  if (((m > 0) && (sigma(m) > eps)) || ((m == 0) && (A.array().abs() > eps).any())) {
    for (int i = 0; i <= m; i++) {
      sigma(i) = 1.0 / sigma(i);
    }
    (*invA) = svd_A.matrixU() * sigma.asDiagonal() * svd_A.matrixV().transpose();
    return true;
  } else {
    return false;
  }
}
示例#8
0
VectorD SparseMatrix::multiply_vector(VectorD& v)
{
    VectorD res(v.size(), 0.0);
    for(map<pair<int, int>, double>::iterator it = values.begin(); it != values.end(); it++)
    {
        int row = it->first.first;
        int col = it->first.second;
        double val = it->second;
        res[row] += val*v[col];
    }
    return res;
}
示例#9
0
Color PhongMaterial::shade(const Intersection& intersection, const Scene& scene) const
{
    Color intensity;
    PointD3 point = intersection.hit_point;
    Options options = scene.options();

    if(options.diffuse_illumination || options.specular_illumination)
    {
        Color i_diffuse;
        Color i_specular;
        VectorD n = intersection.normal;
        for(auto &light : scene.lights())
        {
            VectorD l = -1 * light->direction_at(point);
            float n_dot_l = n.dot(l);
            Color i_light = light->intensity(point, scene);

            // === Diffuse reflection ===
            if(options.diffuse_illumination)
            {
                if(n_dot_l > 0.0f)
                {
                    i_diffuse = i_light * c_diffuse * n_dot_l;
                }
            }

            // === Specular reflection ===
            if(options.specular_illumination)
            {
                VectorD v = -1 * intersection.incident_ray.direction;
                VectorD r = 2 * n_dot_l * n - l;
                r.normalize();
                float v_dot_r = v.dot(r);
                if(v_dot_r > 0.0f)
                {
                   i_specular = i_light * c_specular * std::pow(v_dot_r, s);
                }
            }
        }
        intensity += i_diffuse;
        intensity += i_specular;
    }

    // === Ambient reflection ===
    if(options.ambient_illumination)
    {
        auto ambient_light = scene.ambient_light();
        if(ambient_light)
        {
          Color i_light = scene.ambient_light()->intensity(point, scene);
          Color i_ambient = i_light * c_ambient;
          intensity += i_ambient;
        }
    }


    return intensity;
}
示例#10
0
Color LambertMaterial::shade(const Intersection& intersection, const Scene& scene) const
{
    Color intensity;
    PointD3 point = intersection.hit_point;

    // === Diffuse reflection ===
    if(scene.options().diffuse_illumination)
    {
        Color intensity_diffuse;
        for(auto &light : scene.lights())
        {
            VectorD n = intersection.normal;
            VectorD l = -1.0 * light->direction_at(point);
            float dot = n.dot(l);
            if(dot > 0.0f)
            {
                intensity_diffuse += light->intensity(point, scene) * c_diffuse * dot;
            }
        }
        intensity += intensity_diffuse;
    }

    // === Ambient reflection ===
    if(scene.options().ambient_illumination)
    {
        auto ambient_light = scene.ambient_light();
        if(ambient_light)
        {
          Color ambient_intensity = scene.ambient_light()->intensity(point, scene);
          Color intensity_ambient = ambient_intensity * c_ambient;
          intensity += intensity_ambient;
        }
    }

    return intensity;
}
Scalar OSNSVelocityIK::SNSsingle(int priority,
                                const VectorD &higherPriorityJointVelocity,
                                const MatrixD &higherPriorityNull,
                                const MatrixD &jacobian,
                                const VectorD &task,
                                VectorD *jointVelocity,
                                MatrixD *nullSpaceProjector)
{
  //INITIALIZATION
  //VectorD tildeDotQ;
  MatrixD projectorSaturated;  //(((I-W_k)*P_{k-1})^#
  MatrixD JPinverse;  //(J_k P_{k-1})^#
  MatrixD temp;
  bool isW_identity;
  MatrixD barP = higherPriorityNull;  // remove the pointer arguments advantage... but I need to modify it
  Array<Scalar, Dynamic, 1> a, b;  // used to compute the task scaling factor
  bool limit_excedeed;
  bool singularTask = false;
  bool reachedSingularity = false;
  Scalar scalingFactor = 1.0;
  int mostCriticalJoint;

  //best solution
  Scalar bestScale = -0.1;
  MatrixD bestW;  //(only in OSNS)
  MatrixD bestInvJP;
  VectorD bestDotQn;
  MatrixD bestTildeP;

  VectorD dotQn;  //saturate velocity in the null space
  VectorD dotQs;
  MatrixD tildeP;  // used in the  OSNS

  //these two are needed to consider also W=I in case of a non feasible task
  bool invJPcomputed = false;

  //nSat[priority] = 0;

  //Compute the solution with W=I it is needed anyway to obtain nullSpaceProjector
  //compute (J P)^#
  temp = jacobian * higherPriorityNull;
  singularTask = !pinv_damped_P(temp, &JPinverse, nullSpaceProjector);

  tildeP = MatrixD::Zero(n_dof, n_dof);
  dotQs = higherPriorityJointVelocity + JPinverse * (task - jacobian * higherPriorityJointVelocity);
  a = (JPinverse * task).array();
  b = dotQs.array() - a;
  getTaskScalingFactor(a, b, I, &scalingFactor, &mostCriticalJoint);

  //double scalingI=scalingFactor;
  if (scalingFactor >= 1.0) {
    // this is clearly the optimum since all joints velocity are computed with the pseudoinverse
    (*jointVelocity) = dotQs;
    W[priority] = I;
    dotQopt[priority] = dotQs;
    return scalingFactor;
  }

  if (singularTask) {
    // the task is singular so return a scaled damped solution (no SNS possible)
    if (scalingFactor >= 0.0) {
      W[priority] = I;
      (*jointVelocity) = higherPriorityJointVelocity + scalingFactor * JPinverse * task
          + tildeP * higherPriorityJointVelocity;
      dotQopt[priority] = *jointVelocity;
    } else {
      // the task is not executed
      W[priority] = I;
      *jointVelocity = higherPriorityJointVelocity;
      dotQopt[priority] = *jointVelocity;
      *nullSpaceProjector = higherPriorityNull;
    }
    return scalingFactor;
  }

  if (scalingFactor > bestScale) {
    //save best solution so far
    bestScale = scalingFactor;
    //bestTildeDotQ=tildeDotQ;
    bestInvJP = JPinverse;
    bestW = I;
    bestTildeP = tildeP;
    //bestPS=projectorSaturated;
    bestDotQn = VectorD::Zero(n_dof);
  }

  //W[priority] = I;  //test: do not use the warm start
//----------------------------------------------------------------------- END W=I

  //INIT
  dotQn = VectorD::Zero(n_dof);
  if (isIdentity (W[priority])) {
    isW_identity = true;
    dotQopt[priority] = dotQs;  // use the one computed above
  } else {
    isW_identity = false;
    for (int i = 0; i < n_dof; i++) {
      if ((W[priority])(i, i) < 0.1) {  // equal to 0.0 (safer)
        //nSat[priority]++;
        //I'm not considering a different dotQ for each task... this could be a little improvement
        if (dotQopt[priority](i) >= 0.0) {
          dotQn(i) = dotQmax(i) - higherPriorityJointVelocity(i);
        } else {
          dotQn(i) = dotQmin(i) - higherPriorityJointVelocity(i);
        }
      } else
        dotQn(i) = 0.0;
    }
  }

  //SNS
  int count = 0;
  do {
    reachedSingularity = false;
    count++;
    if (count > 2 * n_dof) {
#ifndef _ONLY_WARNING_ON_ERROR
      //ROS_ERROR("Infinite loop on SNS for task (%d)", priority);
      /*
       ROS_INFO("p:%d  scale:%f  mc:%d  sing:%d",priority,scalingFactor,mostCriticalJoint,(int)reachedSingularity);
       //##############################
       string s;
       stringstream buffer;
       streambuf * old = std::cout.rdbuf(buffer.rdbuf());
       cout << "W\n" << W[priority] << endl << "P(k-1)\n" <<(*higherPriorityNull)<< endl<<"Psat\n" << projectorSaturated <<endl << "dotQ\n" << dotQopt[priority] <<endl << "JPinverse\n" << JPinverse <<endl;
       s = buffer.str();
       ROS_INFO("\n p %d \n %s",priority,s.c_str());
       //#############################
       */
      exit(1);
#else
      //ROS_WARN("Infinite loop on SNS for task (%d)",priority);
      /*
       ROS_INFO("p:%d  scale:%f  mc:%d  sing:%d",priority,scalingFactor,mostCriticalJoint,(int)reachedSingularity);
       //##############################
       string s;
       stringstream buffer;
       streambuf * old = std::cout.rdbuf(buffer.rdbuf());
       cout << "W\n" << W[priority]  <<endl;
       s = buffer.str();
       ROS_INFO("\n bs %f \n %s",scalingFactor,s.c_str());
       //#############################
       */
      // the task is not executed
      if (bestScale >= 0.0) {
        W[priority]=bestW;
        dotQopt[priority] = higherPriorityJointVelocity
            + bestInvJP *( bestScale * task - jacobian * higherPriorityJointVelocity)
            + bestTildeP * bestDotQn;
      } else {
        W[priority] = I;
        dotQopt[priority] = higherPriorityJointVelocity;
      }

      *jointVelocity = dotQopt[priority];
      return bestScale;
#endif
    }
    limit_excedeed = false;
    // If W=I everything is done --> go to the saturation phase

    if (!isW_identity && !invJPcomputed) {
      if (priority == 0) {  //for the primary task higherPriorityNull==I
        barP = W[0];
        projectorSaturated = (I - W[0]);
      } else {
        temp = (I - W[priority]);
        reachedSingularity |= !pinv_forBarP(temp, higherPriorityNull, &projectorSaturated);
        barP = (I - projectorSaturated) * higherPriorityNull;
      }
      temp = jacobian * barP;
      reachedSingularity |= !pinv(temp, &JPinverse);

      invJPcomputed = false;  //it needs to be computed for the next step
    }
    if (!isW_identity) {

      tildeP = (I - JPinverse * jacobian) * projectorSaturated;

      //compute the joint velocity
      dotQopt[priority] = higherPriorityJointVelocity
          + JPinverse * (task - jacobian * higherPriorityJointVelocity)
          + tildeP * dotQn;

      //compute the scaling factor
      a = (JPinverse * task).array();
      b = dotQopt[priority].array() - a;
      getTaskScalingFactor(a, b, W[priority], &scalingFactor, &mostCriticalJoint);

      if ((scalingFactor >= 1.0) || (scalingFactor < 0)) {

        //check optimality
        if (!isOptimal(priority, dotQopt[priority], tildeP, &W[priority], &dotQn)) {
          //modified W and dotQn
          limit_excedeed = true;
          //ROS_INFO("non OPT");
          continue;
        }
      }
      if (scalingFactor >= 1.0) {  //solution found

        //here limit_excedeed=false
        continue;
      }

      //if no solution found
      limit_excedeed = true;

      // is scaled an optimum
      if (scalingFactor >= 0) {
        dotQs = higherPriorityJointVelocity
            + JPinverse * (scalingFactor * task - jacobian * higherPriorityJointVelocity) + tildeP * dotQn;
        if (!isOptimal(priority, dotQs, tildeP, &W[priority], &dotQn)) {
          //ROS_INFO("non OPT s");
          //modified W and dotQn
          limit_excedeed = true;
          continue;
        }
      }

      if (scalingFactor > bestScale) {

        //save best solution so far
        bestScale = scalingFactor;
        bestInvJP = JPinverse;
        bestW = W[priority];
        bestTildeP = tildeP;
        bestDotQn = dotQn;
      }

    } else
      limit_excedeed = true;  //if it was W=I, to be here the limit is exceeded

    //nSat[priority]++;
    W[priority](mostCriticalJoint, mostCriticalJoint) = 0.0;
    isW_identity = false;
    if (dotQopt[priority](mostCriticalJoint) > dotQmax(mostCriticalJoint)) {
      dotQn(mostCriticalJoint) = dotQmax(mostCriticalJoint) - higherPriorityJointVelocity(mostCriticalJoint);
    } else {
      dotQn(mostCriticalJoint) = dotQmin(mostCriticalJoint) - higherPriorityJointVelocity(mostCriticalJoint);
    }

    //compute JPinverse
    if (priority == 0) {  //for the primary task higherPriorityNull==I
      barP = W[0];
      projectorSaturated = (I - W[0]);
    } else {
      temp = (I - W[priority]);
      reachedSingularity |= !pinv_forBarP(temp, higherPriorityNull, &projectorSaturated);
      barP = (I - projectorSaturated) * higherPriorityNull;
    }
    temp = jacobian * barP;
    reachedSingularity |= !pinv(temp, &JPinverse);

    invJPcomputed = true;
    //if reachedSingularity then take the best solution

    if ((reachedSingularity) || (scalingFactor < 1e-12)) {
      if (bestScale >= 0.0) {
        W[priority] = bestW;
        dotQopt[priority] = higherPriorityJointVelocity
            + bestInvJP * (bestScale * task - jacobian * higherPriorityJointVelocity)
            + bestTildeP * bestDotQn;
      } else {
        dotQopt[priority] = higherPriorityJointVelocity;
      }

      *jointVelocity = dotQopt[priority];
      return bestScale;
    }

  } while (limit_excedeed);
  *jointVelocity = dotQopt[priority];
  return scalingFactor;
}