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
4f59546d
Unverified
Commit
4f59546d
authored
Apr 15, 2022
by
Florent Lamiraux
Committed by
GitHub
Apr 15, 2022
Browse files
Merge pull request #154 from florent-lamiraux/devel
[ConvexShapeContact] Add a check before creating an explicit constraint
parents
5a4f363f
581777e3
Changes
5
Hide whitespace changes
Inline
Side-by-side
include/hpp/constraints/qp-static-stability.hh
View file @
4f59546d
...
...
@@ -42,6 +42,17 @@ namespace hpp {
/// \addtogroup constraints
/// \{
///
/// Quasi static equilibrium constraint defined as a QP program
///
/// This class defines a function whose value is equal to zero
/// if and only if there exists a set of non-negative forces applied
/// at some given contact points along some given normals that
// counter-balance the gravity force and momentum of a kinematics chain.
///
/// \sa https://hal.archives-ouvertes.fr/tel-01516897 Chapter 2, Sections
/// 3.1 and 3.2.
class
HPP_CONSTRAINTS_DLLAPI
QPStaticStability
:
public
DifferentiableFunction
{
public:
static
const
Eigen
::
Matrix
<
value_type
,
6
,
1
>
Gravity
;
...
...
@@ -51,7 +62,9 @@ namespace hpp {
typedef
ConvexShapeContact
::
ForceData
ForceData
;
/// Constructor
/// \param name name of the constraint
/// \param robot the robot the constraints is applied to,
/// \param contacts set of contact points
/// \param com COM of the robot
QPStaticStability
(
const
std
::
string
&
name
,
const
DevicePtr_t
&
robot
,
const
Contacts_t
&
contacts
,
...
...
@@ -89,7 +102,7 @@ namespace hpp {
const
QPStaticStability
&
castother
=
dynamic_cast
<
const
QPStaticStability
&>
(
other
);
if
(
!
DifferentiableFunction
::
isEqual
(
other
))
return
false
;
if
(
name
()
!=
castother
.
name
())
return
false
;
if
(
robot_
!=
castother
.
robot_
)
...
...
@@ -102,7 +115,7 @@ namespace hpp {
return
false
;
if
(
nWSR
!=
castother
.
nWSR
)
return
false
;
return
true
;
}
private:
...
...
include/hpp/constraints/static-stability.hh
View file @
4f59546d
...
...
@@ -50,16 +50,13 @@ namespace hpp {
static
const
Eigen
::
Matrix
<
value_type
,
6
,
1
>
Gravity
;
struct
Contact_t
{
JointPtr_t
joint
1
,
joint2
;
vector3_t
point
1
,
point2
;
vector3_t
normal
1
,
normal2
;
JointPtr_t
joint
;
vector3_t
point
;
vector3_t
normal
;
bool
operator
==
(
Contact_t
const
&
other
)
const
{
if
(
joint1
!=
other
.
joint1
)
return
false
;
if
(
joint2
!=
other
.
joint2
)
return
false
;
if
(
point1
!=
other
.
point1
)
return
false
;
if
(
point2
!=
other
.
point2
)
return
false
;
if
(
normal1
!=
other
.
normal1
)
return
false
;
if
(
normal2
!=
other
.
normal2
)
return
false
;
if
(
joint
!=
other
.
joint
)
return
false
;
if
(
point
!=
other
.
point
)
return
false
;
if
(
normal
!=
other
.
normal
)
return
false
;
return
true
;
}
};
...
...
src/explicit/convex-shape-contact.cc
View file @
4f59546d
...
...
@@ -36,12 +36,51 @@
namespace
hpp
{
namespace
constraints
{
namespace
explicit_
{
// Check that object contact surface
// - belong to the same object (joint),
// - are on a freeflyer joint.
// Throw in case these assertions are not true
static
void
checkContactSurfaces
(
const
JointAndShapes_t
&
objectSurfaces
)
{
JointPtr_t
joint
(
0x0
);
for
(
auto
js
:
objectSurfaces
){
if
(
!
joint
)
{
joint
=
js
.
first
;
// Check that joint is a freeflyer
if
((
*
joint
->
configurationSpace
()
!=*
pinocchio
::
LiegroupSpace
::
SE3
())
&&
(
*
joint
->
configurationSpace
()
!=*
pinocchio
::
LiegroupSpace
::
R3xSO3
())){
std
::
ostringstream
os
;
os
<<
"You are trying to build an explicit contact constraint but"
" the joint that holds at least on object contact surface is "
" not a freeflyer joint: "
<<
joint
->
name
();
throw
std
::
logic_error
(
os
.
str
().
c_str
());
if
(
joint
->
parentJoint
()){
os
<<
"You are trying to build an explicit contact constraint "
"but the joint that holds at least on object contact "
"surface: "
<<
joint
->
name
()
<<
" is attached to another joint. This is not supported"
;
throw
std
::
logic_error
(
os
.
str
().
c_str
());
}
}
}
else
if
(
js
.
first
!=
joint
){
std
::
ostringstream
os
;
os
<<
"You are trying to build an explicit contact constraint "
"but several joints hold object contact surface: "
<<
joint
->
name
()
<<
" and "
<<
js
.
first
->
name
();
throw
std
::
logic_error
(
os
.
str
().
c_str
());
}
}
}
ConvexShapeContactPtr_t
ConvexShapeContact
::
create
(
const
std
::
string
&
name
,
DevicePtr_t
robot
,
const
JointAndShapes_t
&
floorSurfaces
,
const
JointAndShapes_t
&
objectSurfaces
,
const
value_type
&
margin
)
{
checkContactSurfaces
(
objectSurfaces
);
ConvexShapeContact
*
ptr
(
new
ConvexShapeContact
(
name
,
robot
,
floorSurfaces
,
objectSurfaces
,
margin
));
...
...
src/qp-static-stability.cc
View file @
4f59546d
...
...
@@ -77,9 +77,9 @@ namespace hpp {
Traits
<
PointCom
>::
Ptr_t
OG
=
PointCom
::
create
(
com
);
for
(
std
::
size_t
i
=
0
;
i
<
contacts
.
size
();
++
i
)
{
Traits
<
PointInJoint
>::
Ptr_t
OP2
=
PointInJoint
::
create
(
contacts
[
i
].
joint
2
,
contacts
[
i
].
point
2
,
robot
->
numberDof
());
(
contacts
[
i
].
joint
,
contacts
[
i
].
point
,
robot
->
numberDof
());
Traits
<
VectorInJoint
>::
Ptr_t
n2
=
VectorInJoint
::
create
(
contacts
[
i
].
joint
2
,
contacts
[
i
].
normal
2
,
robot
->
numberDof
());
(
contacts
[
i
].
joint
,
contacts
[
i
].
normal
,
robot
->
numberDof
());
phi_
(
0
,
i
)
=
n2
;
phi_
(
1
,
i
)
=
(
OG
-
OP2
)
^
n2
;
...
...
@@ -110,7 +110,7 @@ namespace hpp {
std
::
size_t
col
=
0
;
for
(
std
::
size_t
i
=
0
;
i
<
contacts
.
size
();
++
i
)
{
Traits
<
VectorInJoint
>::
Ptr_t
n
=
VectorInJoint
::
create
(
contacts
[
i
].
joint
,
contacts
[
i
].
normal
,
robot
->
numberDof
());
(
contacts
[
i
].
joint
,
contacts
[
i
].
normal
,
robot
->
numberDof
());
for
(
std
::
size_t
j
=
0
;
j
<
contacts
[
i
].
points
.
size
();
++
j
)
{
Traits
<
PointInJoint
>::
Ptr_t
OP
=
PointInJoint
::
create
(
contacts
[
i
].
joint
,
contacts
[
i
].
points
[
j
],
robot
->
numberDof
());
...
...
src/static-stability.cc
View file @
4f59546d
...
...
@@ -63,9 +63,9 @@ namespace hpp {
Traits
<
PointCom
>::
Ptr_t
OG
=
PointCom
::
create
(
com
);
for
(
std
::
size_t
i
=
0
;
i
<
contacts
.
size
();
++
i
)
{
Traits
<
PointInJoint
>::
Ptr_t
OP2
=
PointInJoint
::
create
(
contacts
[
i
].
joint
2
,
contacts
[
i
].
point
2
,
robot
->
numberDof
());
PointInJoint
::
create
(
contacts
[
i
].
joint
,
contacts
[
i
].
point
,
robot
->
numberDof
());
Traits
<
VectorInJoint
>::
Ptr_t
n2
=
VectorInJoint
::
create
(
contacts
[
i
].
joint
2
,
contacts
[
i
].
normal
2
,
robot
->
numberDof
());
VectorInJoint
::
create
(
contacts
[
i
].
joint
,
contacts
[
i
].
normal
,
robot
->
numberDof
());
phi_
(
0
,
i
)
=
n2
;
phi_
(
1
,
i
)
=
(
OG
-
OP2
)
^
n2
;
...
...
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