Commit ee4db6bb authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

Add C++ test projection.cc

parent 78b0c74c
......@@ -41,6 +41,10 @@ ADD_REQUIRED_DEPENDENCY("hpp-util")
ADD_REQUIRED_DEPENDENCY("hpp-model-urdf")
ADD_REQUIRED_DEPENDENCY("hrp2_14_description")
ADD_REQUIRED_DEPENDENCY("hpp-constraints >= 3")
ADD_REQUIRED_DEPENDENCY("hpp-wholebody-step >= 3")
ADD_REQUIRED_DEPENDENCY("hpp-core >= 3")
ADD_SUBDIRECTORY(tests)
SETUP_PROJECT_FINALIZE()
......@@ -54,3 +54,7 @@ ADD_TESTCASE (body-positions-mesh TRUE)
ADD_TESTCASE (interbody-distances-mesh TRUE)
ADD_TESTCASE (interbody-distances-capsule TRUE)
ADD_TESTCASE (interbody-distance-time-of-computation TRUE)
ADD_TESTCASE (projection FALSE)
PKG_CONFIG_USE_DEPENDENCY(projection hpp-constraints)
PKG_CONFIG_USE_DEPENDENCY(projection hpp-wholebody-step)
PKG_CONFIG_USE_DEPENDENCY(projection hpp-core)
// Copyright (c) 2014, LAAS-CNRS
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
//
// This file is part of test-hpp.
// test-hpp is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// test-hpp is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// test-hpp. If not, see <http://www.gnu.org/licenses/>.
#include <algorithm>
#include <ctime>
#include <boost/accumulators/framework/accumulator_set.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/count.hpp>
#include <boost/accumulators/statistics/variance.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>
#include <Eigen/SVD>
#define ERROR_THRESHOLD 1e-4
#define MAX_ITERATIONS 20
#define NB_TRIES 100
#define RATE_WARNING 0.15
#define MESSAGE_INF(m) std::cout << m << std::endl
#define MESSAGE_WAR(m) std::cout << m << std::endl
#define MESSAGE_ERR(m) std::cerr << m << std::endl
#include <hpp/model/urdf/util.hh>
#include <hpp/model/device.hh>
#include <hpp/model/configuration.hh>
#include <hpp/constraints/relative-transformation.hh>
#include <hpp/wholebody-step/static-stability-constraint.hh>
#include <hpp/core/differentiable-function.hh>
#include <hpp/core/basic-configuration-shooter.hh>
#include <hpp/core/config-projector.hh>
#include <hpp/util/timer.hh>
using namespace boost::accumulators;
using hpp::model::Device;
using hpp::model::DevicePtr_t;
using hpp::model::JointPtr_t;
using hpp::core::DifferentiableFunctionPtr_t;
using hpp::core::Constraint;
using hpp::core::ConfigProjector;
using hpp::core::ConfigProjectorPtr_t;
using hpp::core::BasicConfigurationShooter;
using hpp::core::ConfigurationShooter;
using hpp::core::Configuration_t;
using hpp::core::ConfigurationOut_t;
using hpp::core::ConfigurationPtr_t;
using hpp::debug::Timer;
namespace hpp_test {
DevicePtr_t robot;
std::vector <std::vector <DifferentiableFunctionPtr_t> > functionSets;
template <typename T>
void recursiveSubPartOf (const std::vector <T>& set, std::vector < std::vector <T> >& subparts) {
typedef typename std::vector < T > Set_t;
typedef std::vector < Set_t > Sets_t;
if (set.size () == 0) {
subparts.push_back (Set_t (0));
return;
}
Set_t subset (set.size() - 1);
for (size_t i = 0; i < subset.size(); i++)
subset[i] = set[i+1];
Sets_t subsetParts;
recursiveSubPartOf <T> (subset, subsetParts);
for (typename Sets_t::const_iterator it = subsetParts.begin ();
it != subsetParts.end (); it++) {
subparts.push_back (*it);
subparts.push_back (*it);
subparts.back ().push_back (set[0]);
}
}
void loadHRP2 (DevicePtr_t robot) {
hpp::model::urdf::loadRobotModel (
robot,
"freeflyer",
"hrp2_14_description",
"hrp2_14",
"",
"");
robot->rootJoint ()->lowerBound (0, -1);
robot->rootJoint ()->lowerBound (1, -1);
robot->rootJoint ()->lowerBound (2, -1);
robot->rootJoint ()->upperBound (0, 1);
robot->rootJoint ()->upperBound (1, 1);
robot->rootJoint ()->upperBound (2, 1);
robot->rootJoint ()->isBounded (0, true);
robot->rootJoint ()->isBounded (1, true);
robot->rootJoint ()->isBounded (2, true);
}
void loadHRP2constraints (bool partOf = false) {
/// Define the constraints
JointPtr_t leftAnkle = robot->getJointByName ("LLEG_JOINT5");
JointPtr_t rightAnkle = robot->getJointByName ("RLEG_JOINT5");
Configuration_t nc = robot->neutralConfiguration ();
std::vector <DifferentiableFunctionPtr_t> funcs =
hpp::wholebodyStep::createSlidingStabilityConstraint (robot, leftAnkle, rightAnkle, nc);
functionSets.clear ();
if (partOf)
recursiveSubPartOf <DifferentiableFunctionPtr_t> (funcs, functionSets);
else
functionSets.push_back (funcs);
}
void loadPR2 (DevicePtr_t robot) {
hpp::model::urdf::loadRobotModel (
robot,
"planar",
"hpp_tutorial",
"pr2",
"",
"");
robot->rootJoint ()->lowerBound (0, -1);
robot->rootJoint ()->lowerBound (1, -1);
robot->rootJoint ()->upperBound (0, 1);
robot->rootJoint ()->upperBound (1, 1);
robot->rootJoint ()->isBounded (0, true);
robot->rootJoint ()->isBounded (1, true);
}
void loadPR2constraints (bool partOf = false) {
/// Define the constraints
JointPtr_t leftWrist = robot->getJointByBodyName ("l_gripper_tool_frame");
JointPtr_t rightWrist = robot->getJointByBodyName ("r_gripper_tool_frame");
Configuration_t nc = robot->neutralConfiguration ();
std::vector <DifferentiableFunctionPtr_t> funcs;
fcl::Transform3f transform; transform.setIdentity ();
funcs.push_back (hpp::constraints::RelativeTransformation::create (robot, leftWrist, rightWrist, transform));
functionSets.clear ();
if (partOf)
recursiveSubPartOf <DifferentiableFunctionPtr_t> (funcs, functionSets);
else
functionSets.push_back (funcs);
}
}
namespace projection {
struct Result {
Configuration_t before, after;
long long us;
bool success;
double error;
};
typedef std::vector <Result> ResultVector;
typedef accumulator_set <double, stats<tag::variance> > Accumulator;
void shootConfig (ResultVector& results, ConfigurationShooter* cs)
{
ConfigurationPtr_t cfg;
for (size_t i = 0; i < results.size (); i++) {
cfg = cs->shoot ();
results[i].before = *cfg;
}
}
void project (ConfigProjectorPtr_t cp, ResultVector& results, Accumulator& acc)
{
MESSAGE_INF ("Start " << results.size() << " projections...");
for (size_t i = 0; i < results.size (); i++) {
results[i].after = results[i].before;
Timer t = Timer (true);
results[i].success = cp->apply (results[i].after);
t.stop ();
results[i].us = t.duration ().total_microseconds ();
results[i].error = cp->residualError ();
if (!results[i].success) {
acc (results[i].error);
}
}
}
int process () {
int status = EXIT_SUCCESS;
using hpp_test::functionSets;
DevicePtr_t robot = hpp_test::robot;
BasicConfigurationShooter bcs (robot);
std::vector <ConfigProjectorPtr_t> cp;
for (size_t i = 0; i < functionSets.size(); i++) {
cp.push_back (ConfigProjector::create (robot, "", ERROR_THRESHOLD, MAX_ITERATIONS));
for (size_t j = 0; j < functionSets[i].size(); j++)
cp.back()->addConstraint (functionSets[i][j]);
}
MESSAGE_INF ("There are " << cp.size() << " projectors to be tested.");
Accumulator acc;
std::vector <Result> results (NB_TRIES);
shootConfig (results, &bcs);
for (size_t i = 0; i < cp.size (); i++) {
acc = Accumulator ();
project (cp[i], results, acc);
if (count (acc) > 0) {
if (count (acc) < RATE_WARNING * NB_TRIES) {
MESSAGE_WAR ("----------------------------------------------------------");
MESSAGE_WAR (*(cp[i]));
MESSAGE_WAR ("Error rate: " << count (acc) << " / " << NB_TRIES);
MESSAGE_WAR ("Residual error (mean, average): (" << mean (acc) << ", " << variance (acc) << ")");
MESSAGE_WAR ("----------------------------------------------------------");
} else {
MESSAGE_ERR ("----------------------------------------------------------");
MESSAGE_ERR (*(cp[i]));
MESSAGE_ERR ("Error rate: " << count (acc) << " / " << NB_TRIES);
MESSAGE_ERR ("Residual error (mean, average): (" << mean (acc) << ", " << variance (acc) << ")");
//MESSAGE (cp[i]->statistics ());
MESSAGE_ERR ("----------------------------------------------------------");
status = EXIT_FAILURE;
}
}
}
return status;
}
namespace hrp2 {
using hpp_test::loadHRP2;
using hpp_test::loadHRP2constraints;
int launch () {
/// Create the robot
DevicePtr_t robot = Device::create ("hrp2");
hpp_test::robot = robot;
loadHRP2 (robot);
loadHRP2constraints (true);
return process ();
}
}
namespace pr2 {
using hpp_test::loadPR2;
using hpp_test::loadPR2constraints;
int launch () {
/// Create the robot
DevicePtr_t robot = Device::create ("PR2");
hpp_test::robot = robot;
loadPR2 (robot);
loadPR2constraints (true);
return process ();
}
}
}
namespace svd {
using namespace hpp_test;
using namespace hpp::core;
class SVDTest : public Constraint {
public:
vector_t offsetFromConfig (ConfigurationIn_t /* config */) {return vector_t ();}
bool isSatisfied (ConfigurationIn_t config) {
computeValueAndJacobian (config);
return value_.squaredNorm () < squareErrorThreshold_;
}
void addConstraint (const DifferentiableFunctionPtr_t& constraint)
{
size_ += constraint->outputSize ();
vector_t value (constraint->outputSize ());
matrix_t jacobian (constraint->outputSize (),
robot_->numberDof ());
constraints_.push_back (FunctionValueAndJacobian_t (constraint, value,
jacobian));
value_.resize (size_);
Jacobian_.resize (size_, robot_->numberDof ());
}
bool impl_compute (ConfigurationOut_t config)
{
value_type alpha = .2;
value_type alphaMax = .95;
size_type errorDecreased = 3, iter = 0;
value_type previousSquareNorm =
std::numeric_limits<value_type>::infinity();
// Fill value and Jacobian
computeValueAndJacobian (config);
squareNorm_ = value_.squaredNorm ();
while (squareNorm_ > squareErrorThreshold_ && errorDecreased &&
iter < maxIterations_) {
doSVD ();
vector_t v (-alpha * dq_);
hpp::model::integrate (robot_, config, v, config);
// Increase alpha towards alphaMax
computeValueAndJacobian (config);
alpha = alphaMax - .8*(alphaMax - alpha);
squareNorm_ = value_.squaredNorm ();
hppDout (info, "squareNorm = " << squareNorm_);
--errorDecreased;
if (squareNorm_ < previousSquareNorm) errorDecreased = 3;
previousSquareNorm = squareNorm_;
++iter;
};
if (squareNorm_ > squareErrorThreshold_) {
return false;
}
return true;
}
bool doSVD ()
{
std::vector <size_type> nonZeroCols, zeroCols;
for (size_type iCol = 0; iCol < Jacobian_.cols (); iCol++) {
if (Jacobian_.col (iCol).isZero ())
zeroCols.push_back (iCol);
else
nonZeroCols.push_back (iCol);
}
reducedJacobian_.resize (size_, nonZeroCols.size ());
for (size_t i = 0; i < nonZeroCols.size (); i++)
reducedJacobian_.col (i) = Jacobian_.col (nonZeroCols[i]);
Eigen::JacobiSVD <matrix_t> smallSVD (reducedJacobian_,
Eigen::ComputeThinU | Eigen::ComputeThinV);
dqSmall_ = smallSVD.solve(value_);
dq_.setZero ();
for (size_t i = 0; i < nonZeroCols.size (); i++)
dq_ (nonZeroCols[i]) = dqSmall_ (i);
return true;
}
bool doSVDWithCheck ()
{
Timer timeFull, timeSmall;
timeFull.start ();
Eigen::JacobiSVD <matrix_t> svd (Jacobian_,
Eigen::ComputeThinU | Eigen::ComputeThinV);
dq_ = svd.solve(value_);
timeFull.stop ();
timeSmall.start ();
std::vector <size_type> nonZeroCols, zeroCols;
for (size_type iCol = 0; iCol < Jacobian_.cols (); iCol++) {
if (Jacobian_.col (iCol).isZero ())
zeroCols.push_back (iCol);
else
nonZeroCols.push_back (iCol);
}
if ((size_type)(zeroCols.size () + nonZeroCols.size ()) != Jacobian_.cols ())
MESSAGE_ERR ("Size are different.");
reducedJacobian_.resize (size_, nonZeroCols.size ());
for (size_t i = 0; i < nonZeroCols.size (); i++)
reducedJacobian_.col (i) = Jacobian_.col (nonZeroCols[i]);
Eigen::JacobiSVD <matrix_t> smallSVD (reducedJacobian_,
Eigen::ComputeThinU | Eigen::ComputeThinV);
dqSmall_ = smallSVD.solve(value_);
timeSmall.stop ();
vector_t dqZeroPart (zeroCols.size());
vector_t dqNonZeroPart (nonZeroCols.size());
for (size_t i = 0; i < nonZeroCols.size (); i++)
dqNonZeroPart (i) = dq_ (nonZeroCols[i]);
for (size_t i = 0; i < zeroCols.size (); i++) {
dqZeroPart (i) = dq_ (zeroCols[i]);
dq_ (zeroCols[i]) = 0;
}
if ((dqSmall_ - dqNonZeroPart).isZero () && dqZeroPart.isZero ()) {
return true;
}
MESSAGE_INF ("Number of non-zero columns: " << nonZeroCols.size ());
MESSAGE_INF ("Normal SVD time: " << timeFull.duration ());
MESSAGE_INF ("Small SVD time: " << timeSmall.duration ());
if ((svd.nonzeroSingularValues () != smallSVD.nonzeroSingularValues ())
|| !(svd.singularValues () - smallSVD.singularValues ()).isZero ()) {
MESSAGE_ERR ("Normal SVD (" << svd.nonzeroSingularValues () << ") " << svd.singularValues ().transpose ());
MESSAGE_ERR ("Small SVD (" << smallSVD.nonzeroSingularValues () << ") " << smallSVD.singularValues ().transpose ());
}
MESSAGE_INF ("Normal SVD dq zero part error: " << dqZeroPart.norm ());
MESSAGE_INF ("Normal SVD dq: " << dqNonZeroPart.transpose ());
MESSAGE_INF ("Small SVD dq: " << dqSmall_.transpose ());
return false;
}
SVDTest (const DevicePtr_t& robot, value_type errorThreshold, size_type maxIterations):
Constraint ("SVDTest"), robot_ (robot), size_ (0),
squareErrorThreshold_ (errorThreshold * errorThreshold),
maxIterations_ (maxIterations), dq_ (robot_->numberDof ())
{}
std::ostream& print (std::ostream& os) const
{
os << "SVDTest contains";
for (NumericalConstraints_t::const_iterator it = constraints_.begin ();
it != constraints_.end (); it++) {
const DifferentiableFunction& f (*(it->function));
os << std::endl << f;
}
return os;
}
struct FunctionValueAndJacobian_t {
FunctionValueAndJacobian_t (DifferentiableFunctionPtr_t f,
vector_t v, matrix_t j): function (f),
value (v),
jacobian (j) {}
DifferentiableFunctionPtr_t function;
vector_t value;
matrix_t jacobian;
}; // struct FunctionValueAndJacobian_t
typedef std::vector < FunctionValueAndJacobian_t > NumericalConstraints_t;
void computeValueAndJacobian (ConfigurationIn_t configuration) {
size_type row = 0, nbRows = 0;
for (NumericalConstraints_t::iterator itConstraint =
constraints_.begin ();
itConstraint != constraints_.end (); itConstraint ++) {
DifferentiableFunction& f = *(itConstraint->function);
vector_t& value = itConstraint->value;
matrix_t& jacobian = itConstraint->jacobian;
f (value, configuration);
f.jacobian (jacobian, configuration);
nbRows = f.outputSize ();
value_.segment (row, nbRows) = value;
Jacobian_.middleRows (row, nbRows) = jacobian;
row += nbRows;
}
}
DevicePtr_t robot_;
size_t size_;
NumericalConstraints_t constraints_;
value_type squareErrorThreshold_;
value_type squareNorm_;
size_type maxIterations_;
mutable vector_t value_;
mutable matrix_t Jacobian_;
mutable vector_t dq_;
mutable matrix_t reducedJacobian_;
mutable vector_t dqSmall_;
}; // class SVDTest
struct Result {
Configuration_t before, after;
bool success;
};
typedef std::vector <Result> ResultVector;
void shootConfig (ResultVector& results, ConfigurationShooter* cs)
{
ConfigurationPtr_t cfg;
for (size_t i = 0; i < results.size (); i++) {
cfg = cs->shoot ();
results[i].before = *cfg;
}
}
unsigned int project (SVDTest* cp, ResultVector& results)
{
MESSAGE_INF ("Start " << results.size() << " projections...");
size_t failCount = 0;
for (size_t i = 0; i < results.size (); i++) {
results[i].after = results[i].before;
results[i].success = cp->apply (results[i].after);
if (!results[i].success)
failCount++;
}
return failCount;
}
int process () {
int status = EXIT_SUCCESS;
/// Create the robot
DevicePtr_t robot = hpp_test::robot;
BasicConfigurationShooter bcs (robot);
using hpp_test::functionSets;
std::vector <SVDTest*> cp;
for (size_t i = 0; i < functionSets.size(); i++) {
cp.push_back (new SVDTest (robot, ERROR_THRESHOLD, MAX_ITERATIONS));
for (size_t j = 0; j < functionSets[i].size(); j++)
cp.back()->addConstraint (functionSets[i][j]);
}
MESSAGE_INF ("There are " << cp.size() << " SVDTest to be tested.");
std::vector <Result> results (NB_TRIES);
shootConfig (results, &bcs);
for (size_t i = 0; i < cp.size (); i++) {
unsigned int f = project (cp[i], results);
if (f > 0) {
MESSAGE_ERR ("----------------------------------------------------------");
MESSAGE_ERR (*(cp[i]));
MESSAGE_ERR ("Error rate: " << f << " / " << NB_TRIES);
MESSAGE_ERR ("----------------------------------------------------------");
status = EXIT_FAILURE;
}
}
return status;
}
namespace hrp2 {
int launch () {
/// Create the robot
DevicePtr_t robot = Device::create ("hrp2");
hpp_test::robot = robot;
loadHRP2 (robot);
loadHRP2constraints (true);
return process ();
}
}
namespace pr2 {
int launch () {
/// Create the robot
DevicePtr_t robot = Device::create ("PR2");
hpp_test::robot = robot;
loadPR2 (robot);
loadPR2constraints (true);
return process ();
}
}
}
namespace shuffle {
struct Result {
Configuration_t before, after;
bool success;
double error;
};
typedef std::vector <Result> ResultVector;
typedef accumulator_set <double, stats<tag::variance> > Accumulator;
void shootConfig (ResultVector& results, ConfigurationShooter* cs)
{
ConfigurationPtr_t cfg;
for (size_t i = 0; i < results.size (); i++) {
cfg = cs->shoot ();
results[i].before = *cfg;
}
}
void project (const std::vector <ConfigProjectorPtr_t>& cp, Result& r, Accumulator& acc)
{
MESSAGE_INF ("Start " << cp.size() << " projections...");
bool shouldFail = false;
Configuration_t ref (r.before.size ());
r.after = r.before;
r.success = cp[0]->apply (r.after);
if (!r.success) {
MESSAGE_ERR ("Projection failed!");
shouldFail = true;
return;
}
ref = r.after;
for (size_t i = 1; i < cp.size (); i++) {
r.after = r.before;
r.success = cp[i]->apply (r.after);
r.error = cp[i]->residualError ();
if (shouldFail) {
if (r.success)
acc (r.error);
} else {
if (!r.success) {
acc (r.error);
} else if (!(r.after - ref).isZero ()) {
MESSAGE_ERR ("Result differs: " << (r.after - ref).transpose ());
acc (r.error);
}
}