Commit d8b94049 authored by Florent Lamiraux's avatar Florent Lamiraux
Browse files

Make test projection compile

  Test does not pass yet. Need to be fixed.
parent 889db853
...@@ -53,7 +53,9 @@ using hpp::model::DevicePtr_t; ...@@ -53,7 +53,9 @@ using hpp::model::DevicePtr_t;
using hpp::model::JointPtr_t; using hpp::model::JointPtr_t;
using hpp::core::DifferentiableFunctionPtr_t; using hpp::core::DifferentiableFunctionPtr_t;
using hpp::core::NumericalConstraint; using hpp::core::NumericalConstraint;
using hpp::core::NumericalConstraintPtr_t;
using hpp::core::Constraint; using hpp::core::Constraint;
using hpp::core::ConstraintPtr_t;
using hpp::core::ConfigProjector; using hpp::core::ConfigProjector;
using hpp::core::ConfigProjectorPtr_t; using hpp::core::ConfigProjectorPtr_t;
using hpp::core::BasicConfigurationShooter; using hpp::core::BasicConfigurationShooter;
...@@ -66,7 +68,7 @@ using hpp::debug::Timer; ...@@ -66,7 +68,7 @@ using hpp::debug::Timer;
namespace hpp_test { namespace hpp_test {
DevicePtr_t robot; DevicePtr_t robot;
std::vector <std::vector <DifferentiableFunctionPtr_t> > functionSets; std::vector <std::vector <NumericalConstraintPtr_t> > functionSets;
template <typename T> template <typename T>
void recursiveSubPartOf (const std::vector <T>& set, std::vector < std::vector <T> >& subparts) { void recursiveSubPartOf (const std::vector <T>& set, std::vector < std::vector <T> >& subparts) {
...@@ -113,11 +115,12 @@ namespace hpp_test { ...@@ -113,11 +115,12 @@ namespace hpp_test {
JointPtr_t leftAnkle = robot->getJointByName ("LLEG_JOINT5"); JointPtr_t leftAnkle = robot->getJointByName ("LLEG_JOINT5");
JointPtr_t rightAnkle = robot->getJointByName ("RLEG_JOINT5"); JointPtr_t rightAnkle = robot->getJointByName ("RLEG_JOINT5");
Configuration_t nc = robot->neutralConfiguration (); Configuration_t nc = robot->neutralConfiguration ();
std::vector <DifferentiableFunctionPtr_t> funcs = std::vector <NumericalConstraintPtr_t> funcs =
hpp::wholebodyStep::createSlidingStabilityConstraint (robot, leftAnkle, rightAnkle, nc); hpp::wholebodyStep::createSlidingStabilityConstraint
(robot, leftAnkle, rightAnkle, nc);
functionSets.clear (); functionSets.clear ();
if (partOf) if (partOf)
recursiveSubPartOf <DifferentiableFunctionPtr_t> (funcs, functionSets); recursiveSubPartOf <NumericalConstraintPtr_t> (funcs, functionSets);
else else
functionSets.push_back (funcs); functionSets.push_back (funcs);
} }
...@@ -143,12 +146,14 @@ namespace hpp_test { ...@@ -143,12 +146,14 @@ namespace hpp_test {
JointPtr_t leftWrist = robot->getJointByBodyName ("l_gripper_tool_frame"); JointPtr_t leftWrist = robot->getJointByBodyName ("l_gripper_tool_frame");
JointPtr_t rightWrist = robot->getJointByBodyName ("r_gripper_tool_frame"); JointPtr_t rightWrist = robot->getJointByBodyName ("r_gripper_tool_frame");
Configuration_t nc = robot->neutralConfiguration (); Configuration_t nc = robot->neutralConfiguration ();
std::vector <DifferentiableFunctionPtr_t> funcs; std::vector <NumericalConstraintPtr_t> funcs;
fcl::Transform3f transform; transform.setIdentity (); fcl::Transform3f transform; transform.setIdentity ();
funcs.push_back (hpp::constraints::RelativeTransformation::create (robot, leftWrist, rightWrist, transform)); funcs.push_back (NumericalConstraint::create
(hpp::constraints::RelativeTransformation::create
(robot, leftWrist, rightWrist, transform)));
functionSets.clear (); functionSets.clear ();
if (partOf) if (partOf)
recursiveSubPartOf <DifferentiableFunctionPtr_t> (funcs, functionSets); recursiveSubPartOf <NumericalConstraintPtr_t> (funcs, functionSets);
else else
functionSets.push_back (funcs); functionSets.push_back (funcs);
} }
...@@ -201,7 +206,7 @@ namespace projection { ...@@ -201,7 +206,7 @@ namespace projection {
for (size_t i = 0; i < functionSets.size(); i++) { for (size_t i = 0; i < functionSets.size(); i++) {
cp.push_back (ConfigProjector::create (robot, "", ERROR_THRESHOLD, MAX_ITERATIONS)); cp.push_back (ConfigProjector::create (robot, "", ERROR_THRESHOLD, MAX_ITERATIONS));
for (size_t j = 0; j < functionSets[i].size(); j++) for (size_t j = 0; j < functionSets[i].size(); j++)
cp.back()->add (NumericalConstraint::create (functionSets[i][j])); cp.back()->add (functionSets[i][j]);
} }
MESSAGE_INF ("There are " << cp.size() << " projectors to be tested."); MESSAGE_INF ("There are " << cp.size() << " projectors to be tested.");
...@@ -278,16 +283,22 @@ namespace svd { ...@@ -278,16 +283,22 @@ namespace svd {
bool isSatisfied (ConfigurationIn_t config, vector_t& error) { bool isSatisfied (ConfigurationIn_t config, vector_t& error) {
computeValueAndJacobian (config); computeValueAndJacobian (config);
return value_.squaredNorm () - squareErrorThreshold_; error = value_;
return value_.squaredNorm () < squareErrorThreshold_;
} }
void addConstraint (const DifferentiableFunctionPtr_t& constraint) ConstraintPtr_t copy() const
{ {
size_ += constraint->outputSize (); abort ();
vector_t value (constraint->outputSize ()); }
matrix_t jacobian (constraint->outputSize (), void addConstraint (const NumericalConstraintPtr_t& constraint)
{
DifferentiableFunctionPtr_t function = constraint->functionPtr ();
size_ += function->outputSize ();
vector_t value (function->outputSize ());
matrix_t jacobian (function->outputSize (),
robot_->numberDof ()); robot_->numberDof ());
constraints_.push_back (FunctionValueAndJacobian_t (constraint, value, constraints_.push_back (FunctionValueAndJacobian_t (function, value,
jacobian)); jacobian));
value_.resize (size_); value_.resize (size_);
Jacobian_.resize (size_, robot_->numberDof ()); Jacobian_.resize (size_, robot_->numberDof ());
...@@ -607,11 +618,11 @@ namespace shuffle { ...@@ -607,11 +618,11 @@ namespace shuffle {
using hpp_test::functionSets; using hpp_test::functionSets;
std::vector <ConfigProjectorPtr_t> cp; std::vector <ConfigProjectorPtr_t> cp;
std::vector <DifferentiableFunctionPtr_t> dfs = functionSets.front (); std::vector <NumericalConstraintPtr_t> dfs = functionSets.front ();
for (size_t i = 0; i < 10; i++) { for (size_t i = 0; i < 10; i++) {
cp.push_back (ConfigProjector::create (robot, "", ERROR_THRESHOLD, MAX_ITERATIONS)); cp.push_back (ConfigProjector::create (robot, "", ERROR_THRESHOLD, MAX_ITERATIONS));
for (size_t j = 0; j < dfs.size(); j++) for (size_t j = 0; j < dfs.size(); j++)
cp.back()->add (NumericalConstraint::create (dfs[j])); cp.back()->add (dfs[j]);
std::random_shuffle (dfs.begin (), dfs.end ()); std::random_shuffle (dfs.begin (), dfs.end ());
} }
MESSAGE_INF ("There are " << cp.size() << " ConfigProjector to be tested."); MESSAGE_INF ("There are " << cp.size() << " ConfigProjector to be tested.");
......
Markdown is supported
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