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-centroidal-dynamics
Commits
dff4d63d
Commit
dff4d63d
authored
Apr 05, 2022
by
Guilhem Saurel
Browse files
format
parent
645cf409
Changes
20
Expand all
Hide whitespace changes
Inline
Side-by-side
LICENSE
View file @
dff4d63d
...
...
@@ -337,4 +337,3 @@ proprietary programs. If your program is a subroutine library, you may
consider it more useful to permit linking proprietary applications with the
library. If this is what you want to do, use the GNU Lesser General
Public License instead of this License.
include/hpp/centroidal-dynamics/Stdafx.hh
View file @
dff4d63d
...
...
@@ -29,9 +29,9 @@ WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
#pragma once
#include
<ctime>
#include
<iostream>
#include
<map>
#include
<ctime>
#include
<sstream>
#endif
include/hpp/centroidal-dynamics/centroidal_dynamics.hh
View file @
dff4d63d
This diff is collapsed.
Click to expand it.
include/hpp/centroidal-dynamics/logger.hh
View file @
dff4d63d
...
...
@@ -10,10 +10,11 @@
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include
<hpp/centroidal-dynamics/local_config.hh>
#include
<sstream>
#include
<Eigen/Dense>
#include
<hpp/centroidal-dynamics/local_config.hh>
#include
<map>
#include
<sstream>
#include
"boost/assign.hpp"
namespace
centroidal_dynamics
{
...
...
@@ -43,7 +44,8 @@ namespace centroidal_dynamics {
#define SEND_WARNING_MSG(msg) SEND_MSG(msg, MSG_TYPE_WARNING)
#define SEND_ERROR_MSG(msg) SEND_MSG(msg, MSG_TYPE_ERROR)
#define SEND_DEBUG_STREAM_MSG(msg)
#define SEND_INFO_STREAM_MSG(msg) #define SEND_WARNING_STREAM_MSG(msg) SEND_MSG(msg, MSG_TYPE_WARNING_STREAM)
#define SEND_INFO_STREAM_MSG(msg) \
#define SEND_WARNING_STREAM_MSG(msg) SEND_MSG(msg, MSG_TYPE_WARNING_STREAM)
#define SEND_ERROR_STREAM_MSG(msg) SEND_MSG(msg, MSG_TYPE_ERROR_STREAM)
#endif
...
...
@@ -90,7 +92,8 @@ std::string toString(const T& v) {
}
template
<
typename
T
>
std
::
string
toString
(
const
std
::
vector
<
T
>&
v
,
const
std
::
string
separator
=
", "
)
{
std
::
string
toString
(
const
std
::
vector
<
T
>&
v
,
const
std
::
string
separator
=
", "
)
{
std
::
stringstream
ss
;
for
(
int
i
=
0
;
i
<
v
.
size
()
-
1
;
i
++
)
ss
<<
v
[
i
]
<<
separator
;
ss
<<
v
[
v
.
size
()
-
1
];
...
...
@@ -98,7 +101,8 @@ std::string toString(const std::vector<T>& v, const std::string separator = ", "
}
template
<
typename
T
,
int
n
>
std
::
string
toString
(
const
Eigen
::
MatrixBase
<
T
>&
v
,
const
std
::
string
separator
=
", "
)
{
std
::
string
toString
(
const
Eigen
::
MatrixBase
<
T
>&
v
,
const
std
::
string
separator
=
", "
)
{
if
(
v
.
rows
()
>
v
.
cols
())
return
toString
(
v
.
transpose
(),
separator
);
std
::
stringstream
ss
;
ss
<<
v
;
...
...
@@ -132,7 +136,8 @@ class CENTROIDAL_DYNAMICS_DLLAPI Logger {
* the point where sendMsg is called so that streaming messages are
* printed only every streamPrintPeriod iterations.
*/
void
sendMsg
(
std
::
string
msg
,
MsgType
type
,
const
char
*
file
=
""
,
int
line
=
0
);
void
sendMsg
(
std
::
string
msg
,
MsgType
type
,
const
char
*
file
=
""
,
int
line
=
0
);
/** Set the sampling time at which the method countdown()
* is going to be called. */
...
...
@@ -145,26 +150,36 @@ class CENTROIDAL_DYNAMICS_DLLAPI Logger {
void
setVerbosity
(
LoggerVerbosity
lv
);
protected:
LoggerVerbosity
m_lv
;
/// verbosity of the logger
double
m_timeSample
;
/// specify the period of call of the countdown method
LoggerVerbosity
m_lv
;
/// verbosity of the logger
double
m_timeSample
;
/// specify the period of call of the countdown method
double
m_streamPrintPeriod
;
/// specify the time period of the stream prints
double
m_printCountdown
;
/// every time this is < 0 (i.e. every _streamPrintPeriod sec) print stuff
double
m_printCountdown
;
/// every time this is < 0 (i.e. every
/// _streamPrintPeriod sec) print stuff
/** Pointer to the dynamic structure which holds the collection of streaming messages */
/** Pointer to the dynamic structure which holds the collection of streaming
* messages */
std
::
map
<
std
::
string
,
double
>
m_stream_msg_counters
;
bool
isStreamMsg
(
MsgType
m
)
{
return
m
==
MSG_TYPE_ERROR_STREAM
||
m
==
MSG_TYPE_DEBUG_STREAM
||
m
==
MSG_TYPE_INFO_STREAM
||
m
==
MSG_TYPE_WARNING_STREAM
;
return
m
==
MSG_TYPE_ERROR_STREAM
||
m
==
MSG_TYPE_DEBUG_STREAM
||
m
==
MSG_TYPE_INFO_STREAM
||
m
==
MSG_TYPE_WARNING_STREAM
;
}
bool
isDebugMsg
(
MsgType
m
)
{
return
m
==
MSG_TYPE_DEBUG_STREAM
||
m
==
MSG_TYPE_DEBUG
;
}
bool
isDebugMsg
(
MsgType
m
)
{
return
m
==
MSG_TYPE_DEBUG_STREAM
||
m
==
MSG_TYPE_DEBUG
;
}
bool
isInfoMsg
(
MsgType
m
)
{
return
m
==
MSG_TYPE_INFO_STREAM
||
m
==
MSG_TYPE_INFO
;
}
bool
isInfoMsg
(
MsgType
m
)
{
return
m
==
MSG_TYPE_INFO_STREAM
||
m
==
MSG_TYPE_INFO
;
}
bool
isWarningMsg
(
MsgType
m
)
{
return
m
==
MSG_TYPE_WARNING_STREAM
||
m
==
MSG_TYPE_WARNING
;
}
bool
isWarningMsg
(
MsgType
m
)
{
return
m
==
MSG_TYPE_WARNING_STREAM
||
m
==
MSG_TYPE_WARNING
;
}
bool
isErrorMsg
(
MsgType
m
)
{
return
m
==
MSG_TYPE_ERROR_STREAM
||
m
==
MSG_TYPE_ERROR
;
}
bool
isErrorMsg
(
MsgType
m
)
{
return
m
==
MSG_TYPE_ERROR_STREAM
||
m
==
MSG_TYPE_ERROR
;
}
};
/** Method to get the logger (singleton). */
...
...
include/hpp/centroidal-dynamics/solver_LP_abstract.hh
View file @
dff4d63d
...
...
@@ -65,8 +65,9 @@ class CENTROIDAL_DYNAMICS_DLLAPI Solver_LP_abstract {
* subject to Alb <= A x <= Aub
* lb <= x <= ub
*/
virtual
LP_status
solve
(
Cref_vectorX
c
,
Cref_vectorX
lb
,
Cref_vectorX
ub
,
Cref_matrixXX
A
,
Cref_vectorX
Alb
,
Cref_vectorX
Aub
,
Ref_vectorX
sol
)
=
0
;
virtual
LP_status
solve
(
Cref_vectorX
c
,
Cref_vectorX
lb
,
Cref_vectorX
ub
,
Cref_matrixXX
A
,
Cref_vectorX
Alb
,
Cref_vectorX
Aub
,
Ref_vectorX
sol
)
=
0
;
/**
* @brief Solve the linear program described in the specified file.
...
...
@@ -90,13 +91,13 @@ class CENTROIDAL_DYNAMICS_DLLAPI Solver_LP_abstract {
* @param Aub
* @return True if the operation succeeded, false otherwise.
*/
virtual
bool
writeLpToFile
(
const
std
::
string
&
filename
,
Cref_vectorX
c
,
Cref_vectorX
lb
,
Cref_vectorX
ub
,
Cref_matrixXX
A
,
Cref_vectorX
Alb
,
Cref_vectorX
Aub
);
virtual
bool
writeLpToFile
(
const
std
::
string
&
filename
,
Cref_vectorX
c
,
Cref_vectorX
lb
,
Cref_vectorX
ub
,
Cref_matrixXX
A
,
Cref_vectorX
Alb
,
Cref_vectorX
Aub
);
/**
* @brief Read the data describing a Linear Program from the specified binary file.
* The vectors and matrices are resized inside the method.
* minimize c' x
* @brief Read the data describing a Linear Program from the specified binary
* file. The vectors and matrices are resized inside the method. minimize c' x
* subject to Alb <= A x <= Aub
* lb <= x <= ub
* @param filename
...
...
@@ -108,7 +109,8 @@ class CENTROIDAL_DYNAMICS_DLLAPI Solver_LP_abstract {
* @param Aub
* @return True if the operation succeeded, false otherwise.
*/
virtual
bool
readLpFromFile
(
const
std
::
string
&
filename
,
VectorX
&
c
,
VectorX
&
lb
,
VectorX
&
ub
,
MatrixXX
&
A
,
virtual
bool
readLpFromFile
(
const
std
::
string
&
filename
,
VectorX
&
c
,
VectorX
&
lb
,
VectorX
&
ub
,
MatrixXX
&
A
,
VectorX
&
Alb
,
VectorX
&
Aub
);
/** Get the status of the solver. */
...
...
@@ -123,7 +125,9 @@ class CENTROIDAL_DYNAMICS_DLLAPI Solver_LP_abstract {
/** Return true if the solver is allowed to warm start, false otherwise. */
virtual
bool
getUseWarmStart
()
{
return
m_useWarmStart
;
}
/** Specify whether the solver is allowed to use warm-start techniques. */
virtual
void
setUseWarmStart
(
bool
useWarmStart
)
{
m_useWarmStart
=
useWarmStart
;
}
virtual
void
setUseWarmStart
(
bool
useWarmStart
)
{
m_useWarmStart
=
useWarmStart
;
}
/** Get the current maximum number of iterations performed by the solver. */
virtual
unsigned
int
getMaximumIterations
()
{
return
m_maxIter
;
}
...
...
include/hpp/centroidal-dynamics/solver_LP_clp.hh
View file @
dff4d63d
...
...
@@ -9,8 +9,9 @@
#define CENTROIDAL_DYNAMICS_LIB_SOLVER_LP_CLP_HH
#include
<hpp/centroidal-dynamics/local_config.hh>
#include
<hpp/centroidal-dynamics/util.hh>
#include
<hpp/centroidal-dynamics/solver_LP_abstract.hh>
#include
<hpp/centroidal-dynamics/util.hh>
#include
"ClpSimplex.hpp"
namespace
centroidal_dynamics
{
...
...
@@ -27,8 +28,9 @@ class CENTROIDAL_DYNAMICS_DLLAPI Solver_LP_clp : public Solver_LP_abstract {
* subject to Alb <= A x <= Aub
* lb <= x <= ub
*/
virtual
LP_status
solve
(
Cref_vectorX
c
,
Cref_vectorX
lb
,
Cref_vectorX
ub
,
Cref_matrixXX
A
,
Cref_vectorX
Alb
,
Cref_vectorX
Aub
,
Ref_vectorX
sol
);
virtual
LP_status
solve
(
Cref_vectorX
c
,
Cref_vectorX
lb
,
Cref_vectorX
ub
,
Cref_matrixXX
A
,
Cref_vectorX
Alb
,
Cref_vectorX
Aub
,
Ref_vectorX
sol
);
/** Get the status of the solver. */
virtual
LP_status
getStatus
();
...
...
include/hpp/centroidal-dynamics/solver_LP_qpoases.hh
View file @
dff4d63d
...
...
@@ -7,8 +7,8 @@
#define HPP_CENTROIDAL_DYNAMICS_SOLVER_LP_QPOASES_HH
#include
<hpp/centroidal-dynamics/local_config.hh>
#include
<hpp/centroidal-dynamics/util.hh>
#include
<hpp/centroidal-dynamics/solver_LP_abstract.hh>
#include
<hpp/centroidal-dynamics/util.hh>
#include
<qpOASES.hpp>
namespace
centroidal_dynamics
{
...
...
@@ -18,8 +18,8 @@ class CENTROIDAL_DYNAMICS_DLLAPI Solver_LP_qpoases : public Solver_LP_abstract {
qpOASES
::
Options
m_options
;
// solver options
qpOASES
::
SQProblem
m_solver
;
// qpoases solver
MatrixXX
m_H
;
// Hessian matrix
bool
m_init_succeeded
;
// true if solver has been successfully initialized
MatrixXX
m_H
;
// Hessian matrix
bool
m_init_succeeded
;
// true if solver has been successfully initialized
qpOASES
::
returnValue
m_status
;
// status code returned by the solver
public:
...
...
@@ -32,8 +32,9 @@ class CENTROIDAL_DYNAMICS_DLLAPI Solver_LP_qpoases : public Solver_LP_abstract {
* subject to Alb <= A x <= Aub
* lb <= x <= ub
*/
LP_status
solve
(
Cref_vectorX
c
,
Cref_vectorX
lb
,
Cref_vectorX
ub
,
Cref_matrixXX
A
,
Cref_vectorX
Alb
,
Cref_vectorX
Aub
,
Ref_vectorX
sol
);
LP_status
solve
(
Cref_vectorX
c
,
Cref_vectorX
lb
,
Cref_vectorX
ub
,
Cref_matrixXX
A
,
Cref_vectorX
Alb
,
Cref_vectorX
Aub
,
Ref_vectorX
sol
);
/** Get the status of the solver. */
virtual
LP_status
getStatus
();
...
...
@@ -41,7 +42,9 @@ class CENTROIDAL_DYNAMICS_DLLAPI Solver_LP_qpoases : public Solver_LP_abstract {
/** Get the objective value of the last solved problem. */
virtual
double
getObjectiveValue
()
{
return
m_solver
.
getObjVal
();
}
virtual
void
getDualSolution
(
Ref_vectorX
res
)
{
m_solver
.
getDualSolution
(
res
.
data
());
}
virtual
void
getDualSolution
(
Ref_vectorX
res
)
{
m_solver
.
getDualSolution
(
res
.
data
());
}
};
}
// end namespace centroidal_dynamics
...
...
include/hpp/centroidal-dynamics/stop-watch.hh
View file @
dff4d63d
...
...
@@ -169,7 +169,8 @@ class Stopwatch {
void
reset_all
();
/** Dump the data of a certain performance record */
void
report
(
std
::
string
perf_name
,
int
precision
=
2
,
std
::
ostream
&
output
=
std
::
cout
);
void
report
(
std
::
string
perf_name
,
int
precision
=
2
,
std
::
ostream
&
output
=
std
::
cout
);
/** Dump the data of all the performance records */
void
report_all
(
int
precision
=
2
,
std
::
ostream
&
output
=
std
::
cout
);
...
...
@@ -207,7 +208,13 @@ class Stopwatch {
/** Struct to hold the performance data */
struct
PerformanceData
{
PerformanceData
()
:
clock_start
(
0
),
total_time
(
0
),
min_time
(
0
),
max_time
(
0
),
last_time
(
0
),
paused
(
false
),
stops
(
0
)
{}
:
clock_start
(
0
),
total_time
(
0
),
min_time
(
0
),
max_time
(
0
),
last_time
(
0
),
paused
(
false
),
stops
(
0
)
{}
/** Start time */
long
double
clock_start
;
...
...
include/hpp/centroidal-dynamics/util.hh
View file @
dff4d63d
...
...
@@ -5,18 +5,18 @@
#ifndef HPP_CENTROIDAL_DYNAMICS_UTIL_HH
#define HPP_CENTROIDAL_DYNAMICS_UTIL_HH
#include
<iostream>
#include
<fstream>
#include
<cmath>
#include
<cassert>
#include
<Eigen/src/Core/util/Macros.h>
#include
<Eigen/Dense>
#include
<Eigen/src/Core/util/Macros.h>
#include
<cassert>
#include
<cmath>
#include
<fstream>
#include
<iostream>
#include
"cdd.h"
#include
"cddmp.h"
#include
"setoper.h"
#include
"cddtypes.h"
#include
"
cdd
.h"
#include
"
setoper
.h"
namespace
centroidal_dynamics
{
...
...
@@ -44,7 +44,9 @@ typedef Eigen::Matrix<value_type, 6, Eigen::Dynamic, Eigen::RowMajor> Matrix6X;
typedef
Eigen
::
Matrix
<
value_type
,
6
,
2
,
Eigen
::
RowMajor
>
Matrix62
;
typedef
Eigen
::
Matrix
<
value_type
,
6
,
3
,
Eigen
::
RowMajor
>
Matrix63
;
typedef
Eigen
::
Matrix
<
value_type
,
Eigen
::
Dynamic
,
6
,
Eigen
::
RowMajor
>
MatrixX6
;
typedef
Eigen
::
Matrix
<
value_type
,
Eigen
::
Dynamic
,
Eigen
::
Dynamic
,
Eigen
::
RowMajor
>
MatrixXX
;
typedef
Eigen
::
Matrix
<
value_type
,
Eigen
::
Dynamic
,
Eigen
::
Dynamic
,
Eigen
::
RowMajor
>
MatrixXX
;
typedef
Eigen
::
Ref
<
Vector2
>
Ref_vector2
;
typedef
Eigen
::
Ref
<
Vector3
>
Ref_vector3
;
...
...
@@ -67,8 +69,11 @@ typedef const Eigen::Ref<const Matrix63>& Cref_matrix63;
typedef
const
Eigen
::
Ref
<
const
MatrixXX
>&
Cref_matrixXX
;
/**Column major definitions for compatibility with classical eigen use**/
typedef
Eigen
::
Matrix
<
value_type
,
Eigen
::
Dynamic
,
3
,
Eigen
::
ColMajor
>
MatrixX3ColMajor
;
typedef
Eigen
::
Matrix
<
value_type
,
Eigen
::
Dynamic
,
Eigen
::
Dynamic
,
Eigen
::
ColMajor
>
MatrixXXColMajor
;
typedef
Eigen
::
Matrix
<
value_type
,
Eigen
::
Dynamic
,
3
,
Eigen
::
ColMajor
>
MatrixX3ColMajor
;
typedef
Eigen
::
Matrix
<
value_type
,
Eigen
::
Dynamic
,
Eigen
::
Dynamic
,
Eigen
::
ColMajor
>
MatrixXXColMajor
;
typedef
const
Eigen
::
Ref
<
const
MatrixX3ColMajor
>&
Cref_matrixX3ColMajor
;
typedef
Eigen
::
Ref
<
MatrixXXColMajor
>&
ref_matrixXXColMajor
;
...
...
@@ -77,12 +82,14 @@ typedef Eigen::Ref<MatrixXXColMajor>& ref_matrixXXColMajor;
*/
template
<
class
Matrix
>
bool
writeMatrixToFile
(
const
std
::
string
&
filename
,
const
Matrix
&
matrix
)
{
std
::
ofstream
out
(
filename
.
c_str
(),
std
::
ios
::
out
|
std
::
ios
::
binary
|
std
::
ios
::
trunc
);
std
::
ofstream
out
(
filename
.
c_str
(),
std
::
ios
::
out
|
std
::
ios
::
binary
|
std
::
ios
::
trunc
);
if
(
!
out
.
is_open
())
return
false
;
typename
Matrix
::
Index
rows
=
matrix
.
rows
(),
cols
=
matrix
.
cols
();
out
.
write
((
char
*
)(
&
rows
),
sizeof
(
typename
Matrix
::
Index
));
out
.
write
((
char
*
)(
&
cols
),
sizeof
(
typename
Matrix
::
Index
));
out
.
write
((
char
*
)
matrix
.
data
(),
rows
*
cols
*
sizeof
(
typename
Matrix
::
Scalar
));
out
.
write
((
char
*
)
matrix
.
data
(),
rows
*
cols
*
sizeof
(
typename
Matrix
::
Scalar
));
out
.
close
();
return
true
;
}
...
...
@@ -110,10 +117,12 @@ bool readMatrixFromFile(const std::string& filename, Matrix& matrix) {
* @return The mX(n+1) output cdd matrix, which contains an additional column,
* the first one, with all zeros.
*/
dd_MatrixPtr
cone_span_eigen_to_cdd
(
Cref_matrixXX
input
,
const
bool
canonicalize
=
false
);
dd_MatrixPtr
cone_span_eigen_to_cdd
(
Cref_matrixXX
input
,
const
bool
canonicalize
=
false
);
/**
* Compute the cross-product skew-symmetric matrix associated to the specified vector.
* Compute the cross-product skew-symmetric matrix associated to the specified
* vector.
*/
Rotation
crossMatrix
(
Cref_vector3
x
);
...
...
@@ -121,13 +130,17 @@ void init_cdd_library();
void
release_cdd_library
();
// in some distribution the conversion Ref_matrixXX to Ref_vector3 does not compile
void
uniform3
(
Cref_vector3
lower_bounds
,
Cref_vector3
upper_bounds
,
Ref_vector3
out
);
void
uniform
(
Cref_matrixXX
lower_bounds
,
Cref_matrixXX
upper_bounds
,
Ref_matrixXX
out
);
// in some distribution the conversion Ref_matrixXX to Ref_vector3 does not
// compile
void
uniform3
(
Cref_vector3
lower_bounds
,
Cref_vector3
upper_bounds
,
Ref_vector3
out
);
void
uniform
(
Cref_matrixXX
lower_bounds
,
Cref_matrixXX
upper_bounds
,
Ref_matrixXX
out
);
void
euler_matrix
(
double
roll
,
double
pitch
,
double
yaw
,
Ref_rotation
R
);
bool
generate_rectangle_contacts
(
double
lx
,
double
ly
,
Cref_vector3
pos
,
Cref_vector3
rpy
,
Ref_matrix43
p
,
bool
generate_rectangle_contacts
(
double
lx
,
double
ly
,
Cref_vector3
pos
,
Cref_vector3
rpy
,
Ref_matrix43
p
,
Ref_matrix43
N
);
std
::
string
getDateAndTimeAsString
();
...
...
@@ -139,8 +152,10 @@ std::string getDateAndTimeAsString();
value_type
nchoosek
(
const
int
n
,
const
int
k
);
template
<
typename
DerivedV
,
typename
DerivedU
>
void
doCombs
(
Eigen
::
Matrix
<
typename
DerivedU
::
Scalar
,
1
,
Eigen
::
Dynamic
>&
running
,
int
&
running_i
,
int
&
running_j
,
Eigen
::
PlainObjectBase
<
DerivedU
>&
U
,
const
Eigen
::
MatrixBase
<
DerivedV
>&
V
,
int
offset
,
int
k
)
{
void
doCombs
(
Eigen
::
Matrix
<
typename
DerivedU
::
Scalar
,
1
,
Eigen
::
Dynamic
>&
running
,
int
&
running_i
,
int
&
running_j
,
Eigen
::
PlainObjectBase
<
DerivedU
>&
U
,
const
Eigen
::
MatrixBase
<
DerivedV
>&
V
,
int
offset
,
int
k
)
{
int
N
=
(
int
)(
V
.
size
());
if
(
k
==
0
)
{
U
.
row
(
running_i
)
=
running
;
...
...
@@ -156,8 +171,9 @@ void doCombs(Eigen::Matrix<typename DerivedU::Scalar, 1, Eigen::Dynamic>& runnin
}
/**
* Computes a matrix C containing all possible combinations of the elements of vector v taken k at a time.
* Matrix C has k columns and n!/((n–k)! k!) rows, where n is length(v).
* Computes a matrix C containing all possible combinations of the elements of
* vector v taken k at a time. Matrix C has k columns and n!/((n–k)! k!) rows,
* where n is length(v).
* @param V n-long vector of elements
* @param k size of sub-set to consider
* @param U result matrix
...
...
@@ -165,7 +181,8 @@ void doCombs(Eigen::Matrix<typename DerivedU::Scalar, 1, Eigen::Dynamic>& runnin
* the first one, with all zeros.
*/
template
<
typename
DerivedV
,
typename
DerivedU
>
void
nchoosek
(
const
Eigen
::
MatrixBase
<
DerivedV
>&
V
,
const
int
k
,
Eigen
::
PlainObjectBase
<
DerivedU
>&
U
)
{
void
nchoosek
(
const
Eigen
::
MatrixBase
<
DerivedV
>&
V
,
const
int
k
,
Eigen
::
PlainObjectBase
<
DerivedU
>&
U
)
{
using
namespace
Eigen
;
if
(
V
.
size
()
==
0
)
{
U
.
resize
(
0
,
k
);
...
...
python/centroidal_dynamics_python.cpp
View file @
dff4d63d
#include
"hpp/centroidal-dynamics/centroidal_dynamics.hh"
#include
"hpp/centroidal-dynamics/util.hh"
#include
<eigenpy/memory.hpp>
#include
<boost/python.hpp>
#include
<eigenpy/eigenpy.hpp>
#include
<eigenpy/memory.hpp>
#include
<boost/python.hpp>
#include
"hpp/centroidal-dynamics/centroidal_dynamics.hh"
#include
"hpp/centroidal-dynamics/util.hh"
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION
(
centroidal_dynamics
::
Equilibrium
)
namespace
centroidal_dynamics
{
using
namespace
boost
::
python
;
boost
::
python
::
tuple
wrapComputeQuasiEquilibriumRobustness
(
Equilibrium
&
self
,
const
Vector3
&
com
)
{
boost
::
python
::
tuple
wrapComputeQuasiEquilibriumRobustness
(
Equilibrium
&
self
,
const
Vector3
&
com
)
{
double
robustness
;
LP_status
status
=
self
.
computeEquilibriumRobustness
(
com
,
robustness
);
return
boost
::
python
::
make_tuple
(
status
,
robustness
);
}
boost
::
python
::
tuple
wrapComputeEquilibriumRobustness
(
Equilibrium
&
self
,
const
Vector3
&
com
,
const
Vector3
&
acc
)
{
boost
::
python
::
tuple
wrapComputeEquilibriumRobustness
(
Equilibrium
&
self
,
const
Vector3
&
com
,
const
Vector3
&
acc
)
{
double
robustness
;
LP_status
status
=
self
.
computeEquilibriumRobustness
(
com
,
acc
,
robustness
);
return
boost
::
python
::
make_tuple
(
status
,
robustness
);
}
boost
::
python
::
tuple
wrapCheckRobustEquilibrium
(
Equilibrium
&
self
,
const
Vector3
&
com
)
{
boost
::
python
::
tuple
wrapCheckRobustEquilibrium
(
Equilibrium
&
self
,
const
Vector3
&
com
)
{
bool
robustness
;
LP_status
status
=
self
.
checkRobustEquilibrium
(
com
,
robustness
);
return
boost
::
python
::
make_tuple
(
status
,
robustness
);
}
bool
wrapSetNewContacts
(
Equilibrium
&
self
,
const
MatrixX3ColMajor
&
contactPoints
,
const
MatrixX3ColMajor
&
contactNormals
,
const
double
frictionCoefficient
,
bool
wrapSetNewContacts
(
Equilibrium
&
self
,
const
MatrixX3ColMajor
&
contactPoints
,
const
MatrixX3ColMajor
&
contactNormals
,
const
double
frictionCoefficient
,
const
EquilibriumAlgorithm
alg
)
{
return
self
.
setNewContacts
(
contactPoints
,
contactNormals
,
frictionCoefficient
,
alg
);
return
self
.
setNewContacts
(
contactPoints
,
contactNormals
,
frictionCoefficient
,
alg
);
}
bool
wrapSetNewContactsFull
(
Equilibrium
&
self
,
const
MatrixX3ColMajor
&
contactPoints
,
const
MatrixX3ColMajor
&
contactNormals
,
const
double
frictionCoefficient
,
bool
wrapSetNewContactsFull
(
Equilibrium
&
self
,
const
MatrixX3ColMajor
&
contactPoints
,
const
MatrixX3ColMajor
&
contactNormals
,
const
double
frictionCoefficient
,
const
EquilibriumAlgorithm
alg
)
{
return
self
.
setNewContacts
(
contactPoints
,
contactNormals
,
frictionCoefficient
,
alg
);
return
self
.
setNewContacts
(
contactPoints
,
contactNormals
,
frictionCoefficient
,
alg
);
}
boost
::
python
::
tuple
wrapGetPolytopeInequalities
(
Equilibrium
&
self
)
{
...
...
@@ -71,7 +80,9 @@ BOOST_PYTHON_MODULE(hpp_centroidal_dynamics) {
.
value
(
"SOLVER_LP_CLP"
,
SOLVER_LP_CLP
)
.
export_values
();
#else
enum_
<
SolverLP
>
(
"SolverLP"
).
value
(
"SOLVER_LP_QPOASES"
,
SOLVER_LP_QPOASES
).
export_values
();
enum_
<
SolverLP
>
(
"SolverLP"
)
.
value
(
"SOLVER_LP_QPOASES"
,
SOLVER_LP_QPOASES
)
.
export_values
();
#endif
enum_
<
EquilibriumAlgorithm
>
(
"EquilibriumAlgorithm"
)
...
...
@@ -95,19 +106,22 @@ BOOST_PYTHON_MODULE(hpp_centroidal_dynamics) {
/** END enum types **/
// bool (Equilibrium::*setNewContacts)
// (const MatrixX3ColMajor&, const MatrixX3ColMajor&, const double, const EquilibriumAlgorithm, const int
// graspIndex, const double maxGraspForce) = &Equilibrium::setNewContacts;
// (const MatrixX3ColMajor&, const MatrixX3ColMajor&, const double,
// const EquilibriumAlgorithm, const int graspIndex, const double
// maxGraspForce) = &Equilibrium::setNewContacts;
class_
<
Equilibrium
>
(
"Equilibrium"
,
init
<
std
::
string
,
double
,
unsigned
int
,
optional
<
SolverLP
,
bool
,
const
unsigned
int
,
const
bool
>
>
())
init
<
std
::
string
,
double
,
unsigned
int
,
optional
<
SolverLP
,
bool
,
const
unsigned
int
,
const
bool
>
>
())
.
def
(
"useWarmStart"
,
&
Equilibrium
::
useWarmStart
)
.
def
(
"setUseWarmStart"
,
&
Equilibrium
::
setUseWarmStart
)
.
def
(
"getName"
,
&
Equilibrium
::
getName
)
.
def
(
"getAlgorithm"
,
&
Equilibrium
::
getAlgorithm
)
.
def
(
"setNewContacts"
,
wrapSetNewContacts
)
.
def
(
"setNewContacts"
,
wrapSetNewContactsFull
)
.
def
(
"computeEquilibriumRobustness"
,
wrapComputeQuasiEquilibriumRobustness
)
.
def
(
"computeEquilibriumRobustness"
,
wrapComputeQuasiEquilibriumRobustness
)
.
def
(
"computeEquilibriumRobustness"
,
wrapComputeEquilibriumRobustness
)
.
def
(
"checkRobustEquilibrium"
,
wrapCheckRobustEquilibrium
)
.
def
(
"getPolytopeInequalities"
,
wrapGetPolytopeInequalities
);
...
...
python/test/binding_tests.py
View file @
dff4d63d
...
...
@@ -2,18 +2,23 @@ import unittest
from
numpy
import
array
,
asmatrix
,
cross
from
hpp_centroidal_dynamics
import
LP_STATUS_OPTIMAL
,
Equilibrium
,
EquilibriumAlgorithm
,
SolverLP
from
hpp_centroidal_dynamics
import
(
LP_STATUS_OPTIMAL
,
Equilibrium
,
EquilibriumAlgorithm
,
SolverLP
,
)
class
TestCentroidalDynamics
(
unittest
.
TestCase
):
def
test_all
(
self
):
# testing constructors
eq
=
Equilibrium
(
"test"
,
54.
,
4
)
eq
=
Equilibrium
(
"test"
,
54.
,
4
,
SolverLP
.
SOLVER_LP_QPOASES
)
eq
=
Equilibrium
(
"test"
,
54.
,
4
,
SolverLP
.
SOLVER_LP_QPOASES
)
eq
=
Equilibrium
(
"test"
,
54.
,
4
,
SolverLP
.
SOLVER_LP_QPOASES
,
False
)
eq
=
Equilibrium
(
"test"
,
54.
,
4
,
SolverLP
.
SOLVER_LP_QPOASES
,
False
,
1
)
eq
=
Equilibrium
(
"test"
,
54.
,
4
,
SolverLP
.
SOLVER_LP_QPOASES
,
True
,
1
,
True
)
eq
=
Equilibrium
(
"test"
,
54.
0
,
4
)
eq
=
Equilibrium
(
"test"
,
54.
0
,
4
,
SolverLP
.
SOLVER_LP_QPOASES
)
eq
=
Equilibrium
(
"test"
,
54.
0
,
4
,
SolverLP
.
SOLVER_LP_QPOASES
)
eq
=
Equilibrium
(
"test"
,
54.
0
,
4
,
SolverLP
.
SOLVER_LP_QPOASES
,
False
)
eq
=
Equilibrium
(
"test"
,
54.
0
,
4
,
SolverLP
.
SOLVER_LP_QPOASES
,
False
,
1
)
eq
=
Equilibrium
(
"test"
,
54.
0
,
4
,
SolverLP
.
SOLVER_LP_QPOASES
,
True
,
1
,
True
)
# whether useWarmStart is enable (True by default)
previous
=
eq
.
useWarmStart
()
...
...
@@ -24,52 +29,64 @@ class TestCentroidalDynamics(unittest.TestCase):
# access solver name
self
.
assertEqual
(
eq
.
getName
(),
"test"
)
z
=
array
([
0.
,
0.
,
1.
])
P
=
asmatrix
(
array
([
array
([
x
,
y
,
0
])
for
x
in
[
-
0.05
,
0.05
]
for
y
in
[
-
0.1
,
0.1
]]))
z
=
array
([
0.0
,
0.0
,
1.0
])
P
=
asmatrix
(
array
([
array
([
x
,
y
,
0
])
for
x
in
[
-
0.05
,
0.05
]
for
y
in
[
-
0.1
,
0.1
]])
)
N
=
asmatrix
(
array
([
z
for
_
in
range
(
4
)]))
# setting contact positions and normals, as well as friction coefficients
eq
.
setNewContacts
(
asmatrix
(
P
),
asmatrix
(
N
),
0.3
,
EquilibriumAlgorithm
.
EQUILIBRIUM_ALGORITHM_LP
)
eq
.
setNewContacts
(
asmatrix
(
P
),
asmatrix
(
N
),
0.3
,
EquilibriumAlgorithm
.
EQUILIBRIUM_ALGORITHM_LP
)
c
=
asmatrix
(
array
([
0.
,
0.
,
1.
])).
T
c
=
asmatrix
(
array
([
0.
0
,
0.
0
,
1.
0
])).
T
# computing robustness of a given configuration, first with no argument (0 acceleration, static equilibrium)
# computing robustness of a given configuration,
# first with no argument (0 acceleration, static equilibrium)
status
,
robustness
=
eq
.
computeEquilibriumRobustness
(
c
)
self
.
assertEqual
(
status
,
LP_STATUS_OPTIMAL
,
"LP should not fail"
)
self
.
assertGreater
(
robustness
,
0
,
"first test should be in equilibrirum"
)
# computing robustness of a given configuration with non zero acceleration
ddc
=
asmatrix
(
array
([
1000.
,
0.
,
0.
])).
T
ddc
=
asmatrix
(
array
([
1000.
0
,
0.
0
,
0.
0
])).
T
status
,
robustness
=
eq
.
computeEquilibriumRobustness
(
c
,
ddc
)
self
.
assertEqual
(
status
,
LP_STATUS_OPTIMAL
,
"LP should not fail"
)
self
.
assertLess
(
robustness
,
0
,
"first test should NOT be in equilibrirum"
)
# now, use polytope projection algorithm
eq
.
setNewContacts
(
asmatrix
(
P
),
asmatrix
(
N
),
0.3
,
EquilibriumAlgorithm
.
EQUILIBRIUM_ALGORITHM_PP
)
eq
.
setNewContacts
(
asmatrix
(
P
),
asmatrix
(
N
),
0.3
,
EquilibriumAlgorithm
.
EQUILIBRIUM_ALGORITHM_PP
)
H
,
h
=
eq
.
getPolytopeInequalities
()
# c= asmatrix(array([0.,0.,1.])).T
status
,
stable
=
eq
.
checkRobustEquilibrium
(
c
)
# TODO: This works well unless we add a --coverage in CXXFLAGS
# self.assertEqual(status, LP_STATUS_OPTIMAL, "checkRobustEquilibrium should not fail")
# self.assertEqual(status, LP_STATUS_OPTIMAL,
# "checkRobustEquilibrium should not fail")
self
.
assertTrue
(
stable
,
"lat test should be in equilibrirum"
)
def
compute_w
(
c
,
ddc
,
dL
=
array
([
0.
,
0.
,
0.
]),
m
=
54.
,
g_vec
=
array
([
0.
,
0.
,
-
9.81
])):
def
compute_w
(
c
,
ddc
,
dL
=
array
([
0.0
,
0.0
,
0.0
]),
m
=
54.0
,
g_vec
=
array
([
0.0
,
0.0
,
-
9.81
])
):
w1
=
m
*
(
ddc
-
g_vec
)
return
array
(
w1
.
tolist
()
+
(
cross
(
c
,
w1
)
+
dL
).
tolist
())