Commit 52e03acb authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

Merge remote-tracking branch 'hpp/devel'

parents c82562c0 880e6364
This diff is collapsed.
This diff is collapsed.
......@@ -136,85 +136,85 @@ void fillExpectedPositionsMap (PositionsMap_t& positions)
{
Eigen::Matrix4d position;
position << -0.885052, -0.465491, 4.52066e-10, 0.783276, 0.465491, -0.885052, 4.74263e-10, -0.628749, 1.79336e-10, 6.30181e-10, 1, 0.658642, 0, 0, 0, 1;
positions ["BODY"] = position;
positions ["BODY_0"] = position;
position << -0.855729, -0.517424, 4.52066e-10, 0.754954, 0.517424, -0.855729, 4.74263e-10, -0.613853, 1.41451e-10, 6.39751e-10, 1, 1.00634, 0, 0, 0, 1;
positions ["CHEST_LINK0"] = position;
positions ["CHEST_LINK0_0"] = position;
position << -0.853005, -0.517424, 0.0682238, 0.754954, 0.515777, -0.855729, -0.041252, -0.613853, 0.0797258, 6.39751e-10, 0.996817, 1.00634, 0, 0, 0, 1;
positions ["torso"] = position;
positions ["torso_0"] = position;
position << -0.926742, -0.369451, 0.0682238, 0.781208, 0.367377, -0.929157, -0.041252, -0.629728, 0.0786312, -0.0131662, 0.996817, 1.30214, 0, 0, 0, 1;
positions ["HEAD_LINK0"] = position;
positions ["HEAD_LINK0_0"] = position;
position << -0.926452, -0.369451, 0.072064, 0.781208, 0.367203, -0.929157, -0.0427742, -0.629728, 0.0827617, -0.0131662, 0.996482, 1.30214, 0, 0, 0, 1;
positions ["HEAD_LINK1"] = position;
positions ["HEAD_LINK1_0"] = position;
position << -0.841123, -0.517424, -0.157433, 0.631123, 0.508592, -0.855729, 0.0951931, -0.831126, -0.183975, 6.39751e-10, 0.982931, 1.1874, 0, 0, 0, 1;
positions ["LARM_LINK0"] = position;
positions ["LARM_LINK0_0"] = position;
position << -0.841123, -0.540542, -0.0180832, 0.631123, 0.508592, -0.801895, 0.313527, -0.831126, -0.183975, 0.254518, 0.949407, 1.1874, 0, 0, 0, 1;
positions ["LARM_LINK1"] = position;
positions ["LARM_LINK1_0"] = position;
position << -0.812884, -0.582145, -0.0180832, 0.631123, 0.548263, -0.775312, 0.313527, -0.831126, -0.196538, 0.244947, 0.949407, 1.1874, 0, 0, 0, 1;
positions ["LARM_LINK2"] = position;
positions ["LARM_LINK2_0"] = position;
position << -0.764318, -0.582145, 0.277355, 0.635644, 0.624569, -0.775312, 0.0938345, -0.909508, 0.160411, 0.244947, 0.956174, 0.950052, 0, 0, 0, 1;
positions ["LARM_LINK3"] = position;
positions ["LARM_LINK3_0"] = position;
position << -0.776657, -0.565577, 0.277355, 0.566305, 0.607755, -0.788561, 0.0938345, -0.932966, 0.16564, 0.241441, 0.956174, 0.711008, 0, 0, 0, 1;
positions ["LARM_LINK4"] = position;
positions ["LARM_LINK4_0"] = position;
position << -0.805465, -0.565577, 0.177054, 0.566305, 0.59104, -0.788561, 0.169833, -0.932966, 0.0435641, 0.241441, 0.969437, 0.711008, 0, 0, 0, 1;
positions ["l_wrist"] = position;
positions ["l_wrist_0"] = position;
position << 0.805465, 0.590905, -0.0453603, 0.560796, -0.59104, 0.795304, -0.134775, -0.933329, -0.0435641, 0.135366, 0.989837, 0.614083, 0, 0, 0, 1;
positions ["LARM_LINK6"] = position;
positions ["LARM_LINK6_0"] = position;
position << 0.805465, 0.565577, 0.177054, 0.56362, -0.59104, 0.788561, 0.169833, -0.924939, -0.0435641, -0.241441, 0.969437, 0.552461, 0, 0, 0, 1;
positions ["LHAND_LINK0"] = position;
positions ["LHAND_LINK0_0"] = position;
position << 0.805465, 0.590905, -0.0453603, 0.552092, -0.59104, 0.795304, -0.134775, -0.939934, -0.0435641, 0.135366, 0.989837, 0.542752, 0, 0, 0, 1;
positions ["LHAND_LINK1"] = position;
positions ["LHAND_LINK1_0"] = position;
position << 0.805465, 0.459559, 0.374208, 0.538173, -0.59104, 0.669314, 0.450212, -0.964872, -0.0435641, -0.583802, 0.810727, 0.62374, 0, 0, 0, 1;
positions ["LHAND_LINK2"] = position;
positions ["LHAND_LINK2_0"] = position;
position << 0.805465, 0.565577, 0.177054, 0.514888, -0.59104, 0.788561, 0.169833, -0.992886, -0.0435641, -0.241441, 0.969437, 0.573292, 0, 0, 0, 1;
positions ["LHAND_LINK3"] = position;
positions ["LHAND_LINK3_0"] = position;
position << 0.805465, 0.459559, 0.374208, 0.521459, -0.59104, 0.669314, 0.450212, -0.982647, -0.0435641, -0.583802, 0.810727, 0.555857, 0, 0, 0, 1;
positions ["LHAND_LINK4"] = position;
positions ["LHAND_LINK4_0"] = position;
position << -0.763048, -0.517424, 0.387338, 0.889835, 0.461383, -0.855729, -0.234207, -0.403261, 0.45264, 6.39751e-10, 0.891693, 1.1874, 0, 0, 0, 1;
positions ["RARM_LINK0"] = position;
positions ["RARM_LINK0_0"] = position;
position << -0.763048, -0.645532, 0.0323343, 0.889835, 0.461383, -0.579045, -0.672185, -0.403261, 0.45264, -0.497991, 0.739677, 1.1874, 0, 0, 0, 1;
positions ["RARM_LINK1"] = position;
positions ["RARM_LINK1_0"] = position;
position << -0.622616, -0.78186, 0.0323343, 0.889835, 0.565382, -0.478027, -0.672185, -0.403261, 0.541011, -0.400232, 0.739677, 1.1874, 0, 0, 0, 1;
positions ["RARM_LINK2"] = position;
positions ["RARM_LINK2_0"] = position;
position << -0.171563, -0.78186, 0.599385, 0.881751, -0.452207, -0.478027, -0.752993, -0.235215, 0.875257, -0.400232, -0.271551, 1.00248, 0, 0, 0, 1;
positions ["RARM_LINK3"] = position;
positions ["RARM_LINK3_0"] = position;
position << -0.378416, -0.705365, 0.599385, 0.731905, -0.565494, -0.336479, -0.752993, -0.0469667, 0.732815, -0.623893, -0.271551, 1.07037, 0, 0, 0, 1;
positions ["RARM_LINK4"] = position;
positions ["RARM_LINK4_0"] = position;
position << -0.361628, -0.705365, 0.60966, 0.731905, -0.586183, -0.336479, -0.737002, -0.0469667, 0.724993, -0.623893, -0.291793, 1.07037, 0, 0, 0, 1;
positions ["r_wrist"] = position;
positions ["r_wrist_0"] = position;
position << -0.361628, -0.10054, 0.926886, 0.65988, -0.586183, -0.748567, -0.309899, 0.0163189, 0.724993, -0.655393, 0.211768, 1.08561, 0, 0, 0, 1;
positions ["RARM_LINK6"] = position;
positions ["RARM_LINK6_0"] = position;
position << -0.361628, -0.705365, 0.60966, 0.602178, -0.586183, -0.336479, -0.737002, 0.0356113, 0.724993, -0.623893, -0.291793, 1.07243, 0, 0, 0, 1;
positions ["RHAND_LINK0"] = position;
positions ["RHAND_LINK0_0"] = position;
position << -0.361628, -0.10054, 0.926886, 0.604928, -0.586183, -0.748567, -0.309899, 0.051313, 0.724993, -0.655393, 0.211768, 1.0865, 0, 0, 0, 1;
positions ["RHAND_LINK1"] = position;
positions ["RHAND_LINK1_0"] = position;
position << -0.361628, -0.931676, -0.0347225, 0.688095, -0.586183, 0.256171, -0.768613, 0.0297781, 0.724993, -0.257598, -0.638771, 1.11057, 0, 0, 0, 1;
positions ["RHAND_LINK2"] = position;
positions ["RHAND_LINK2_0"] = position;
position << -0.361628, -0.705365, 0.60966, 0.690255, -0.586183, -0.336479, -0.737002, 0.0776053, 0.724993, -0.623893, -0.291793, 1.15032, 0, 0, 0, 1;
positions ["RHAND_LINK3"] = position;
positions ["RHAND_LINK3_0"] = position;
position << -0.361628, -0.931676, -0.0347225, 0.670434, -0.586183, 0.256171, -0.768613, 0.0825397, 0.724993, -0.257598, -0.638771, 1.14442, 0, 0, 0, 1;
positions ["RHAND_LINK4"] = position;
positions ["RHAND_LINK4_0"] = position;
position << -0.899269, -0.437396, 4.52066e-10, 0.755346, 0.437396, -0.899269, 4.74263e-10, -0.681852, 1.99088e-10, 6.24222e-10, 1, 0.655642, 0, 0, 0, 1;
positions ["LLEG_LINK0"] = position;
positions ["LLEG_LINK0_0"] = position;
position << -0.899269, -0.437359, -0.00562022, 0.755346, 0.437396, -0.899195, -0.011555, -0.681852, 1.99088e-10, -0.0128493, 0.999917, 0.655642, 0, 0, 0, 1;
positions ["LLEG_LINK1"] = position;
positions ["LLEG_LINK1_0"] = position;
position << -0.814879, -0.437359, 0.380379, 0.755346, 0.390225, -0.899195, -0.197922, -0.681852, 0.428598, -0.0128493, 0.903404, 0.655642, 0, 0, 0, 1;
positions ["LLEG_LINK2"] = position;
positions ["LLEG_LINK2_0"] = position;
position << -0.834845, -0.437359, -0.334292, 0.641233, 0.411289, -0.899195, 0.149298, -0.622475, -0.36589, -0.0128493, 0.930569, 0.384621, 0, 0, 0, 1;
positions ["LLEG_LINK3"] = position;
positions ["LLEG_LINK3_0"] = position;
position << -0.899269, -0.437359, -0.00562022, 0.726213, 0.437396, -0.899195, -0.011555, -0.698737, 5.55422e-09, -0.0128493, 0.999917, 0.105, 0, 0, 0, 1;
positions ["LLEG_LINK4"] = position;
positions ["LLEG_LINK4_0"] = position;
position << -0.899269, -0.437396, 6.87548e-09, 0.726213, 0.437396, -0.899269, 1.43734e-09, -0.698737, 5.55422e-09, 4.29986e-09, 1, 0.105, 0, 0, 0, 1;
positions ["l_ankle"] = position;
positions ["l_ankle_0"] = position;
position << -0.899269, -0.437396, 4.52066e-10, 0.811206, 0.437396, -0.899269, 4.74263e-10, -0.575646, 1.99088e-10, 6.24222e-10, 1, 0.655642, 0, 0, 0, 1;
positions ["RLEG_LINK0"] = position;
positions ["RLEG_LINK0_0"] = position;
position << -0.899269, -0.437359, -0.00566284, 0.811206, 0.437396, -0.899194, -0.0116426, -0.575646, 1.99088e-10, -0.0129467, 0.999916, 0.655642, 0, 0, 0, 1;
positions ["RLEG_LINK1"] = position;
positions ["RLEG_LINK1_0"] = position;
position << -0.813988, -0.437359, 0.382284, 0.811206, 0.389714, -0.899194, -0.198932, -0.575646, 0.430752, -0.0129467, 0.902378, 0.655642, 0, 0, 0, 1;
positions ["RLEG_LINK2"] = position;
positions ["RLEG_LINK2_0"] = position;
position << -0.838519, -0.437359, -0.324966, 0.69652, 0.412965, -0.899194, 0.144604, -0.515966, -0.355451, -0.0129467, 0.934605, 0.384928, 0, 0, 0, 1;
positions ["RLEG_LINK3"] = position;
positions ["RLEG_LINK3_0"] = position;
position << -0.899269, -0.437359, -0.00566284, 0.809318, 0.437396, -0.899194, -0.0116426, -0.527876, -1.28638e-09, -0.0129467, 0.999916, 0.105, 0, 0, 0, 1;
positions ["RLEG_LINK4"] = position;
positions ["RLEG_LINK4_0"] = position;
position << -0.899269, -0.437396, -1.11439e-09, 0.809318, 0.437396, -0.899269, 6.49853e-10, -0.527876, -1.28638e-09, 9.69619e-11, 1, 0.105, 0, 0, 0, 1;
positions ["r_ankle"] = position;
positions ["r_ankle_0"] = position;
}
......@@ -51,8 +51,6 @@ BOOST_AUTO_TEST_CASE (interbody_distances_time_of_computation)
using hpp::model::Device;
using hpp::model::Joint;
using hpp::model::Body;
typedef hpp::model::ObjectVector_t ObjectVector_t;
typedef hpp::model::JointVector_t JointVector_t;
typedef std::vector<double> vector_t;
std::ifstream fileConfig;
vector_t::size_type configSize;
......@@ -99,7 +97,8 @@ BOOST_AUTO_TEST_CASE (interbody_distances_time_of_computation)
humanoidRobot->computeDistances ();
}
gettimeofday (&t1, NULL);
double t = (t1.tv_sec - t0.tv_sec) + 1e-6*(t1.tv_usec - t0.tv_usec + 0.);
double t = (double)(t1.tv_sec - t0.tv_sec) +
1e-6*(double)(t1.tv_usec - t0.tv_usec);
std::cout << "Time for 1000 self-collision tests in seconds: "
<< t << std::endl;
}
......
......@@ -182,8 +182,6 @@ BOOST_AUTO_TEST_CASE (interbody_distances_capsule)
using hpp::model::Device;
using hpp::model::Joint;
using hpp::model::Body;
typedef hpp::model::ObjectVector_t ObjectVector_t;
typedef hpp::model::JointVector_t JointVector_t;
typedef std::vector<double> vector_t;
std::ifstream fileConfig;
std::ofstream fileHrp2;
......
......@@ -109,8 +109,6 @@ BOOST_AUTO_TEST_CASE (interbody_distances_mesh)
using hpp::model::Device;
using hpp::model::Joint;
using hpp::model::Body;
typedef hpp::model::ObjectVector_t ObjectVector_t;
typedef hpp::model::JointVector_t JointVector_t;
typedef std::vector<double> vector_t;
std::ifstream fileConfig;
std::ofstream fileHrp2;
......@@ -179,13 +177,13 @@ BOOST_AUTO_TEST_CASE (interbody_distances_mesh)
<< it->fcl.nearest_points [1][1] << ",\t"
<< it->fcl.nearest_points [1][2] << ",\t");
std::pair <std::string, std::string> key (body1, body2);
InterDistances_t::iterator it = interDistances.find (key);
InterDistances_t::iterator itInter = interDistances.find (key);
// Check that pair of bodies is in map
if (it == interDistances.end ()) {
if (itInter == interDistances.end ()) {
throw std::runtime_error ("pair (" + body1 + "," + body2 +
") is not in " + filename);
}
Distance_t distanceFile = it->second;
Distance_t distanceFile = itInter->second;
hppDout (info, "Checking pair " << body1 << ", " << body2);
distance.checkClose (distanceFile);
}
......
......@@ -51,6 +51,7 @@ using namespace boost::accumulators;
using hpp::model::Device;
using hpp::model::DevicePtr_t;
using hpp::model::JointPtr_t;
using hpp::core::ConfigurationShooterPtr_t;
using hpp::core::DifferentiableFunctionPtr_t;
using hpp::core::NumericalConstraint;
using hpp::core::NumericalConstraintPtr_t;
......@@ -59,6 +60,7 @@ using hpp::core::ConstraintPtr_t;
using hpp::core::ConfigProjector;
using hpp::core::ConfigProjectorPtr_t;
using hpp::core::BasicConfigurationShooter;
using hpp::core::BasicConfigurationShooterPtr_t;
using hpp::core::ConfigurationShooter;
using hpp::core::Configuration_t;
using hpp::core::ConfigurationOut_t;
......@@ -169,7 +171,7 @@ namespace projection {
typedef std::vector <Result> ResultVector;
typedef accumulator_set <double, stats<tag::variance> > Accumulator;
void shootConfig (ResultVector& results, ConfigurationShooter* cs)
void shootConfig (ResultVector& results, ConfigurationShooterPtr_t cs)
{
ConfigurationPtr_t cfg;
for (size_t i = 0; i < results.size (); i++) {
......@@ -200,7 +202,8 @@ namespace projection {
using hpp_test::functionSets;
DevicePtr_t robot = hpp_test::robot;
BasicConfigurationShooter bcs (robot);
BasicConfigurationShooterPtr_t bcs (BasicConfigurationShooter::create
(robot));
std::vector <ConfigProjectorPtr_t> cp;
for (size_t i = 0; i < functionSets.size(); i++) {
......@@ -212,7 +215,7 @@ namespace projection {
Accumulator acc;
std::vector <Result> results (NB_TRIES);
shootConfig (results, &bcs);
shootConfig (results, bcs);
for (size_t i = 0; i < cp.size (); i++) {
acc = Accumulator ();
......@@ -476,7 +479,7 @@ namespace svd {
};
typedef std::vector <Result> ResultVector;
void shootConfig (ResultVector& results, ConfigurationShooter* cs)
void shootConfig (ResultVector& results, ConfigurationShooterPtr_t cs)
{
ConfigurationPtr_t cfg;
for (size_t i = 0; i < results.size (); i++) {
......@@ -485,7 +488,7 @@ namespace svd {
}
}
unsigned int project (SVDTest* cp, ResultVector& results)
size_t project (SVDTest* cp, ResultVector& results)
{
MESSAGE_INF ("Start " << results.size() << " projections...");
size_t failCount = 0;
......@@ -503,7 +506,8 @@ namespace svd {
/// Create the robot
DevicePtr_t robot = hpp_test::robot;
BasicConfigurationShooter bcs (robot);
BasicConfigurationShooterPtr_t bcs (BasicConfigurationShooter::create
(robot));
using hpp_test::functionSets;
std::vector <SVDTest*> cp;
......@@ -515,10 +519,10 @@ namespace svd {
MESSAGE_INF ("There are " << cp.size() << " SVDTest to be tested.");
std::vector <Result> results (NB_TRIES);
shootConfig (results, &bcs);
shootConfig (results, bcs);
for (size_t i = 0; i < cp.size (); i++) {
unsigned int f = project (cp[i], results);
std::size_t f = project (cp[i], results);
if (f > 0) {
MESSAGE_ERR ("----------------------------------------------------------");
......@@ -566,7 +570,7 @@ namespace shuffle {
typedef std::vector <Result> ResultVector;
typedef accumulator_set <double, stats<tag::variance> > Accumulator;
void shootConfig (ResultVector& results, ConfigurationShooter* cs)
void shootConfig (ResultVector& results, ConfigurationShooterPtr_t cs)
{
ConfigurationPtr_t cfg;
for (size_t i = 0; i < results.size (); i++) {
......@@ -614,7 +618,8 @@ namespace shuffle {
/// Create the robot
DevicePtr_t robot = hpp_test::robot;
BasicConfigurationShooter bcs (robot);
BasicConfigurationShooterPtr_t bcs (BasicConfigurationShooter::create
(robot));
using hpp_test::functionSets;
std::vector <ConfigProjectorPtr_t> cp;
......@@ -628,7 +633,7 @@ namespace shuffle {
MESSAGE_INF ("There are " << cp.size() << " ConfigProjector to be tested.");
std::vector <Result> results (NB_TRIES);
shootConfig (results, &bcs);
shootConfig (results, bcs);
for (size_t i = 0; i < results.size (); i++) {
Accumulator acc;
......
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