Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
Humanoid Path Planner
hpp-constraints
Commits
1d933ee1
Commit
1d933ee1
authored
Jul 29, 2021
by
Florent Lamiraux
Browse files
Merge remote-tracking branch 'athiault/diff-func-equality' into devel
parents
4382538f
d52760dc
Changes
29
Hide whitespace changes
Inline
Side-by-side
include/hpp/constraints/active-set-differentiable-function.hh
View file @
1d933ee1
...
...
@@ -83,6 +83,19 @@ namespace hpp {
jacobian
.
middleCols
(
_int
->
first
,
_int
->
second
).
setZero
();
}
bool
isEqual
(
const
DifferentiableFunction
&
other
)
const
{
const
ActiveSetDifferentiableFunction
&
castother
=
dynamic_cast
<
const
ActiveSetDifferentiableFunction
&>
(
other
);
if
(
!
DifferentiableFunction
::
isEqual
(
other
))
return
false
;
if
(
function_
!=
castother
.
function_
)
return
false
;
if
(
intervals_
!=
castother
.
intervals_
)
return
false
;
return
true
;
}
DifferentiableFunctionPtr_t
function_
;
segments_t
intervals_
;
};
// class ActiveSetDifferentiableFunction
...
...
include/hpp/constraints/affine-function.hh
View file @
1d933ee1
...
...
@@ -56,6 +56,14 @@ namespace hpp {
J
.
setIdentity
();
}
bool
isEqual
(
const
DifferentiableFunction
&
other
)
const
{
dynamic_cast
<
const
Identity
&>
(
other
);
if
(
!
DifferentiableFunction
::
isEqual
(
other
))
return
false
;
return
true
;
}
private:
Identity
()
{}
HPP_SERIALIZABLE
();
...
...
@@ -100,6 +108,19 @@ namespace hpp {
init
();
}
bool
isEqual
(
const
DifferentiableFunction
&
other
)
const
{
const
AffineFunction
&
castother
=
dynamic_cast
<
const
AffineFunction
&>
(
other
);
if
(
!
DifferentiableFunction
::
isEqual
(
other
))
return
false
;
if
(
J_
!=
castother
.
J_
)
return
false
;
if
(
b_
!=
castother
.
b_
)
return
false
;
return
true
;
}
private:
/// User implementation of function evaluation
void
impl_compute
(
LiegroupElementRef
y
,
vectorIn_t
x
)
const
...
...
@@ -171,6 +192,17 @@ namespace hpp {
void
impl_jacobian
(
matrixOut_t
J
,
vectorIn_t
)
const
{
J
.
setZero
();
}
bool
isEqual
(
const
DifferentiableFunction
&
other
)
const
{
const
ConstantFunction
&
castother
=
dynamic_cast
<
const
ConstantFunction
&>
(
other
);
if
(
!
DifferentiableFunction
::
isEqual
(
other
))
return
false
;
if
(
c_
.
vector
()
==
castother
.
c_
.
vector
())
return
false
;
return
true
;
}
const
LiegroupElement
c_
;
private:
...
...
include/hpp/constraints/com-between-feet.hh
View file @
1d933ee1
...
...
@@ -94,6 +94,34 @@ namespace hpp {
virtual
void
impl_jacobian
(
matrixOut_t
jacobian
,
ConfigurationIn_t
arg
)
const
;
bool
isEqual
(
const
DifferentiableFunction
&
other
)
const
{
const
ComBetweenFeet
&
castother
=
dynamic_cast
<
const
ComBetweenFeet
&>
(
other
);
if
(
!
DifferentiableFunction
::
isEqual
(
other
))
return
false
;
if
(
robot_
!=
castother
.
robot_
)
return
false
;
if
(
com_
->
centerOfMassComputation
()
!=
castother
.
com_
->
centerOfMassComputation
())
return
false
;
if
(
left_
->
joint
()
!=
castother
.
left_
->
joint
())
return
false
;
if
(
right_
->
joint
()
!=
castother
.
right_
->
joint
())
return
false
;
if
(
left_
->
local
()
!=
castother
.
left_
->
local
())
return
false
;
if
(
right_
->
local
()
!=
castother
.
right_
->
local
())
return
false
;
if
(
jointRef_
!=
castother
.
jointRef_
)
return
false
;
if
(
pointRef_
!=
castother
.
pointRef_
)
return
false
;
if
(
mask_
!=
castother
.
mask_
)
return
false
;
return
true
;
}
private:
DevicePtr_t
robot_
;
mutable
Traits
<
PointCom
>::
Ptr_t
com_
;
...
...
include/hpp/constraints/configuration-constraint.hh
View file @
1d933ee1
...
...
@@ -72,12 +72,27 @@ namespace hpp {
ConfigurationIn_t
arg
)
const
;
std
::
ostream
&
print
(
std
::
ostream
&
o
)
const
;
bool
isEqual
(
const
DifferentiableFunction
&
other
)
const
{
const
ConfigurationConstraint
&
castother
=
dynamic_cast
<
const
ConfigurationConstraint
&>
(
other
);
if
(
!
DifferentiableFunction
::
isEqual
(
other
))
return
false
;
if
(
robot_
!=
castother
.
robot_
)
return
false
;
if
(
goal_
.
vector
()
!=
castother
.
goal_
.
vector
())
return
false
;
if
(
weights_
!=
castother
.
weights_
)
return
false
;
return
true
;
}
private:
typedef
Eigen
::
Array
<
bool
,
Eigen
::
Dynamic
,
1
>
EigenBoolVector_t
;
DevicePtr_t
robot_
;
LiegroupElement
goal_
;
vector_t
weights_
;
};
// class Co
mBetweenFee
t
};
// class Co
nfigurationConstrain
t
}
// namespace constraints
}
// namespace hpp
#endif // HPP_CONSTRAINTS_CONFIGURATION_CONSTRAINT_HH
include/hpp/constraints/convex-shape-contact.hh
View file @
1d933ee1
...
...
@@ -154,6 +154,21 @@ 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
;
}
private:
/// Add a ConvexShape as an object.
...
...
@@ -255,7 +270,12 @@ 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_
);
}
private:
void
impl_compute
(
LiegroupElementRef
result
,
ConfigurationIn_t
argument
)
const
;
...
...
@@ -314,6 +334,19 @@ namespace hpp {
const
;
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
;
}
private:
ConvexShapeContactPtr_t
constraint_
;
ConvexShapeContactComplementPtr_t
complement_
;
...
...
include/hpp/constraints/convex-shape.hh
View file @
1d933ee1
...
...
@@ -156,6 +156,21 @@ namespace hpp {
/// Transform of the shape in the joint frame
inline
const
Transform3f
&
positionInJoint
()
const
{
return
MinJoint_
;
}
bool
operator
==
(
ConvexShape
const
&
other
)
const
{
if
(
Pts_
!=
other
.
Pts_
)
return
false
;
if
(
shapeDimension_
!=
other
.
shapeDimension_
)
return
false
;
if
(
C_
!=
other
.
C_
)
return
false
;
if
(
N_
!=
other
.
N_
)
return
false
;
if
(
Ns_
!=
other
.
Ns_
)
return
false
;
if
(
Us_
!=
other
.
Us_
)
return
false
;
if
(
Ls_
!=
other
.
Ls_
)
return
false
;
if
(
MinJoint_
!=
other
.
MinJoint_
)
return
false
;
if
(
joint_
!=
other
.
joint_
)
return
false
;
}
bool
operator
!=
(
ConvexShape
const
&
other
)
const
{
return
!
(
this
->
operator
==
(
other
));
}
/// The points in the joint frame. It is constant.
std
::
vector
<
vector3_t
>
Pts_
;
size_t
shapeDimension_
;
...
...
include/hpp/constraints/differentiable-function-set.hh
View file @
1d933ee1
...
...
@@ -126,6 +126,23 @@ namespace hpp {
row
+=
f
.
outputDerivativeSize
();
}
}
bool
isEqual
(
const
DifferentiableFunction
&
other
)
const
{
const
DifferentiableFunctionSet
&
castother
=
dynamic_cast
<
const
DifferentiableFunctionSet
&>
(
other
);
if
(
!
DifferentiableFunction
::
isEqual
(
other
))
return
false
;
if
(
functions_
!=
castother
.
functions_
)
return
false
;
if
(
result_
.
size
()
!=
castother
.
result_
.
size
())
return
false
;
for
(
std
::
size_t
i
=
0
;
i
<
result_
.
size
();
i
++
)
if
(
result_
[
i
]
!=
castother
.
result_
[
i
])
return
false
;
return
true
;
}
private:
Functions_t
functions_
;
mutable
std
::
vector
<
LiegroupElement
>
result_
;
...
...
include/hpp/constraints/differentiable-function.hh
View file @
1d933ee1
...
...
@@ -19,6 +19,7 @@
#ifndef HPP_CONSTRAINTS_DIFFERENTIABLE_FUNCTION_HH
# define HPP_CONSTRAINTS_DIFFERENTIABLE_FUNCTION_HH
#include
<hpp/util/debug.hh>
# include <hpp/constraints/fwd.hh>
# include <hpp/constraints/config.hh>
# include <hpp/pinocchio/liegroup-element.hh>
...
...
@@ -53,6 +54,7 @@ namespace hpp {
{
public:
virtual
~
DifferentiableFunction
()
{}
/// Evaluate the function at a given parameter.
///
/// \note parameters should be of the correct size.
...
...
@@ -173,6 +175,19 @@ 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
);
}
//virtual bool isEqual(DifferentiableFunctionPtr_t const &, bool) const {return true;}
protected:
/// \brief Concrete class constructor should call this constructor.
///
...
...
@@ -203,6 +218,20 @@ namespace hpp {
virtual
void
impl_jacobian
(
matrixOut_t
jacobian
,
vectorIn_t
arg
)
const
=
0
;
virtual
bool
isEqual
(
const
DifferentiableFunction
&
other
)
const
{
if
(
name_
!=
other
.
name_
)
return
false
;
if
(
inputSize_
!=
other
.
inputSize_
)
return
false
;
if
(
inputDerivativeSize_
!=
other
.
inputDerivativeSize_
)
return
false
;
if
(
*
outputSpace_
!=
*
(
other
.
outputSpace_
))
return
false
;
return
true
;
}
/// Dimension of input vector.
size_type
inputSize_
;
/// Dimension of input derivative
...
...
@@ -240,5 +269,24 @@ namespace hpp {
}
// namespace constraints
}
// namespace hpp
/*
// This will override boost::shared_ptr 's equality operator
// between 2 DifferentiableFunctionPtr_t
namespace boost {
using namespace hpp::constraints;
typedef DifferentiableFunction T;
typedef shared_ptr<T> TPtr;
template<> inline bool operator==<T, T> (TPtr const & a, TPtr const & b) BOOST_SP_NOEXCEPT
{
return *a == *b;
}
template<> inline bool operator!=<T, T> (TPtr const & a, TPtr const & b) BOOST_SP_NOEXCEPT
{
return !(a == b);
}
}
*/
#endif // HPP_CONSTRAINTS_DIFFERENTIABLE_FUNCTION_HH
include/hpp/constraints/distance-between-bodies.hh
View file @
1d933ee1
...
...
@@ -92,6 +92,23 @@ namespace hpp {
ConfigurationIn_t
argument
)
const
;
virtual
void
impl_jacobian
(
matrixOut_t
jacobian
,
ConfigurationIn_t
arg
)
const
;
bool
isEqual
(
const
DifferentiableFunction
&
other
)
const
{
const
DistanceBetweenBodies
&
castother
=
dynamic_cast
<
const
DistanceBetweenBodies
&>
(
other
);
if
(
!
DifferentiableFunction
::
isEqual
(
other
))
return
false
;
if
(
robot_
!=
castother
.
robot_
)
return
false
;
if
(
joint1_
!=
castother
.
joint1_
)
return
false
;
if
(
joint2_
!=
castother
.
joint2_
)
return
false
;
if
(
data_
!=
castother
.
data_
)
return
false
;
return
true
;
}
private:
typedef
::
pinocchio
::
GeometryData
GeometryData
;
...
...
include/hpp/constraints/distance-between-points-in-bodies.hh
View file @
1d933ee1
...
...
@@ -95,6 +95,25 @@ namespace hpp {
ConfigurationIn_t
argument
)
const
;
virtual
void
impl_jacobian
(
matrixOut_t
jacobian
,
ConfigurationIn_t
arg
)
const
;
bool
isEqual
(
const
DifferentiableFunction
&
other
)
const
{
const
DistanceBetweenPointsInBodies
&
castother
=
dynamic_cast
<
const
DistanceBetweenPointsInBodies
&>
(
other
);
if
(
!
DifferentiableFunction
::
isEqual
(
other
))
return
false
;
if
(
robot_
!=
castother
.
robot_
)
return
false
;
if
(
joint1_
!=
castother
.
joint1_
)
return
false
;
if
(
joint2_
!=
castother
.
joint2_
)
return
false
;
if
(
point1_
!=
castother
.
point1_
)
return
false
;
if
(
point2_
!=
castother
.
point2_
)
return
false
;
return
true
;
}
private:
DevicePtr_t
robot_
;
JointPtr_t
joint1_
;
...
...
include/hpp/constraints/explicit.hh
View file @
1d933ee1
...
...
@@ -248,6 +248,8 @@ namespace hpp {
/// Copy constructor
Explicit
(
const
Explicit
&
other
);
bool
isEqual
(
const
Implicit
&
other
,
bool
swapAndTest
)
const
;
// Store weak pointer to itself
void
init
(
const
ExplicitWkPtr_t
&
weak
);
...
...
include/hpp/constraints/explicit/implicit-function.hh
View file @
1d933ee1
...
...
@@ -79,6 +79,27 @@ namespace hpp {
/// Compute Jacobian of g (q_out) - f (q_in) with respect to q.
void
impl_jacobian
(
matrixOut_t
jacobian
,
vectorIn_t
arg
)
const
;
bool
isEqual
(
const
DifferentiableFunction
&
other
)
const
{
const
ImplicitFunction
&
castother
=
dynamic_cast
<
const
ImplicitFunction
&>
(
other
);
if
(
!
DifferentiableFunction
::
isEqual
(
other
))
return
false
;
if
(
robot_
!=
castother
.
robot_
)
return
false
;
if
(
inputToOutput_
!=
castother
.
inputToOutput_
)
return
false
;
if
(
inputConfIntervals_
.
rows
()
!=
castother
.
inputConfIntervals_
.
rows
())
return
false
;
if
(
outputConfIntervals_
.
rows
()
!=
castother
.
outputConfIntervals_
.
rows
())
return
false
;
if
(
inputDerivIntervals_
.
rows
()
!=
castother
.
inputDerivIntervals_
.
rows
())
return
false
;
if
(
outputDerivIntervals_
.
rows
()
!=
castother
.
outputDerivIntervals_
.
rows
())
return
false
;
return
true
;
}
private:
void
computeJacobianBlocks
();
...
...
include/hpp/constraints/explicit/relative-transformation.hh
View file @
1d933ee1
...
...
@@ -112,6 +112,37 @@ namespace hpp {
void
impl_jacobian
(
matrixOut_t
jacobian
,
vectorIn_t
arg
)
const
;
bool
isEqual
(
const
DifferentiableFunction
&
other
)
const
{
const
RelativeTransformation
&
castother
=
dynamic_cast
<
const
RelativeTransformation
&>
(
other
);
if
(
!
DifferentiableFunction
::
isEqual
(
other
))
return
false
;
if
(
robot_
!=
castother
.
robot_
)
return
false
;
if
(
parentJoint_
!=
castother
.
parentJoint_
)
return
false
;
if
(
joint1_
!=
castother
.
joint1_
)
return
false
;
if
(
joint2_
!=
castother
.
joint2_
)
return
false
;
if
(
frame1_
!=
castother
.
frame1_
)
return
false
;
if
(
frame2_
!=
castother
.
frame2_
)
return
false
;
if
(
inConf_
.
rows
()
!=
castother
.
inConf_
.
rows
())
return
false
;
if
(
inVel_
.
cols
()
!=
castother
.
inVel_
.
cols
())
return
false
;
if
(
outConf_
.
rows
()
!=
castother
.
outConf_
.
rows
())
return
false
;
if
(
outVel_
.
rows
()
!=
castother
.
outVel_
.
rows
())
return
false
;
if
(
F1inJ1_invF2inJ2_
!=
castother
.
F1inJ1_invF2inJ2_
)
return
false
;
return
true
;
}
private:
void
forwardKinematics
(
vectorIn_t
arg
)
const
;
...
...
include/hpp/constraints/function/difference.hh
View file @
1d933ee1
...
...
@@ -58,6 +58,25 @@ namespace hpp {
J
.
middleCols
(
rsd_
.
first
,
rsd_
.
second
)
*=
-
1
;
}
bool
isEqual
(
const
DifferentiableFunction
&
other
)
const
{
const
Difference
&
castother
=
dynamic_cast
<
const
Difference
&>
(
other
);
if
(
!
DifferentiableFunction
::
isEqual
(
other
))
return
false
;
if
(
inner_
!=
castother
.
inner_
)
return
false
;
if
(
lsa_
!=
castother
.
lsa_
)
return
false
;
if
(
lsd_
!=
castother
.
lsd_
)
return
false
;
if
(
rsa_
!=
castother
.
rsa_
)
return
false
;
if
(
rsd_
!=
castother
.
rsd_
)
return
false
;
return
true
;
}
std
::
ostream
&
print
(
std
::
ostream
&
os
)
const
;
DifferentiableFunctionPtr_t
inner_
;
...
...
include/hpp/constraints/function/of-parameter-subset.hh
View file @
1d933ee1
...
...
@@ -79,6 +79,21 @@ namespace hpp {
arg
.
segment
(
sa_
.
first
,
sa_
.
second
));
}
bool
isEqual
(
const
DifferentiableFunction
&
other
)
const
{
const
OfParameterSubset
&
castother
=
dynamic_cast
<
const
OfParameterSubset
&>
(
other
);
if
(
!
DifferentiableFunction
::
isEqual
(
other
))
return
false
;
if
(
g_
!=
castother
.
g_
)
return
false
;
if
(
sa_
!=
castother
.
sa_
)
return
false
;
if
(
sd_
!=
castother
.
sd_
)
return
false
;
return
true
;
}
std
::
ostream
&
print
(
std
::
ostream
&
os
)
const
;
DifferentiableFunctionPtr_t
g_
;
...
...
include/hpp/constraints/generic-transformation.hh
View file @
1d933ee1
...
...
@@ -309,6 +309,19 @@ namespace hpp {
ConfigurationIn_t
argument
)
const
;
virtual
void
impl_jacobian
(
matrixOut_t
jacobian
,
ConfigurationIn_t
arg
)
const
;
bool
isEqual
(
const
DifferentiableFunction
&
other
)
const
{
const
GenericTransformation
&
castother
=
dynamic_cast
<
const
GenericTransformation
&>
(
other
);
if
(
!
DifferentiableFunction
::
isEqual
(
other
))
return
false
;
if
(
robot_
!=
castother
.
robot_
)
return
false
;
if
(
mask_
!=
castother
.
mask_
)
return
false
;
return
true
;
}
private:
void
computeActiveParams
();
DevicePtr_t
robot_
;
...
...
include/hpp/constraints/manipulability.hh
View file @
1d933ee1
...
...
@@ -55,6 +55,25 @@ namespace hpp {
void
impl_jacobian
(
matrixOut_t
jacobian
,
vectorIn_t
arg
)
const
;
bool
isEqual
(
const
DifferentiableFunction
&
other
)
const
{
const
Manipulability
&
castother
=
dynamic_cast
<
const
Manipulability
&>
(
other
);
if
(
!
DifferentiableFunction
::
isEqual
(
other
))
return
false
;
if
(
function_
!=
castother
.
function_
)
return
false
;
if
(
robot_
!=
castother
.
robot_
)
return
false
;
if
(
cols_
.
cols
()
!=
castother
.
cols_
.
cols
())
return
false
;
if
(
J_
!=
castother
.
J_
)
return
false
;
if
(
J_JT_
!=
castother
.
J_JT_
)
return
false
;
return
true
;
}
private:
DifferentiableFunctionPtr_t
function_
;
DevicePtr_t
robot_
;
...
...
include/hpp/constraints/qp-static-stability.hh
View file @
1d933ee1
...
...
@@ -72,7 +72,27 @@ namespace hpp {
MatrixOfExpressions
<>&
phi
()
{
return
phi_
;
}
protected:
bool
isEqual
(
const
DifferentiableFunction
&
other
)
const
{
const
QPStaticStability
&
castother
=
dynamic_cast
<
const
QPStaticStability
&>
(
other
);
if
(
!
DifferentiableFunction
::
isEqual
(
other
))
return
false
;
if
(
name_
!=
castother
.
name_
)
return
false
;
if
(
robot_
!=
castother
.
robot_
)
return
false
;
if
(
nbContacts_
!=
castother
.
nbContacts_
)
return
false
;
if
(
com_
!=
castother
.
com_
)
return
false
;
if
(
Zeros
!=
castother
.
Zeros
)
return
false
;
if
(
nWSR
!=
castother
.
nWSR
)
return
false
;
return
true
;
}
private:
static
const
Eigen
::
Matrix
<
value_type
,
6
,
1
>
MinusGravity
;
...
...
include/hpp/constraints/relative-com.hh
View file @
1d933ee1
...
...
@@ -97,6 +97,25 @@ namespace hpp {
const
;
virtual
void
impl_jacobian
(
matrixOut_t
jacobian
,
ConfigurationIn_t
arg
)
const
;
bool
isEqual
(
const
DifferentiableFunction
&
other
)
const
{
const
RelativeCom
&
castother
=
dynamic_cast
<
const
RelativeCom
&>
(
other
);
if
(
!
DifferentiableFunction
::
isEqual
(