Unverified Commit db6dd757 authored by Florent Lamiraux's avatar Florent Lamiraux Committed by GitHub
Browse files

Merge pull request #152 from Toefinder/devel

Add method to retrieve joints involved in a constraint function
parents 4f59546d 64005f0a
......@@ -47,7 +47,7 @@ namespace hpp {
/** The function returns a relative transformation between the two "closest"
convex shapes it contains.
Twos set of convex shapes can be given to this class:
Two sets of convex shapes can be given to this class:
\li a set of object contact surfaces, \f$ (o_i)_{i \in I } \f$, which can be in contact with the environment,
\li a set of floor contact surfaces, \f$ (f_j)_{j \in J } \f$, which can support objects.
......@@ -156,6 +156,53 @@ namespace hpp {
/// Display object in a stream
std::ostream& print (std::ostream& o) const;
/// Return pair of joints the relative pose between which
/// modifies the function value if any
///
/// Currently only supports the case when all the joints for floors are the same
/// and all the joints for objects involved are the same
/// \param robot the robot the constraints are applied on,
/// \return pair of joints whose relative pose determines the value
/// of the contact function, arranged in order of increasing joint index,
/// or a pair of empty shared pointers.
std::pair<JointConstPtr_t, JointConstPtr_t> dependsOnRelPoseBetween
(DeviceConstPtr_t robot) const
{
if ((floorConvexShapes_.size() == 0) || (objectConvexShapes_.size() == 0)) {
return std::pair<JointConstPtr_t, JointConstPtr_t>(nullptr, nullptr);
}
JointConstPtr_t floor0_joint = floorConvexShapes_[0].joint_;
JointConstPtr_t object0_joint = objectConvexShapes_[0].joint_;
size_type index1 = Joint::index(floor0_joint);
size_type index2 = Joint::index(object0_joint);
// check that all the joints involved are the same
for (ConvexShapes_t::const_iterator it (floorConvexShapes_.begin());
it != floorConvexShapes_.end(); ++it)
{
size_type jointIndex = Joint::index(it->joint_);
if (jointIndex != index1) {
return std::pair<JointConstPtr_t, JointConstPtr_t>(nullptr, nullptr);
}
}
for (ConvexShapes_t::const_iterator it (objectConvexShapes_.begin());
it != objectConvexShapes_.end(); ++it)
{
size_type jointIndex = Joint::index(it->joint_);
if (jointIndex != index2) {
return std::pair<JointConstPtr_t, JointConstPtr_t>(nullptr, nullptr);
}
}
if (index1 <= index2) {
return std::pair<JointConstPtr_t, JointConstPtr_t>(floor0_joint, object0_joint);
} else {
return std::pair<JointConstPtr_t, JointConstPtr_t>(object0_joint, floor0_joint);
}
};
protected:
/// Constructor
/// \param name name of the constraint,
......@@ -257,6 +304,22 @@ namespace hpp {
void computeRelativePoseRightHandSide
(LiegroupElementConstRef rhs, std::size_t& ifloor, std::size_t& iobject,
LiegroupElementRef relativePoseRhs) const;
/// Return pair of joints the relative pose between which
/// modifies the function value if any
///
/// Currently only supports the case when all the joints for floors are the same
/// and all the joints for objects involved are the same
/// \param robot the robot the constraints are applied on,
/// \return pair of joints whose relative pose determines the value
/// of the contact function, arranged in order of increasing joint index,
/// or a pair of empty shared pointers.
std::pair<JointConstPtr_t, JointConstPtr_t> dependsOnRelPoseBetween
(DeviceConstPtr_t robot) const
{
return sibling_->dependsOnRelPoseBetween(robot);
};
protected:
/// Constructor
/// \param name name of the sibling ConvexShapeContact constraint,
......@@ -311,6 +374,21 @@ namespace hpp {
return complement_;
}
/// Return pair of joints the relative pose between which
/// modifies the function value if any
///
/// Currently only supports the case when all the joints for floors are the same
/// and all the joints for objects involved are the same
/// \param robot the robot the constraints are applied on,
/// \return pair of joints whose relative pose determines the value
/// of the contact function, arranged in order of increasing joint index,
/// or a pair of empty shared pointers.
std::pair<JointConstPtr_t, JointConstPtr_t> dependsOnRelPoseBetween
(DeviceConstPtr_t robot) const
{
return constraint_->dependsOnRelPoseBetween(robot);
}
protected:
/// Constructor
/// \param constraintName name of the ConvexShapeContact instance,
......
......@@ -188,7 +188,23 @@ namespace hpp {
bool operator== (DifferentiableFunction const & other) const;
bool operator!= (DifferentiableFunction const & b) const;
//virtual bool isEqual(DifferentiableFunctionPtr_t const &, bool) const {return true;}
/// Return pair of joints the relative pose between which
/// modifies the function value if any
///
/// This method is useful to know whether an instance of Implicit constrains
/// the relative pose between two joints.
/// \param robot the robot the constraints are applied on,
/// \return the pair of joints involved, arranged in order of increasing
/// joint index, or a pair of empty shared pointers.
/// \note if absolute pose (relative pose with respect to "universe"),
/// "universe" is returned as empty shared pointer
virtual std::pair<JointConstPtr_t, JointConstPtr_t> dependsOnRelPoseBetween
(DeviceConstPtr_t robot) const
{
return std::pair<JointConstPtr_t, JointConstPtr_t>(nullptr, nullptr);
};
protected:
/// \brief Concrete class constructor should call this constructor.
///
......
......@@ -42,7 +42,7 @@ namespace hpp {
/// q,
/// \li q_in is the vector composed of other configuration variables of
/// q,
/// f, g are differentiable functions with values in a Lie group.
/// \li f, g are differentiable functions with values in a Lie group.
///
/// This class is mainly used to create hpp::constraints::Explicit
/// instances.
......@@ -112,6 +112,22 @@ namespace hpp {
return true;
}
/// Return pair of joints the relative pose between which
/// modifies the function value if any
///
/// Currently checks if the implicit function specifies a joint
/// where
/// \li q_out is a vector corresponding to only 1 joint
/// \li q_in is an empty vector (since f is constant and specifies
/// the whole or part of the pose of the joint)
/// \param robot the robot the constraints are applied on,
/// \return pair of pointers to the lock joint and its parent joint,
/// arranged in order of increasing joint index, or a pair of empty
/// shared pointers if the implicit function does not specify a locked
/// joint.
std::pair<JointConstPtr_t, JointConstPtr_t> dependsOnRelPoseBetween
(DeviceConstPtr_t robot) const;
private:
void computeJacobianBlocks ();
......
......@@ -45,9 +45,9 @@ namespace hpp {
/// Relative transformation as a mapping between configuration variables
///
/// When the positions of two joints are constrained by a full
/// transformation constraint, if the second joint is hold by a freeflyer
/// transformation constraint, if the second joint is held by a freeflyer
/// joint, the position of this latter joint can be
/// explicitely expressed with respect to the position of the first joint.
/// explicitly expressed with respect to the position of the first joint.
///
/// This class provides this expression. The input configuration variables
/// are the joint values of all joints except the above mentioned freeflyer
......@@ -84,6 +84,29 @@ namespace hpp {
return joint2_;
}
/// Return pair of joints the relative pose between which
/// modifies the function value if any
///
/// This method is useful to know whether an instance of Implicit constrains
/// the relative pose between two joints.
/// \param robot the robot the constraints are applied on,
/// \return the pair of joints involved, arranged in order of increasing
/// joint index, or a pair of empty shared pointers.
/// \note if absolute pose (relative pose with respect to "universe"),
/// "universe" is returned as empty shared pointer
std::pair<JointConstPtr_t, JointConstPtr_t> dependsOnRelPoseBetween
(DeviceConstPtr_t robot) const {
JointConstPtr_t j1 = joint1();
JointConstPtr_t j2 = joint2();
size_type index1 = (j1? j1->index(): 0);
size_type index2 = (j2? j2->index(): 0);
if (index1 <= index2) {
return std::pair<JointConstPtr_t, JointConstPtr_t>(j1, j2);
} else {
return std::pair<JointConstPtr_t, JointConstPtr_t>(j2, j1);
}
};
protected:
typedef Eigen::BlockIndex BlockIndex;
typedef Eigen::RowBlockIndices RowBlockIndices;
......
......@@ -48,6 +48,7 @@ namespace hpp {
typedef pinocchio::value_type value_type;
typedef pinocchio::JointPtr_t JointPtr_t;
typedef pinocchio::JointConstPtr_t JointConstPtr_t;
typedef pinocchio::Joint Joint;
typedef pinocchio::vector3_t vector3_t;
typedef pinocchio::matrix3_t matrix3_t;
typedef Eigen::Matrix<value_type, 6, 6> matrix6_t;
......@@ -106,6 +107,7 @@ namespace hpp {
typedef pinocchio::ConfigurationOut_t ConfigurationOut_t;
typedef pinocchio::Device Device;
typedef pinocchio::DevicePtr_t DevicePtr_t;
typedef pinocchio::DeviceConstPtr_t DeviceConstPtr_t;
typedef pinocchio::CenterOfMassComputation CenterOfMassComputation;
typedef pinocchio::CenterOfMassComputationPtr_t CenterOfMassComputationPtr_t;
typedef shared_ptr <DifferentiableFunction>
......
......@@ -300,6 +300,30 @@ namespace hpp {
return m_.F2inJ2;
}
/// Return pair of joints the relative pose between which
/// modifies the function value if any
///
/// This method is useful to know whether an instance of Implicit constrains
/// the relative pose between two joints.
/// \param robot the robot the constraints are applied on,
/// \return the pair of joints involved, arranged in order of increasing
/// joint index, or a pair of empty shared pointers. or a pair of empty shared pointers.
/// \note if absolute pose (relative pose with respect to "universe"),
/// "universe" is returned as empty shared pointer
std::pair<JointConstPtr_t, JointConstPtr_t> dependsOnRelPoseBetween
(DeviceConstPtr_t robot) const
{
JointConstPtr_t j1 = joint1();
JointConstPtr_t j2 = joint2();
size_type index1 = Joint::index(j1);
size_type index2 = Joint::index(j2);
if (index1 <= index2) {
return std::pair<JointConstPtr_t, JointConstPtr_t>(j1, j2);
} else {
return std::pair<JointConstPtr_t, JointConstPtr_t>(j2, j1);
}
};
virtual std::ostream& print (std::ostream& o) const;
///Constructor
......
......@@ -242,11 +242,19 @@ namespace hpp {
/// Set the comparison type
void comparisonType (const ComparisonTypes_t& comp);
/// Return the active rows taken into account
/// to determine whether the constraint is satisfied
const segments_t& activeRows() const
{
return activeRows_;
}
/// Check if all rows are active (no inactive rows)
bool checkAllRowsActive() const
{
return inactiveRows_.nbRows() == 0;
}
/// Get indices of constraint coordinates that are equality
const Eigen::RowBlockIndices& equalityIndices () const
{
......@@ -270,6 +278,21 @@ namespace hpp {
return function_;
}
/// Get pair of joints whose relative pose is fully constrained
///
/// If the function value depends on the relative pose between j1 and j2
/// and if the relative pose between j1 and j2 is fully constrained
/// because of Implicit constraint (relative transformation may still
/// differ from one path to another), return these two joints.
/// \param robot the device that this constraint is applied to
/// \return the pair of joints constrained, in order of increasing
/// joint index, or a pair of empty shared pointers
/// \note absolute pose is considered relative pose with respect to
/// "universe". "universe" is returned as a nullpointer
/// as the first element of the pair, if applicable.
virtual std::pair<JointConstPtr_t, JointConstPtr_t> doesConstrainRelPoseBetween
(DeviceConstPtr_t robot) const;
protected:
/// Constructor
/// \param function the differentiable function
......
......@@ -139,6 +139,18 @@ namespace hpp {
}
/// Print object in a stream
std::ostream& print (std::ostream& os) const;
/// Get pair of joints whose relative pose is fully constrained
///
/// \param robot the device that this constraint is applied to
/// \return pair of pointers to the parent joint and the locked joint,
/// arranged in order of increasing joint index
/// \note absolute pose is considered relative pose with respect to
/// "universe". "universe" is returned as a nullpointer
/// as the first element of the pair, if applicable.
virtual std::pair<JointConstPtr_t, JointConstPtr_t> doesConstrainRelPoseBetween
(DeviceConstPtr_t robot) const;
protected:
/// Constructor
/// \param joint joint that is locked,
......
......@@ -35,6 +35,7 @@
#include <hpp/pinocchio/device.hh>
#include <hpp/pinocchio/serialization.hh>
#include <hpp/pinocchio/liegroup-space.hh>
#include <hpp/pinocchio/joint.hh>
#include <hpp/constraints/differentiable-function.hh>
#include <hpp/constraints/matrix-view.hh>
#include <hpp/constraints/tools.hh>
......@@ -311,6 +312,36 @@ namespace hpp {
}
}
std::pair<JointConstPtr_t, JointConstPtr_t> ImplicitFunction::dependsOnRelPoseBetween
(DeviceConstPtr_t robot) const
{
// check that this is a constant function
if (inputToOutput_->inputSize() != 0) {
return std::pair<JointConstPtr_t, JointConstPtr_t>(nullptr, nullptr);
}
// get the joints involved in the output config
JointConstPtr_t j1;
// check that output interval matches with the config range of one joint
if (outputConfIntervals_.nbRows() != 1) {
return std::pair<JointConstPtr_t, JointConstPtr_t>(nullptr, nullptr);
}
segment_t row = outputConfIntervals_.rows()[0];
j1 = robot->getJointAtConfigRank(row.first);
if (!j1 || row.second != j1->configSize()) {
return std::pair<JointConstPtr_t, JointConstPtr_t>(nullptr, nullptr);
}
JointConstPtr_t j2 = j1->parentJoint();
size_type index1 = j1->index();
size_type index2 = (j2? j2->index(): 0);
if (index1 <= index2) {
return std::pair<JointConstPtr_t, JointConstPtr_t>(j1, j2);
} else {
return std::pair<JointConstPtr_t, JointConstPtr_t>(j2, j1);
}
}
HPP_SERIALIZATION_IMPLEMENT(ImplicitFunction);
} // namespace explicit_
} // namespace constraints
......
......@@ -285,6 +285,21 @@ namespace hpp {
}
}
std::pair<JointConstPtr_t, JointConstPtr_t> Implicit::doesConstrainRelPoseBetween
(DeviceConstPtr_t robot) const
{
std::pair<JointConstPtr_t, JointConstPtr_t> joints =
function_->dependsOnRelPoseBetween(robot);
// check that the constraint fully constrains the relative pose
if (function_->outputSpace()->nv() != 6 ||
!checkAllRowsActive()) {
hppDout (info, "Constraint " << function_->name ()
<< " does not fully constrain the relative pose of joints");
return std::pair<JointConstPtr_t, JointConstPtr_t>(nullptr, nullptr);
}
return joints;
}
template<class Archive>
void Implicit::serialize(Archive & ar, const unsigned int version)
{
......
......@@ -37,6 +37,7 @@
#include <hpp/pinocchio/configuration.hh>
#include <hpp/pinocchio/device.hh>
#include <hpp/pinocchio/joint.hh>
#include <pinocchio/multibody/model.hpp>
#include <hpp/constraints/affine-function.hh>
#include <hpp/constraints/explicit/implicit-function.hh>
......@@ -146,6 +147,7 @@ namespace hpp {
ComparisonTypes_t(joint->numberDof(), Equality),
std::vector<bool>(joint->numberDof(), true)),
jointName_ (joint->name ()),
joint_ (joint),
configSpace_ (joint->configurationSpace ())
{
assert (HPP_DYNAMIC_PTR_CAST (explicit_::ImplicitFunction,
......@@ -248,6 +250,25 @@ namespace hpp {
}
}
std::pair<JointConstPtr_t, JointConstPtr_t> LockedJoint::doesConstrainRelPoseBetween
(DeviceConstPtr_t robot) const
{
if (!robot->model().existJointName(jointName())) {
// Extra dofs and partial locked joints have a name that won't be
// recognized by Device::getJointByName. So they can be filtered
// this way.
return std::pair<JointConstPtr_t, JointConstPtr_t>(nullptr, nullptr);
}
JointConstPtr_t j1 = joint_->parentJoint();
size_type index1 = Joint::index(j1); // parent joint may be universe
size_type index2 = joint_->index();
if (index1 <= index2) {
return std::pair<JointConstPtr_t, JointConstPtr_t>(j1, joint_);
} else {
return std::pair<JointConstPtr_t, JointConstPtr_t>(joint_, j1);
}
}
template<class Archive>
void LockedJoint::serialize(Archive & ar, const unsigned int version)
{
......
......@@ -152,6 +152,19 @@ BOOST_AUTO_TEST_CASE(convexShapeContact)
contactConstraint->functionPtr()));
value_type M(f->contactConstraint()->radius());
BOOST_CHECK(M > sqrt(2));
// check that the two joints involved can be retrieved correctly
std::pair<JointConstPtr_t, JointConstPtr_t> joints =
f->dependsOnRelPoseBetween(robot);
BOOST_CHECK_EQUAL (Joint::index(joints.first), Joint::index(j1));
BOOST_CHECK_EQUAL (Joint::index(joints.second), Joint::index(j2));
joints = f->contactConstraint()->dependsOnRelPoseBetween(robot);
BOOST_CHECK_EQUAL (Joint::index(joints.first), Joint::index(j1));
BOOST_CHECK_EQUAL (Joint::index(joints.second), Joint::index(j2));
joints = f->complement()->dependsOnRelPoseBetween(robot);
BOOST_CHECK_EQUAL (Joint::index(joints.first), Joint::index(j1));
BOOST_CHECK_EQUAL (Joint::index(joints.second), Joint::index(j2));
// box 1 above box 2: surfaces in contact are
// - floor surface 1 for box 1,
// - object surface 0 for box 2.
......
......@@ -57,6 +57,8 @@ using hpp::pinocchio::DevicePtr_t;
using hpp::pinocchio::JointPtr_t;
using hpp::pinocchio::LiegroupSpace;
using hpp::pinocchio::Transform3f;
using hpp::constraints::EqualToZero;
using hpp::pinocchio::JointIndex;
using hpp::pinocchio::urdf::loadModelFromString;
......@@ -307,9 +309,9 @@ BOOST_AUTO_TEST_CASE(RelativeTransformation_R3xSO3)
std::vector<bool> mask = {false, false, false, false, false, true};
ImplicitPtr_t constraint
(Implicit::create (RelativeTransformationR3xSO3::create
("RelativeTransformationR3xSO3", robot, j1, j2,
tf1, tf2),
6 * Equality, mask));
("RelativeTransformationR3xSO3", robot, j1, j2,
tf1, tf2),
6 * Equality, mask));
BasicConfigurationShooter cs (robot);
solver::BySubstitution solver(robot->configSpace());
solver.errorThreshold(1e-10);
......@@ -328,15 +330,15 @@ BOOST_AUTO_TEST_CASE(RelativeTransformation_R3xSO3)
// Create grasp constraint with one degree of freedom in rotation along z
mask = {true, true, true, true, true, false};
ImplicitPtr_t c1 (Implicit::create(RelativeTransformationR3xSO3::create
("RelativeTransformationR3xSO3", robot,
j1, j2, tf1, tf2), 6*EqualToZero, mask));
("RelativeTransformationR3xSO3", robot,
j1, j2, tf1, tf2), 6*EqualToZero, mask));
solver::BySubstitution s1(robot->configSpace());
s1.errorThreshold(1e-10);
s1.add(c1);
// Create grasp + complement as an explicit constraint
ExplicitPtr_t c2(explicit_::RelativePose::create
("ExplicitRelativePose", robot, j1, j2, tf1, tf2,
5 * EqualToZero << Equality));
("ExplicitRelativePose", robot, j1, j2, tf1, tf2,
5 * EqualToZero << Equality));
solver::BySubstitution s2(robot->configSpace());
s2.errorThreshold(1e-4);
s2.add(c2);
......@@ -353,17 +355,17 @@ BOOST_AUTO_TEST_CASE(RelativeTransformation_R3xSO3)
// satisfy c1.
// Configuration q_new below satisfies c2 but not c1.
*q_near << 0.18006349590534418, 0.3627623741970175, 0.9567759630330663,
0.044416054309488175, 0.31532356328825556, 0.4604329042168087,
0.8286131819306576,
0.45813483973344404, 0.23514459283216355, 0.7573015903787429,
0.8141495491430896, 0.1383820163733335, 0.3806970356973106,
0.4160296818567576;
0.044416054309488175, 0.31532356328825556, 0.4604329042168087,
0.8286131819306576,
0.45813483973344404, 0.23514459283216355, 0.7573015903787429,
0.8141495491430896, 0.1383820163733335, 0.3806970356973106,
0.4160296818567576;
*q_new << 0.16026892741853033, 0.33925098736439646, 0.8976880203169203,
-0.040130835169737825, 0.37473431876437147, 0.4405275981290593,
0.8148000624051422,
0.43787674119234027, 0.18316291571416676, 0.7189377922181226,
0.7699579340925136, 0.1989432638510445, 0.35960786236482944,
0.4881275886709128;
-0.040130835169737825, 0.37473431876437147, 0.4405275981290593,
0.8148000624051422,
0.43787674119234027, 0.18316291571416676, 0.7189377922181226,
0.7699579340925136, 0.1989432638510445, 0.35960786236482944,
0.4881275886709128;
}
s2.rightHandSideFromConfig(*q_near);
vector6_t error;
......@@ -407,4 +409,168 @@ BOOST_AUTO_TEST_CASE (equality) {
BOOST_CHECK (*functions[0] != *functions[2]); // a lot of things are different
BOOST_CHECK (*functions[2] != *functions[4]); // only the names are different
BOOST_CHECK (*functions[0] != *functions[1]); // only the names are equal
}
\ No newline at end of file
}
BOOST_AUTO_TEST_CASE (dependsOnRelPoseBetween) {
DevicePtr_t device = hpp::pinocchio::unittest::makeDevice(
hpp::pinocchio::unittest::HumanoidSimple);
device->numberDeviceData (4);
JointPtr_t ee1 = device->getJointByName ("lleg5_joint"),
ee2 = device->getJointByName ("rleg5_joint");
BOOST_REQUIRE (device);
// ensure that the joint indices are as expected
BOOST_REQUIRE (ee1->index() < ee2->index());
BasicConfigurationShooter cs (device);
device->currentConfiguration (*cs.shoot ());
device->computeForwardKinematics ();
Transform3f tf1 (ee1->currentTransformation ());
Transform3f tf2 (ee2->currentTransformation ());
DifferentiableFunctionPtr_t function;
std::pair<JointConstPtr_t, JointConstPtr_t> joints;
std::pair<JointConstPtr_t, JointConstPtr_t> jointsConstrained;
ImplicitPtr_t constraint;
function = Orientation::create ("Orientation" , device, ee2, tf2);
joints = function->dependsOnRelPoseBetween (device);
BOOST_CHECK (!joints.first);
BOOST_CHECK_EQUAL (joints.second->index(), ee2->index());
// constraint does not fully constrain the relative pose
// since it only involves the orientation
constraint = Implicit::create(function,
ComparisonTypes_t(function->outputSpace()->nv(), EqualToZero),
std::vector<bool>(function->outputSpace()->nv(), true));
jointsConstrained = constraint->doesConstrainRelPoseBetween(device);
BOOST_CHECK (!jointsConstrained.first);
BOOST_CHECK (!jointsConstrained.second);
function = Position::create ("Position" , device, ee2, tf2, tf1);
joints = function->dependsOnRelPoseBetween (device);
BOOST_CHECK (!joints.first);
BOOST_CHECK_EQUAL (joints.second->index(), ee2->index());
// constraint does not fully constrain the relative pose
// since it only involves the position
constraint = Implicit::create(function,
ComparisonTypes_t(function->outputSpace()->nv(), EqualToZero),
std::vector<bool>(function->outputSpace()->nv(), true));
jointsConstrained = constraint->doesConstrainRelPoseBetween(device);
BOOST_CHECK (!jointsConstrained.first);
BOOST_CHECK (!jointsConstrained.second);
function = Transformation::create ("Transformation" , device, ee1, tf1);
joints = function->dependsOnRelPoseBetween (device);
BOOST_CHECK (!joints.first);
BOOST_CHECK_EQUAL (joints.second->index(), ee1->index());
// constraint does not fully constrain the relative pose
// since the mask is not full
constraint = Implicit::create(function,
ComparisonTypes_t(function->outputSpace()->nv(), EqualToZero),
std::vector<bool>(function->outputSpace()->nv(), false));
jointsConstrained = constraint->doesConstrainRelPoseBetween(device);
BOOST_CHECK (!jointsConstrained.first);
BOOST_CHECK (!jointsConstrained.second);
// constraint fully constrains the relative pose
constraint = Implicit::create(function,
ComparisonTypes_t(function->outputSpace()->nv(), EqualToZero),
std::vector<bool>(function->outputSpace()->nv(), true));
jointsConstrained = constraint->doesConstrainRelPoseBetween(device);
BOOST_CHECK (!jointsConstrained.first);
BOOST_CHECK_EQUAL (jointsConstrained.second->index(), ee1->index());
function = RelativeOrientation::create ("RelativeOrientation" , device, ee1, ee2, tf1);
joints = function->dependsOnRelPoseBetween (device);
BOOST_CHECK_EQUAL (joints.first->index(), ee1->index());
BOOST_CHECK_EQUAL (joints.second->index(), ee2->index());