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
adfde796
Unverified
Commit
adfde796
authored
Feb 25, 2022
by
Florent Lamiraux
Committed by
GitHub
Feb 25, 2022
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
Changes
5
Pipelines
1
Hide whitespace changes
Inline
Side-by-side
include/hpp/constraints/convex-shape-contact.hh
View file @
adfde796
...
...
@@ -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_
;
...
...
include/hpp/constraints/differentiable-function.hh
View file @
adfde796
...
...
@@ -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.
...
...
include/hpp/constraints/matrix-view.hh
View file @
adfde796
...
...
@@ -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
...
...
src/convex-shape-contact.cc
View file @
adfde796
...
...
@@ -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
src/differentiable-function.cc
View file @
adfde796
...
...
@@ -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
)
:
...
...
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment