Commit 48b9902c authored by Florent Lamiraux's avatar Florent Lamiraux
Browse files

Merge remote-tracking branch 'ahn/devel' into devel

parents 241853f6 d049c55d
Pipeline #17677 passed with stage
in 9 minutes and 37 seconds
......@@ -156,6 +156,39 @@ namespace hpp {
/// Display object in a stream
std::ostream& print (std::ostream& o) const;
/// Return pair of joints on which the value of the function depends
///
/// If the value of the function only depends on the relative pose between
/// two joints, the function returns the pair of joints.
/// If a joint is empty, it stands for the environment.
///
/// Currently only supports the case when all the joints for floors are
/// the same and all the joints for objects involved are the same.
std::pair<JointConstPtr_t, JointConstPtr_t> jointsInvolved
(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_;
// check that all the joints involved are the same
for (ConvexShapes_t::const_iterator it (floorConvexShapes_.begin());
it != floorConvexShapes_.end(); ++it)
{
assert (it->joint_ == floor0_joint);
}
for (ConvexShapes_t::const_iterator it (objectConvexShapes_.begin());
it != objectConvexShapes_.end(); ++it)
{
assert (it->joint_ == object0_joint);
}
return std::pair<JointConstPtr_t, JointConstPtr_t>(floor0_joint, object0_joint);
};
protected:
/// Constructor
/// \param name name of the constraint,
......
......@@ -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,21 @@ namespace hpp {
return true;
}
/// Return pair of joints whose relative pose modifies the 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 robotPtr the robot the constraints are applied on,
/// \return (null_ptr, jointPtr) where jointPtr is a shared pointer to
/// the joint, or an empty pointer if the implicit
/// function does not specify a locked joint.
/// \note this method is intended to detect locked joints.
std::pair<JointConstPtr_t, JointConstPtr_t> jointsInvolved
(DeviceConstPtr_t robotPtr) 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,16 @@ namespace hpp {
return joint2_;
}
/// Return pair of joints on which the value of the function depends
///
/// If the value of the function only depends on the relative pose between
/// two joints, the function returns the pair of joints.
/// If a joint is empty, it stands for the environment.
std::pair<JointConstPtr_t, JointConstPtr_t> jointsInvolved
(DeviceConstPtr_t robot) const {
return std::pair<JointConstPtr_t, JointConstPtr_t>(joint1(), joint2());
};
protected:
typedef Eigen::BlockIndex BlockIndex;
typedef Eigen::RowBlockIndices RowBlockIndices;
......
......@@ -106,6 +106,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,16 @@ namespace hpp {
return m_.F2inJ2;
}
/// Return pair of joints on which the value of the function depends
///
/// If the value of the function only depends on the relative pose between
/// two joints, the function returns the pair of joints.
/// If a joint is empty, it stands for the environment.
std::pair<JointConstPtr_t, JointConstPtr_t> jointsInvolved
(DeviceConstPtr_t robot) const {
return std::pair<JointConstPtr_t, JointConstPtr_t>(joint1(), joint2());
};
virtual std::ostream& print (std::ostream& o) const;
///Constructor
......
......@@ -311,6 +311,35 @@ namespace hpp {
}
}
/// Return pair of joints on which the value of the function depends
///
/// If the value of the function only depends on the relative pose between
/// two joints, the function returns the pair of joints.
/// If a joint is empty, it stands for the environment.
std::pair<JointConstPtr_t, JointConstPtr_t> ImplicitFunction::jointsInvolved
(DeviceConstPtr_t robotPtr) const {
if (inputToOutput_->inputSize() != 0)
return std::pair<JointConstPtr_t, JointConstPtr_t>(nullptr, nullptr);
// get the joints involved in the output config
// quite inefficient for now
JointConstPtr_t jointPtr = nullptr;
for (segment_t row: outputConfIntervals_.rows()) {
for (size_type rank(row.first); rank < row.first + row.second; ++rank) {
JointConstPtr_t newJointPtr = robotPtr->getJointAtConfigRank(rank);
if (newJointPtr) {
// if there is more than 1 locked joint, return null
if ((jointPtr) && (jointPtr != newJointPtr)) {
hppDout(debug, "this constraint locks both joints "
<< jointPtr->index() << " and " << newJointPtr->index());
return std::pair<JointConstPtr_t, JointConstPtr_t>(nullptr, nullptr);
}
jointPtr = newJointPtr;
}
}
}
return std::pair<JointConstPtr_t, JointConstPtr_t>(nullptr, jointPtr);
};
HPP_SERIALIZATION_IMPLEMENT(ImplicitFunction);
} // namespace explicit_
} // namespace constraints
......
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