Skip to content
GitLab
Explore
Sign in
Register
Primary navigation
Search or go to…
Project
H
hpp-centroidal-dynamics
Manage
Activity
Members
Labels
Plan
Issues
Issue boards
Milestones
Wiki
Code
Merge requests
Repository
Branches
Commits
Tags
Repository graph
Compare revisions
Snippets
Build
Pipelines
Jobs
Pipeline schedules
Artifacts
Deploy
Releases
Container Registry
Model registry
Operate
Environments
Monitor
Incidents
Analyze
Value stream analytics
Contributor analytics
CI/CD analytics
Repository analytics
Model experiments
Help
Help
Support
GitLab documentation
Compare GitLab plans
Community forum
Contribute to GitLab
Provide feedback
Keyboard shortcuts
?
Snippets
Groups
Projects
Show more breadcrumbs
Humanoid Path Planner
hpp-centroidal-dynamics
Commits
d73eefad
Commit
d73eefad
authored
7 years ago
by
Steve Tonneau
Committed by
Pierre Fernbach
6 years ago
Browse files
Options
Downloads
Patches
Plain Diff
implemented binomial
parent
d6d7d100
No related branches found
No related tags found
No related merge requests found
Changes
3
Hide whitespace changes
Inline
Side-by-side
Showing
3 changed files
include/centroidal-dynamics-lib/util.hh
+59
-0
59 additions, 0 deletions
include/centroidal-dynamics-lib/util.hh
src/centroidal_dynamics.cpp
+31
-2
31 additions, 2 deletions
src/centroidal_dynamics.cpp
src/util.cpp
+45
-0
45 additions, 0 deletions
src/util.cpp
with
135 additions
and
2 deletions
include/centroidal-dynamics-lib/util.hh
+
59
−
0
View file @
d73eefad
...
...
@@ -7,6 +7,8 @@
#include
<iostream>
#include
<fstream>
#include
<cmath>
#include
<cassert>
#include
<Eigen/Dense>
#include
<Eigen/src/Core/util/Macros.h>
...
...
@@ -136,6 +138,63 @@ namespace centroidal_dynamics
std
::
string
getDateAndTimeAsString
();
/**
* Computes a binomal coefficient
* @return n!/((n–k)! k!).
*/
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
)
{
int
N
=
(
int
)(
V
.
size
());
if
(
k
==
0
)
{
U
.
row
(
running_i
)
=
running
;
running_i
++
;
return
;
}
for
(
int
i
=
offset
;
i
<=
N
-
k
;
++
i
)
{
running
(
running_j
)
=
V
(
i
);
running_j
++
;
doCombs
(
running
,
running_i
,
running_j
,
U
,
V
,
i
+
1
,
k
-
1
);
running_j
--
;
}
}
/**
* 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
* @return nchoosek by k long matrix where each row is a unique k-size
* 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
)
{
using
namespace
Eigen
;
if
(
V
.
size
()
==
0
)
{
U
.
resize
(
0
,
k
);
return
;
}
assert
((
V
.
cols
()
==
1
||
V
.
rows
()
==
1
)
&&
"V must be a vector"
);
U
.
resize
(
nchoosek
((
int
)(
V
.
size
()),
k
),
k
);
int
running_i
=
0
;
int
running_j
=
0
;
Matrix
<
typename
DerivedU
::
Scalar
,
1
,
Dynamic
>
running
(
1
,
k
);
/* const std::function<void(int,int)> doCombs =
[&running,&N,&doCombs,&running_i,&running_j,&U,&V](int offset, int k)*/
;
doCombs
(
running
,
running_i
,
running_j
,
U
,
V
,
0
,
k
);
}
}
//namespace centroidal_dynamics
#endif //_CENTROIDAL_DYNAMICS_LIB_UTIL_HH
This diff is collapsed.
Click to expand it.
src/centroidal_dynamics.cpp
+
31
−
2
View file @
d73eefad
...
...
@@ -521,12 +521,41 @@ LP_status Equilibrium::findExtremumInDirection(Cref_vector3 direction, Ref_vecto
SEND_ERROR_MSG
(
"findExtremumInDirection not implemented yet"
);
return
LP_STATUS_ERROR
;
}
///
/// \brief Computes factorial of a number
///
int
fact
(
const
int
n
)
{
assert
(
n
>=
0
);
int
res
=
1
;
for
(
int
i
=
2
;
i
<=
n
;
++
i
)
res
*=
i
;
return
res
;
}
///
/// \brief Computes a binomal coefficient
///
int
choosenk
(
const
int
n
,
const
int
k
)
{
return
fact
(
n
)
/
(
fact
(
k
)
*
fact
(
n
-
k
));
}
bool
Equilibrium
::
computePolytopeProjection
(
Cref_matrix6X
v
)
{
int
n
=
(
int
)(
v
.
rows
());
int
m
=
(
int
)(
v
.
cols
());
// todo: for the moment using ad hoc upper bound = 500 N
int
n
=
(
int
)
v
.
rows
();
int
m
=
(
int
)
v
.
cols
();
if
(
n
>
m
)
{
SEND_ERROR_MSG
(
"V has more lines that columns, this case is not handled!"
);
return
false
;
}
MatrixXX
test
=
v
;
VectorX
vec
;
nchoosek
(
vec
,
6
,
test
);
//int I = I=nchoosek(1:m,n-1);*/
// getProfiler().start("eigen_to_cdd");
dd_MatrixPtr
V
=
cone_span_eigen_to_cdd
(
v
.
transpose
(),
canonicalize_cdd_matrix
);
// getProfiler().stop("eigen_to_cdd");
...
...
This diff is collapsed.
Click to expand it.
src/util.cpp
+
45
−
0
View file @
d73eefad
...
...
@@ -165,6 +165,51 @@ std::string getDateAndTimeAsString()
strftime
(
buffer
,
80
,
"%Y%m%d_%I%M%S"
,
timeinfo
);
return
std
::
string
(
buffer
);
}
/*
int fact(const int n)
{
assert(n>=0);
int res = 1;
for (int i=2 ; i <= n ; ++i)
res *= i;
return res;
}
int choosenk(const int n, const int k)
{
return fact(n) / (fact(k) * fact(n - k));
}*/
/* is this faster ?
value_type choosenk(const int n, const int k)
{
if(k>n/2)
return nchoosek(n,n-k);
else if(k==1)
return n;
else
{
double c = 1;
for(int i = 1;i<=k;i++)
c *= (((double)n-k+i)/((double)i));
return std::round(c);
}
}*/
value_type
nchoosek
(
const
int
n
,
const
int
k
)
{
if
(
k
>
n
/
2
)
return
nchoosek
(
n
,
n
-
k
);
else
if
(
k
==
1
)
return
n
;
else
{
value_type
c
=
1
;
for
(
int
i
=
1
;
i
<=
k
;
i
++
)
c
*=
(((
value_type
)
n
-
k
+
i
)
/
((
value_type
)
i
));
return
round
(
c
);
}
}
}
//namespace centroidal_dynamics
...
...
This diff is collapsed.
Click to expand it.
Preview
0%
Loading
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!
Save comment
Cancel
Please
register
or
sign in
to comment