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