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
a5e8eea1
Commit
a5e8eea1
authored
Apr 07, 2022
by
Florent Lamiraux
Browse files
[QPStaticStability] Refactor and document.
parent
5a4f363f
Changes
4
Hide whitespace changes
Inline
Side-by-side
include/hpp/constraints/qp-static-stability.hh
View file @
a5e8eea1
...
...
@@ -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 @
a5e8eea1
...
...
@@ -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/qp-static-stability.cc
View file @
a5e8eea1
...
...
@@ -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 @
a5e8eea1
...
...
@@ -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