Commit 30678a10 authored by Florent Lamiraux's avatar Florent Lamiraux
Browse files

Cosmetic change: remove some debug output.

parent fa8e4dfa
...@@ -65,14 +65,6 @@ namespace hpp { ...@@ -65,14 +65,6 @@ namespace hpp {
vector_t value (constraint->outputSize ()); vector_t value (constraint->outputSize ());
matrix_t jacobian (constraint->outputSize (), matrix_t jacobian (constraint->outputSize (),
robot_->numberDof ()); robot_->numberDof ());
hppDout (info, "constraint->getName () = " << constraint->name ());
hppDout (info, "constraint->inputSize () = " <<
constraint->inputSize ());
hppDout (info, "constraint->outputSize () = " <<
constraint->outputSize ());
hppDout (info, "value.rows () = " << value.rows ());
hppDout (info, "jacobian.rows () = " << jacobian.rows ());
hppDout (info, "jacobian.cols () = " << jacobian.cols ());
constraints_.push_back (FunctionValueAndJacobian_t (constraint, value, constraints_.push_back (FunctionValueAndJacobian_t (constraint, value,
jacobian)); jacobian));
computeIntervals (); computeIntervals ();
...@@ -96,13 +88,10 @@ namespace hpp { ...@@ -96,13 +88,10 @@ namespace hpp {
if (size > 0) { if (size > 0) {
interval.first = latestIndex + 1; interval.first = latestIndex + 1;
interval.second = size; interval.second = size;
hppDout (info, "Adding interval [" << interval.first << ","
<< interval.second << "]");
intervals_.push_back (interval); intervals_.push_back (interval);
} }
latestIndex = index; latestIndex = index;
} }
hppDout (info, "");
// Remove temporary element. // Remove temporary element.
lockedDofs_.pop_back (); lockedDofs_.pop_back ();
} }
...@@ -140,9 +129,6 @@ namespace hpp { ...@@ -140,9 +129,6 @@ namespace hpp {
matrix_t& jacobian = itConstraint->jacobian; matrix_t& jacobian = itConstraint->jacobian;
f (value, configuration); f (value, configuration);
f.jacobian (jacobian, configuration); f.jacobian (jacobian, configuration);
hppDout (info, "Function " << f.name ());
hppDout (info, "value: " << std::endl << value);
hppDout (info, "Jacobian: " << std::endl << jacobian);
nbRows = f.outputSize (); nbRows = f.outputSize ();
// Copy columns that are not locked // Copy columns that are not locked
size_type col = 0; size_type col = 0;
...@@ -157,8 +143,6 @@ namespace hpp { ...@@ -157,8 +143,6 @@ namespace hpp {
} }
row += nbRows; row += nbRows;
} }
hppDout (info, "Constraint value: " << value_);
hppDout (info, "Reduced Jacobian: " << reducedJacobian_);
} }
/// Convert vector of non locked degrees of freedom to vector of /// Convert vector of non locked degrees of freedom to vector of
...@@ -215,9 +199,7 @@ namespace hpp { ...@@ -215,9 +199,7 @@ namespace hpp {
Eigen::JacobiSVD <matrix_t> svd (reducedJacobian_, Eigen::JacobiSVD <matrix_t> svd (reducedJacobian_,
Eigen::ComputeThinU | Eigen::ComputeThinU |
Eigen::ComputeThinV); Eigen::ComputeThinV);
hppDout (info, "singular values: " << svd.singularValues ());
dqSmall_ = svd.solve(value_); dqSmall_ = svd.solve(value_);
hppDout (info, "dqSmall_ = " << dqSmall_);
smallToNormal (dqSmall_, dq_); smallToNormal (dqSmall_, dq_);
model::integrate (robot_, configuration, -alpha * dq_, configuration); model::integrate (robot_, configuration, -alpha * dq_, configuration);
// Increase alpha towards alphaMax // Increase alpha towards alphaMax
...@@ -230,6 +212,7 @@ namespace hpp { ...@@ -230,6 +212,7 @@ namespace hpp {
++iter; ++iter;
computeValueAndJacobian (configuration); computeValueAndJacobian (configuration);
}; };
hppDout (info, "number of iterations: " << iter);
if (squareNorm > squareErrorThreshold_) { if (squareNorm > squareErrorThreshold_) {
hppDout (info, "Projection failed."); hppDout (info, "Projection failed.");
return false; return false;
...@@ -254,9 +237,6 @@ namespace hpp { ...@@ -254,9 +237,6 @@ namespace hpp {
reducedProjector_ -= V1 * V1.transpose (); reducedProjector_ -= V1 * V1.transpose ();
projMinusFromSmall_ = reducedProjector_ * toMinusFromSmall_; projMinusFromSmall_ = reducedProjector_ * toMinusFromSmall_;
smallToNormal (projMinusFromSmall_, projMinusFrom_); smallToNormal (projMinusFromSmall_, projMinusFrom_);
hppDout (info, "projMinusFromSmall_ = "
<< projMinusFromSmall_.transpose ());
hppDout (info, "projMinusFrom_ = " << projMinusFrom_.transpose ());
model::integrate (robot_, from, projMinusFrom_, result); model::integrate (robot_, from, projMinusFrom_, result);
} }
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment