Commit ca39cf3e authored by Le Quang Anh's avatar Le Quang Anh
Browse files

[API] Get joints in fn and joints constrained

Get pair of joints involved in differentiable fn

Get locked joint for explicit constraint

Add method to retrieve a joint whose whole or partial configuration will
decide the value of the implicit function in an explicit constraint.
This is currently done by inspecting the set of configuration input and output
intervals: if input has size 0 and output falls into interval of a joint,
then the joint is "locked" its config alone will affect the fn value.

Fix bug comparing joints from their pointers

Fix: locked joint should be locked wrt parent joint

Previous implementation misunderstands the locked joint, and assumes
that locked joint is locked wrt the world joint. The correct
understanding is that locked joints are locked wrt the parent joint,
which may or may not be the world joint.

Edit documentation based on comments

Clearer documentation on how the method works and what the arguments are

Add API check if all rows in constraint are active

Add API to find joint pair constrained

Return a pair of joints whose relative pose is fully constrained because
of constraint.

Clearer name for API to get joints involved

Add API for convex shape contact complement and hold
For now, it only works if the all convex shapes in floorConvexShapes_
are on the same joint an all the convex shapes in objectConvexShapes_
are on the same joint.

Refactor APIs and fix minor bugs
parent 5a4f363f
......@@ -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)
{
......
Supports Markdown
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