Commit 43e02c0d authored by Florent Lamiraux's avatar Florent Lamiraux
Browse files

Update to modifications in hpp-constraints.

  Explicit relative pose take values in R^3xSO(3) in the implicit formulation.
parent 245525e8
Pipeline #13973 failed with stage
in 4 minutes and 23 seconds
......@@ -91,6 +91,8 @@ namespace hpp {
{
using constraints::Transformation;
using constraints::RelativeTransformation;
using constraints::TransformationR3xSO3;
using constraints::RelativeTransformationR3xSO3;
assert (robot);
assert (c);
......@@ -129,22 +131,33 @@ namespace hpp {
continue;
}
// Detect relative pose constraints
if (nc->functionPtr()->outputSize() != 6) {
if ((nc->functionPtr()->outputSize() != 6) &&
(nc->functionPtr()->outputSize()) != 7) {
hppDout (info, "Constraint " << nc->functionPtr()->name ()
<< " is not of dimension 6.");
<< " is neither of dimension 6 nor 7.");
continue;
}
if (!check <Transformation>::is (nc->functionPtr (), i1, i2)) {
hppDout (info, "Constraint function " << nc->functionPtr()->name ()
<< " is not of type Transformation");
if (!check <RelativeTransformation>::is
(nc->functionPtr (), i1, i2)) {
hppDout (info, "Constraint function "
<< nc->functionPtr()->name ()
<< " is not of type RelativeTransformation");
continue;
}
if (!check <TransformationR3xSO3>::is (nc->functionPtr (), i1, i2)) {
hppDout (info, "Constraint function " << nc->functionPtr()->name ()
<< " is not of type TransformationR3xSO3");
if (!check <RelativeTransformation>::is
(nc->functionPtr (), i1, i2)) {
hppDout (info, "Constraint function "
<< nc->functionPtr()->name ()
<< " is not of type RelativeTransformation");
if (!check <RelativeTransformationR3xSO3>::is
(nc->functionPtr (), i1, i2)) {
hppDout (info, "Constraint function "
<< nc->functionPtr()->name ()
<< " is not of type RelativeTransformationR3xSO3");
continue;
}
}
}
}
bool cstRHS (nc->parameterSize () == 0);
......
......@@ -283,9 +283,9 @@ BOOST_AUTO_TEST_CASE (compare_to_relative_transform)
object2, M1inO1, M2inO2, 6 * Equality));
DifferentiableFunctionPtr_t ert (enm->explicitFunction ());
DifferentiableFunctionPtr_t irt = enm->functionPtr();
RelativeTransformation::Ptr_t rt = RelativeTransformation::create (
"relative_transformation", robot,
object1, object2, M1inO1, M2inO2);
RelativeTransformationR3xSO3::Ptr_t rt (RelativeTransformationR3xSO3::create(
"relative_transformation_R3xSO3", robot,
object1, object2, M1inO1, M2inO2));
Configuration_t q = robot->currentConfiguration (),
qrand = *cs->shoot(),
......@@ -329,8 +329,8 @@ BOOST_AUTO_TEST_CASE (compare_to_relative_transform)
rt ->value (v1, q0);
rt ->jacobian (J1, q0);
BOOST_CHECK (v0.vector().isZero(test_precision));
BOOST_CHECK (v1.vector().isZero(test_precision));
BOOST_CHECK (log(v0).isZero(test_precision));
BOOST_CHECK (log(v1).isZero(test_precision));
EIGEN_IS_APPROX (J0, J1, test_precision);
// Second at random configurations
......
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