bool Basis::push (const Vector3& p)
{
int i, j;
double eps = 1e-32;
if (m==0) {
q0 = p;
c[0] = q0;
sqr_r[0] = 0;
}
else {
// set v_m to Q_m
v[m] = p-q0;
// compute the a_{m,i}, i< m
for (i=1; i<m; ++i) {
a[m].getAt(i) = 0;
for (j=0; j<3; ++j)
a[m].getAt(i) += v[i].getAt(j) * v[m].getAt(j);
a[m].getAt(i)*=(2/z[i]);
}
// update v_m to Q_m-\bar{Q}_m
for (i=1; i<m; ++i) {
for (j=0; j<3; ++j)
v[m].getAt(j) -= a[m].getAt(i)*v[i].getAt(j);
}
// compute z_m
z[m]=0;
z[m]+= normSquared(v[m]);
z[m]*=2;
// reject push if z_m too small
if (z[m]<eps*current_sqr_r) {
return false;
}
// update c, sqr_r
double e = -sqr_r[m-1];
e += normSquared(p-c[m-1]);
f[m]=e/z[m];
c[m] = c[m-1]+v[m]*f[m];
sqr_r[m] = sqr_r[m-1] + e*f[m]/2;
}
current_c = &(c[m]);
current_sqr_r = sqr_r[m];
s = ++m;
return true;
}
double Miniball::max_excess (It t, It i, It& pivot) const
{
const Vector3 c = B.center();
const double sqr_r = B.squared_radius();
double e, max_e = 0;
for (It k=t; k!=i; ++k) {
e = -sqr_r;
e += normSquared(*k-c);
if (e > max_e) {
max_e = e;
pivot = k;
}
}
return max_e;
}
Vector3
LineicModel::findClosest(const Vector3& p, real_t* ui) const{
real_t u0 = getFirstKnot();
real_t u1 = getLastKnot();
real_t deltau = (u1 - u0)/getStride();
Vector3 p1 = getPointAt(u0);
Vector3 res = p1;
real_t dist = normSquared(p-res);
Vector3 p2, pt;
real_t lu;
for(real_t u = u0 + deltau ; u <= u1 ; u += deltau){
p2 = getPointAt(u);
pt = p;
real_t d = __closestPointToSegment(pt,p1,p2,&lu);
if(d < dist){
dist = d;
res = pt;
if (ui != NULL) *ui = u + deltau * (lu -1);
}
p1 = p2;
}
return res;
}
double Basis::excess (const Vector3& p) const
{
double e = -current_sqr_r;
e += normSquared(p-*current_c);
return e;
}
int main(int argc, char** argv)
{
ros::init(argc,argv,"waypoint_generator");
ros::NodeHandle node;
ros::Rate loop_rate(50);
ros::Subscriber pos_sub, joy_sub;
pos_sub = node.subscribe("current_position",1,pos_callback);
joy_sub = node.subscribe("joy",1,joy_callback);
ros::Publisher des_pos_pub;
des_pos_pub = node.advertise<geometry_msgs::Vector3>("desired_position",1);
if ( node.getParam("timed_loop",timedLoop) ) {;}
else
{
ROS_ERROR("Are waypoints timed or position based?");
return 0;
}
if ( timedLoop && node.getParam("waypoint_timestep",timestep) ) {;}
else
{
ROS_ERROR("What is the timing between waypoints?");
return 0;
}
if ( node.getParam("robot_name",robotName) ) {;}
else
{
ROS_ERROR("Which robot number is this?");
return 0;
}
if ( node.getParam("trajectory_shape",shape) ) {;}
else
{
ROS_ERROR("What shape to follow?");
return 0;
}
std::vector<geometry_msgs::Vector3> desired_positions;
fillPositionList(desired_positions);
double bound = 0.05; //0.025;
des_pos_out = desired_positions[0];
int max = desired_positions.size()-1;
int arg = 0;
int count = 0;
int countBound = 10;
startWaypoints = 0;
while(ros::ok())
{
ros::spinOnce();
if (startControl && !startWaypoints)
{
des_pos_pub.publish(desired_positions[arg]);
}
else if( startControl && startWaypoints )
{
if(!timedLoop) // position based loop
{
if(normSquared(des_pos_out,curr_pos)<bound*bound)
{
count++;
}
if(count > countBound)
{
count = 0;
if(arg < max)
arg++;
else
arg = 0;
des_pos_out = desired_positions [arg];
}
}
else // time based loop
{
count++;
if( (double)count/100.0 > timestep )
{
count = 0;
if(arg < max)
arg++;
else
arg = 0;
}
}
des_pos_pub.publish(desired_positions[arg]);
ros::spinOnce();
loop_rate.sleep();
}
else
{
loop_rate.sleep();
}
}
}
OrthogonalProjections::OrthogonalProjections(const Matrix& originalVectors,
Real multiplierCutoff,
Real tolerance)
: originalVectors_(originalVectors),
multiplierCutoff_(multiplierCutoff),
numberVectors_(originalVectors.rows()),
dimension_(originalVectors.columns()),
validVectors_(true,originalVectors.rows()), // opposite way round from vector constructor
orthoNormalizedVectors_(originalVectors.rows(),
originalVectors.columns())
{
std::vector<Real> currentVector(dimension_);
for (Size j=0; j < numberVectors_; ++j)
{
if (validVectors_[j])
{
for (Size k=0; k< numberVectors_; ++k) // create an orthormal basis not containing j
{
for (Size m=0; m < dimension_; ++m)
orthoNormalizedVectors_[k][m] = originalVectors_[k][m];
if ( k !=j && validVectors_[k])
{
for (Size l=0; l < k; ++l)
{
if (validVectors_[l] && l !=j)
{
Real dotProduct = innerProduct(orthoNormalizedVectors_, k, orthoNormalizedVectors_,l);
for (Size n=0; n < dimension_; ++n)
orthoNormalizedVectors_[k][n] -= dotProduct*orthoNormalizedVectors_[l][n];
}
}
Real normBeforeScaling= norm(orthoNormalizedVectors_,k);
if (normBeforeScaling < tolerance)
{
validVectors_[k] = false;
}
else
{
Real normBeforeScalingRecip = 1.0/normBeforeScaling;
for (Size m=0; m < dimension_; ++m)
orthoNormalizedVectors_[k][m] *= normBeforeScalingRecip;
} // end of else (norm < tolerance)
} // end of if k !=j && validVectors_[k])
}// end of for (Size k=0; k< numberVectors_; ++k)
// we now have an o.n. basis for everything except j
Real prevNormSquared = normSquared(originalVectors_, j);
for (Size r=0; r < numberVectors_; ++r)
if (validVectors_[r] && r != j)
{
Real dotProduct = innerProduct(orthoNormalizedVectors_, j, orthoNormalizedVectors_,r);
for (Size s=0; s < dimension_; ++s)
orthoNormalizedVectors_[j][s] -= dotProduct*orthoNormalizedVectors_[r][s];
}
Real projectionOnOriginalDirection = innerProduct(originalVectors_,j,orthoNormalizedVectors_,j);
Real sizeMultiplier = prevNormSquared/projectionOnOriginalDirection;
if (std::fabs(sizeMultiplier) < multiplierCutoff_)
{
for (Size t=0; t < dimension_; ++t)
currentVector[t] = orthoNormalizedVectors_[j][t]*sizeMultiplier;
}
else
validVectors_[j] = false;
} // end of if (validVectors_[j])
projectedVectors_.push_back(currentVector);
} //end of j loop
numberValidVectors_ =0;
for (Size i=0; i < numberVectors_; ++i)
numberValidVectors_ += validVectors_[i] ? 1 : 0;
} // end of constructor
double Vector4d::norm() const
{
return sqrt( normSquared() );
}
double vec3::norm()const{
return sqrt(normSquared());
}
T SparseVectorCompressed<T>::norm() const { return Sqrt(normSquared()); }
T SparseVectorTemplate<T>::distanceSquared(const MyT& b) const
{
return normSquared() + b.normSquared() - Two*dot(b);
}
T SparseVectorTemplate<T>::norm() const { return Sqrt(normSquared()); }
/**
* Compute the Euclidian norm of vector \c v.
* @copydoc normSquared()
*/
inline double norm(const LinAlVector& v) {
return sqrt(normSquared(v));
}
void SecondOrderTrustRegion::doOptimize(OptimizationProblemSecond& problem) {
#ifdef NICE_USELIB_LINAL
bool previousStepSuccessful = true;
double previousError = problem.objective();
problem.computeGradientAndHessian();
double delta = computeInitialDelta(problem.gradientCached(), problem.hessianCached());
double normOldPosition = 0.0;
// iterate
for (int iteration = 0; iteration < maxIterations; iteration++) {
// Log::debug() << "iteration, objective: " << iteration << ", "
// << problem.objective() << std::endl;
if (previousStepSuccessful && iteration > 0) {
problem.computeGradientAndHessian();
}
// gradient-norm stopping condition
if (problem.gradientNormCached() < epsilonG) {
Log::debug() << "SecondOrderTrustRegion stopped: gradientNorm "
<< iteration << std::endl;
break;
}
LinAlVector gradient(problem.gradientCached().linalCol());
LinAlVector negativeGradient(gradient);
negativeGradient.multiply(-1.0);
double lambda;
int lambdaMinIndex = -1;
// FIXME will this copy the matrix? no copy needed here!
LinAlMatrix hessian(problem.hessianCached().linal());
LinAlMatrix l(hessian);
try {
//l.CHdecompose(); // FIXME
choleskyDecompose(hessian, l);
lambda = 0.0;
} catch (...) { //(LinAl::BLException& e) { // FIXME
const LinAlVector& eigenValuesHessian = LinAl::eigensym(hessian);
// find smallest eigenvalue
lambda = std::numeric_limits<double>::infinity();
for (unsigned int i = 0; i < problem.dimension(); i++) {
const double eigenValue = eigenValuesHessian(i);
if (eigenValue < lambda) {
lambda = eigenValue;
lambdaMinIndex = i;
}
}
const double originalLambda = lambda;
lambda = -lambda * (1.0 + epsilonLambda);
l = hessian;
for (unsigned int i = 0; i < problem.dimension(); i++) {
l(i, i) += lambda;
}
try {
//l.CHdecompose(); // FIXME
LinAlMatrix temp(l);
choleskyDecompose(temp, l);
} catch (...) { // LinAl::BLException& e) { // FIXME
/*
* Cholesky factorization failed, which should theortically not be
* possible (can still happen due to numeric problems,
* also note that there seems to be a bug in CHdecompose()).
* Try a really great lambda as last attempt.
*/
// lambda = -originalLambda * (1.0 + epsilonLambda * 100.0)
// + 2.0 * epsilonM;
lambda = fabs(originalLambda) * (1.0 + epsilonLambda * 1E5)
+ 1E3 * epsilonM;
// lambda = fabs(originalLambda);// * 15.0;
l = hessian;
for (unsigned int i = 0; i < problem.dimension(); i++) {
l(i, i) += lambda;
}
try {
//l.CHdecompose(); // FIXME
LinAlMatrix temp(l);
choleskyDecompose(temp, l);
} catch (...) { // (LinAl::BLException& e) { // FIXME
// Cholesky factorization failed again, give up.
l = hessian;
for (unsigned int i = 0; i < problem.dimension(); i++) {
l(i, i) += lambda;
}
const LinAlVector& eigenValuesL = LinAl::eigensym(l);
Log::detail()
<< "l.CHdecompose: exception" << std::endl
//<< e.what() << std::endl // FIXME
<< "lambda=" << lambda << std::endl
<< "l" << std::endl << l
<< "hessian" << std::endl << hessian
<< "gradient" << std::endl << gradient
<< "eigenvalues hessian" << std::endl << eigenValuesHessian
<< "eigenvalues l" << std::endl << eigenValuesL
<< std::endl;
return;
}
}
}
// FIXME will the copy the vector? copy is needed here
LinAlVector step(negativeGradient);
l.CHsubstitute(step);
double normStepSquared = normSquared(step);
double normStep = sqrt(normStepSquared);
// exact: if normStep <= delta
if (normStep - delta <= tolerance) {
// exact: if lambda == 0 || normStep == delta
if (std::fabs(lambda) < tolerance
|| std::fabs(normStep - delta) < tolerance) {
// done
} else {
LinAlMatrix eigenVectors(problem.dimension(), problem.dimension());
eigensym(hessian, eigenVectors);
double a = 0.0;
double b = 0.0;
double c = 0.0;
for (unsigned int i = 0; i < problem.dimension(); i++) {
const double ui = eigenVectors(i, lambdaMinIndex);
const double si = step(i);
a += ui * ui;
b += si * ui;
c += si * si;
}
b *= 2.0;
c -= delta * delta;
const double sq = sqrt(b * b - 4.0 * a * c);
const double root1 = 0.5 * (-b + sq) / a;
const double root2 = 0.5 * (-b - sq) / a;
LinAlVector step1(step);
LinAlVector step2(step);
for (unsigned int i = 0; i < problem.dimension(); i++) {
step1(i) += root1 * eigenVectors(i, lambdaMinIndex);
step2(i) += root2 * eigenVectors(i, lambdaMinIndex);
}
const double psi1
= dotProduct(gradient, step1)
+ 0.5 * productVMV(step1, hessian, step1);
const double psi2
= dotProduct(gradient, step2)
+ 0.5 * productVMV(step2, hessian, step2);
if (psi1 < psi2) {
step = step1;
} else {
step = step2;
}
}
} else {
for (unsigned int subIteration = 0;
subIteration < maxSubIterations;
subIteration++) {
if (std::fabs(normStep - delta) <= kappa * delta) {
break;
}
// NOTE specialized algorithm may be more effifient than solvelineq
// (l is lower triangle!)
// Only lower triangle values of l are valid (other implicitly = 0.0),
// but solvelineq doesn't know that -> explicitly set to 0.0
for (int i = 0; i < l.rows(); i++) {
for (int j = i + 1; j < l.cols(); j++) {
l(i, j) = 0.0;
}
}
LinAlVector y(step.size());
try {
y = solvelineq(l, step);
} catch (LinAl::Exception& e) {
// FIXME if we end up here, something is pretty wrong!
// give up the whole thing
Log::debug() << "SecondOrderTrustRegion stopped: solvelineq failed "
<< iteration << std::endl;
return;
}
lambda += (normStep - delta) / delta
* normStepSquared / normSquared(y);
l = hessian;
for (unsigned int i = 0; i < problem.dimension(); i++) {
l(i, i) += lambda;
}
try {
//l.CHdecompose(); // FIXME
LinAlMatrix temp(l);
choleskyDecompose(temp, l);
} catch (...) { // (LinAl::BLException& e) { // FIXME
Log::detail()
<< "l.CHdecompose: exception" << std::endl
// << e.what() << std::endl // FIXME
<< "lambda=" << lambda << std::endl
<< "l" << std::endl << l
<< std::endl;
return;
}
step = negativeGradient;
l.CHsubstitute(step);
normStepSquared = normSquared(step);
normStep = sqrt(normStepSquared);
}
}
// computation of step is complete, convert to NICE::Vector
Vector stepLimun(step);
// minimal change stopping condition
if (changeIsMinimal(stepLimun, problem.position())) {
Log::debug() << "SecondOrderTrustRegion stopped: change is minimal "
<< iteration << std::endl;
break;
}
if (previousStepSuccessful) {
normOldPosition = problem.position().normL2();
}
// set new region parameters (to be verified later)
problem.applyStep(stepLimun);
// compute reduction rate
const double newError = problem.objective();
//Log::debug() << "newError: " << newError << std::endl;
const double errorReduction = newError - previousError;
const double psi = problem.gradientCached().scalarProduct(stepLimun)
+ 0.5 * productVMV(step, hessian, step);
double rho;
if (std::fabs(psi) <= epsilonRho
&& std::fabs(errorReduction) <= epsilonRho) {
rho = 1.0;
} else {
rho = errorReduction / psi;
}
// NOTE psi and errorReduction checks added to the algorithm
// described in Ferid Bajramovic's Diplomarbeit
if (rho < eta1 || psi >= 0.0 || errorReduction > 0.0) {
previousStepSuccessful = false;
problem.unapplyStep(stepLimun);
delta = alpha2 * normStep;
} else {
previousStepSuccessful = true;
previousError = newError;
if (rho >= eta2) {
const double newDelta = alpha1 * normStep;
if (newDelta > delta) {
delta = newDelta;
}
} // else: don't change delta
}
// delta stopping condition
if (delta < epsilonDelta * normOldPosition) {
Log::debug() << "SecondOrderTrustRegion stopped: delta too small "
<< iteration << std::endl;
break;
}
}
#else // no linal
fthrow(Exception,
"SecondOrderTrustRegion needs LinAl. Please recompile with LinAl");
#endif
}
