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

Merge pull request #147 from florent-lamiraux/devel

Fix ConvexShapeContactComplement::isEqual method
parents f0c6c35b 63b6b8c5
Pipeline #17648 passed with stage
in 25 minutes and 3 seconds
......@@ -154,21 +154,7 @@ namespace hpp {
const JointAndShapes_t& floorSurfaces,
const JointAndShapes_t& objectSurfaces);
bool isEqual(const DifferentiableFunction& other) const {
const ConvexShapeContact& castother = dynamic_cast<const ConvexShapeContact&>(other);
if (!DifferentiableFunction::isEqual(other))
return false;
if (robot_ != castother.robot_)
return false;
if (objectConvexShapes_.size() != castother.objectConvexShapes_.size())
return false;
for (std::size_t i = 0; i < objectConvexShapes_.size(); i++)
if (floorConvexShapes_[i] != castother.floorConvexShapes_[i])
return false;
return true;
}
bool isEqual(const DifferentiableFunction& other) const;
private:
/// Add a ConvexShape as an object.
......@@ -270,12 +256,8 @@ namespace hpp {
const JointAndShapes_t& floorSurfaces,
const JointAndShapes_t& objectSurfaces);
bool isEqual(const DifferentiableFunction& other) const {
const ConvexShapeContactComplement& castother = dynamic_cast<const ConvexShapeContactComplement&>(other);
if (!DifferentiableFunction::isEqual(other))
return false;
return (*sibling_ != *(castother.sibling_));
}
bool isEqual(const DifferentiableFunction& other) const;
private:
void impl_compute (LiegroupElementRef result, ConfigurationIn_t argument) const;
......@@ -335,18 +317,8 @@ namespace hpp {
virtual void impl_jacobian(matrixOut_t jacobian, vectorIn_t arg)
const;
bool isEqual(const DifferentiableFunction& other) const {
const ConvexShapeContactHold& castother = dynamic_cast<const ConvexShapeContactHold&>(other);
if (!DifferentiableFunction::isEqual(other))
return false;
if (constraint_ != castother.constraint_)
return false;
if (complement_ != castother.complement_)
return false;
return true;
}
bool isEqual(const DifferentiableFunction& other) const;
private:
ConvexShapeContactPtr_t constraint_;
ConvexShapeContactComplementPtr_t complement_;
......
......@@ -175,18 +175,8 @@ namespace hpp {
DevicePtr_t robot = DevicePtr_t (),
value_type eps = std::sqrt(Eigen::NumTraits<value_type>::epsilon())) const;
inline bool operator== (DifferentiableFunction const & other) const
{
try {
return isEqual(other) && other.isEqual(*this);
} catch (const std::bad_cast& exc) {
return false;
}
}
inline bool operator!= (DifferentiableFunction const & b) const
{
return !(*this == b);
}
bool operator== (DifferentiableFunction const & other) const;
bool operator!= (DifferentiableFunction const & b) const;
//virtual bool isEqual(DifferentiableFunctionPtr_t const &, bool) const {return true;}
protected:
/// \brief Concrete class constructor should call this constructor.
......
......@@ -507,13 +507,6 @@ namespace Eigen {
m_nbRows(BlockIndex::cardinal(rows)),
m_nbCols(BlockIndex::cardinal(cols)), m_rows(rows), m_cols(cols)
{
# ifndef NDEBUG
// test that input is sorted
segments_t r (rows); BlockIndex::sort (r);
assert (r == rows);
segments_t c (cols); BlockIndex::sort (c);
assert (c == cols);
#endif
}
/// Constructor by vectors of segments
......@@ -526,13 +519,6 @@ namespace Eigen {
const size_type& nbCols, const ColIndices_t& cols) :
m_nbRows(nbRows), m_nbCols(nbCols), m_rows(rows), m_cols(cols)
{
# ifndef NDEBUG
// test that input is sorted
segments_t r (rows); BlockIndex::sort (r);
assert (r == rows);
segments_t c (cols); BlockIndex::sort (c);
assert (c == cols);
#endif
}
/// Constructor of single block
......
......@@ -58,6 +58,22 @@ namespace hpp {
computeRadius();
}
bool ConvexShapeContact::isEqual(const DifferentiableFunction& other) const
{
const ConvexShapeContact& castother = dynamic_cast
<const ConvexShapeContact&>(other);
if (!DifferentiableFunction::isEqual(other))
return false;
if (robot_ != castother.robot_)
return false;
if (objectConvexShapes_.size() != castother.objectConvexShapes_.size())
return false;
for (std::size_t i = 0; i < objectConvexShapes_.size(); i++)
if (floorConvexShapes_[i] != castother.floorConvexShapes_[i])
return false;
return true;
}
ConvexShapeContactPtr_t ConvexShapeContact::create (
const std::string& name, DevicePtr_t robot,
const JointAndShapes_t& floorSurfaces,
......@@ -339,6 +355,17 @@ namespace hpp {
{
}
bool ConvexShapeContactComplement::isEqual
(const DifferentiableFunction& other) const
{
const ConvexShapeContactComplement& castother = dynamic_cast
<const ConvexShapeContactComplement&>(other);
if (!DifferentiableFunction::isEqual(other))
return false;
if (sibling_ == castother.sibling_) return true;
return (*sibling_ == *(castother.sibling_));
}
std::pair < ConvexShapeContactPtr_t,
ConvexShapeContactComplementPtr_t >
ConvexShapeContactComplement::createPair
......@@ -495,5 +522,19 @@ namespace hpp {
complement_->impl_jacobian(jacobian.bottomRows<3>(), arg);
}
bool ConvexShapeContactHold::isEqual(const DifferentiableFunction& other)
const
{
const ConvexShapeContactHold& castother = dynamic_cast
<const ConvexShapeContactHold&>(other);
if (!DifferentiableFunction::isEqual(other))
return false;
if (constraint_ != castother.constraint_)
return false;
if (complement_ != castother.complement_)
return false;
return true;
}
} // namespace constraints
} // namespace hpp
......@@ -204,6 +204,21 @@ namespace hpp {
finiteDiffCentral(jacobian, x, FiniteDiffVectorSpaceOp(eps), *this);
}
bool DifferentiableFunction::operator==
(DifferentiableFunction const & other) const
{
try {
return isEqual(other) && other.isEqual(*this);
} catch (const std::bad_cast& exc) {
return false;
}
}
bool DifferentiableFunction::operator!=
(DifferentiableFunction const & b) const
{
return !(*this == b);
}
DifferentiableFunction::DifferentiableFunction
(size_type sizeInput, size_type sizeInputDerivative,
size_type sizeOutput, std::string name) :
......
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