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
Guilhem Saurel
ndcurves
Commits
c2ed8387
Commit
c2ed8387
authored
Feb 07, 2019
by
stevet
Browse files
python bindings and tests
parent
3dec2d43
Changes
13
Hide whitespace changes
Inline
Side-by-side
.gitignore
View file @
c2ed8387
...
...
@@ -5,3 +5,4 @@ build-rel/
# temp files
.*~
*.user
*.pyc
python/CMakeLists.txt
View file @
c2ed8387
...
...
@@ -3,10 +3,10 @@ STRING(REGEX REPLACE "-" "_" PY_NAME ${PROJECT_NAME})
ADD_REQUIRED_DEPENDENCY
(
"eigenpy"
)
# Define the wrapper library that wraps our library
add_library
(
${
PY_NAME
}
SHARED spline_python.cpp
)
add_library
(
${
PY_NAME
}
SHARED spline_python.cpp
python_variables.cpp python_variables.h
)
#~ target_link_libraries( centroidal_dynamics ${Boost_LIBRARIES} ${PROJECT_NAME} )
# don't prepend wrapper library name with lib
set_target_properties
(
${
PY_NAME
}
PROPERTIES PREFIX
""
)
set_target_properties
(
${
PY_NAME
}
PROPERTIES PREFIX
""
)
IF
(
APPLE
)
# We need to change the extension for python bindings
...
...
python/python_definitions.h
0 → 100644
View file @
c2ed8387
#include
"hpp/spline/bezier_curve.h"
#include
"hpp/spline/linear_variable.h"
#include
<vector>
#ifndef _DEFINITION_PYTHON_BINDINGS
#define _DEFINITION_PYTHON_BINDINGS
namespace
spline
{
typedef
double
real
;
typedef
Eigen
::
Vector3d
point_t
;
typedef
Eigen
::
VectorXd
vectorX_t
;
typedef
Eigen
::
Matrix
<
double
,
6
,
1
,
0
,
6
,
1
>
point6_t
;
typedef
Eigen
::
Matrix
<
double
,
3
,
1
,
0
,
3
,
1
>
ret_point_t
;
typedef
Eigen
::
Matrix
<
double
,
6
,
1
,
0
,
6
,
1
>
ret_point6_t
;
typedef
Eigen
::
VectorXd
time_waypoints_t
;
typedef
Eigen
::
Matrix
<
real
,
3
,
Eigen
::
Dynamic
>
point_list_t
;
typedef
Eigen
::
Matrix
<
real
,
6
,
Eigen
::
Dynamic
>
point_list6_t
;
typedef
std
::
vector
<
point_t
,
Eigen
::
aligned_allocator
<
point_t
>
>
t_point_t
;
typedef
std
::
vector
<
point6_t
,
Eigen
::
aligned_allocator
<
point6_t
>
>
t_point6_t
;
typedef
std
::
pair
<
real
,
point_t
>
Waypoint
;
typedef
std
::
vector
<
Waypoint
>
T_Waypoint
;
typedef
std
::
pair
<
real
,
point6_t
>
Waypoint6
;
typedef
std
::
vector
<
Waypoint6
>
T_Waypoint6
;
template
<
typename
PointList
,
typename
T_Point
>
T_Point
vectorFromEigenArray
(
const
PointList
&
array
)
{
T_Point
res
;
for
(
int
i
=
0
;
i
<
array
.
cols
();
++
i
)
res
.
push_back
(
array
.
col
(
i
));
return
res
;
}
}
//namespace spline.
#endif //_DEFINITION_PYTHON_BINDINGS
python/python_variables.cpp
0 → 100644
View file @
c2ed8387
#include
"python_variables.h"
#include
"python_definitions.h"
#include
<Eigen/Core>
namespace
spline
{
std
::
vector
<
linear_variable_3_t
>
matrix3DFromEigenArray
(
const
point_list_t
&
matrices
,
const
point_list_t
&
vectors
)
{
assert
(
vectors
.
cols
()
*
3
==
matrices
.
cols
()
)
;
std
::
vector
<
linear_variable_3_t
>
res
;
for
(
int
i
=
0
;
i
<
vectors
.
cols
();
++
i
)
res
.
push_back
(
linear_variable_3_t
(
matrices
.
block
<
3
,
3
>
(
0
,
i
*
3
),
vectors
.
col
(
i
)));
return
res
;
}
variables_3_t
fillWithZeros
(
const
linear_variable_3_t
&
var
,
const
std
::
size_t
totalvar
,
const
std
::
size_t
i
)
{
variables_3_t
res
;
std
::
vector
<
linear_variable_3_t
>&
vars
=
res
.
variables_
;
for
(
std
::
size_t
idx
=
0
;
idx
<
i
;
++
idx
)
vars
.
push_back
(
linear_variable_3_t
::
Zero
());
vars
.
push_back
(
var
);
for
(
std
::
size_t
idx
=
i
+
1
;
idx
<
totalvar
;
++
idx
)
vars
.
push_back
(
linear_variable_3_t
::
Zero
());
return
res
;
}
std
::
vector
<
variables_3_t
>
computeLinearControlPoints
(
const
point_list_t
&
matrices
,
const
point_list_t
&
vectors
)
{
std
::
vector
<
variables_3_t
>
res
;
std
::
vector
<
linear_variable_3_t
>
variables
=
matrix3DFromEigenArray
(
matrices
,
vectors
);
// now need to fill all this with zeros...
std
::
size_t
totalvar
=
variables
.
size
();
for
(
std
::
size_t
i
=
0
;
i
<
totalvar
;
++
i
)
res
.
push_back
(
fillWithZeros
(
variables
[
i
],
totalvar
,
i
));
return
res
;
}
/*linear variable control points*/
bezier_linear_variable_t
*
wrapBezierLinearConstructor
(
const
point_list_t
&
matrices
,
const
point_list_t
&
vectors
)
{
std
::
vector
<
variables_3_t
>
asVector
=
computeLinearControlPoints
(
matrices
,
vectors
);
return
new
bezier_linear_variable_t
(
asVector
.
begin
(),
asVector
.
end
(),
1.
)
;
}
bezier_linear_variable_t
*
wrapBezierLinearConstructorBounds
(
const
point_list_t
&
matrices
,
const
point_list_t
&
vectors
,
const
real
ub
)
{
std
::
vector
<
variables_3_t
>
asVector
=
computeLinearControlPoints
(
matrices
,
vectors
);
return
new
bezier_linear_variable_t
(
asVector
.
begin
(),
asVector
.
end
(),
ub
)
;
}
LinearControlPointsHolder
*
wayPointsToLists
(
const
bezier_linear_variable_t
&
self
)
{
typedef
typename
bezier_linear_variable_t
::
t_point_t
t_point
;
typedef
typename
bezier_linear_variable_t
::
t_point_t
::
const_iterator
cit_point
;
const
t_point
&
wps
=
self
.
waypoints
();
// retrieve num variables.
std
::
size_t
dim
=
wps
[
0
].
variables_
.
size
()
*
3
;
Eigen
::
Matrix
<
real
,
Eigen
::
Dynamic
,
Eigen
::
Dynamic
>
matrices
(
dim
,
wps
.
size
()
*
3
);
Eigen
::
Matrix
<
real
,
Eigen
::
Dynamic
,
Eigen
::
Dynamic
>
vectors
(
dim
,
wps
.
size
());
int
col
=
0
;
for
(
cit_point
cit
=
wps
.
begin
();
cit
!=
wps
.
end
();
++
cit
,
++
col
)
{
const
std
::
vector
<
linear_variable_3_t
>&
variables
=
cit
->
variables_
;
int
i
=
0
;
for
(
std
::
vector
<
linear_variable_3_t
>::
const_iterator
varit
=
variables
.
begin
();
varit
!=
variables
.
end
();
++
varit
,
i
+=
3
)
{
vectors
.
block
<
3
,
1
>
(
i
,
col
)
=
varit
->
b_
;
matrices
.
block
<
3
,
3
>
(
i
,
col
*
3
)
=
varit
->
A_
;
}
}
LinearControlPointsHolder
*
res
(
new
LinearControlPointsHolder
);
res
->
res
=
std
::
make_pair
(
matrices
,
vectors
);
return
res
;
}
// does not include end time
LinearBezierVector
*
split
(
const
bezier_linear_variable_t
&
self
,
const
vectorX_t
&
times
)
{
LinearBezierVector
*
res
(
new
LinearBezierVector
);
bezier_linear_variable_t
current
=
self
;
real
current_time
=
0.
;
real
tmp
;
for
(
int
i
=
0
;
i
<
times
.
rows
();
++
i
)
{
tmp
=
times
[
i
];
std
::
pair
<
bezier_linear_variable_t
,
bezier_linear_variable_t
>
pairsplit
=
current
.
split
(
tmp
-
current_time
);
res
->
beziers_
.
push_back
(
pairsplit
.
first
);
current
=
pairsplit
.
second
;
current_time
+=
tmp
-
current_time
;
}
res
->
beziers_
.
push_back
(
current
);
return
res
;
}
}
// namespace spline
python/python_variables.h
0 → 100644
View file @
c2ed8387
#include
"hpp/spline/bezier_curve.h"
#include
"hpp/spline/linear_variable.h"
#include
"python_definitions.h"
#include
<vector>
#ifndef _VARIABLES_PYTHON_BINDINGS
#define _VARIABLES_PYTHON_BINDINGS
namespace
spline
{
static
const
int
dim
=
3
;
typedef
spline
::
linear_variable
<
dim
,
real
>
linear_variable_3_t
;
typedef
spline
::
variables
<
linear_variable_3_t
>
variables_3_t
;
typedef
spline
::
bezier_curve
<
real
,
real
,
dim
,
true
,
variables_3_t
>
bezier_linear_variable_t
;
/*linear variable control points*/
bezier_linear_variable_t
*
wrapBezierLinearConstructor
(
const
point_list_t
&
matrices
,
const
point_list_t
&
vectors
);
bezier_linear_variable_t
*
wrapBezierLinearConstructorBounds
(
const
point_list_t
&
matrices
,
const
point_list_t
&
vectors
,
const
real
ub
);
typedef
std
::
pair
<
Eigen
::
Matrix
<
real
,
Eigen
::
Dynamic
,
Eigen
::
Dynamic
>
,
Eigen
::
Matrix
<
real
,
Eigen
::
Dynamic
,
Eigen
::
Dynamic
>
>
linear_points_t
;
struct
LinearControlPointsHolder
{
linear_points_t
res
;
Eigen
::
Matrix
<
real
,
Eigen
::
Dynamic
,
Eigen
::
Dynamic
>
A
()
{
return
res
.
first
;}
Eigen
::
Matrix
<
real
,
Eigen
::
Dynamic
,
Eigen
::
Dynamic
>
b
()
{
return
res
.
second
;}
};
LinearControlPointsHolder
*
wayPointsToLists
(
const
bezier_linear_variable_t
&
self
);
struct
LinearBezierVector
{
std
::
vector
<
bezier_linear_variable_t
>
beziers_
;
std
::
size_t
size
()
{
return
beziers_
.
size
();}
bezier_linear_variable_t
*
at
(
std
::
size_t
i
)
{
assert
(
i
<
size
());
return
new
bezier_linear_variable_t
(
beziers_
[
i
]);
}
};
// does not include end time
LinearBezierVector
*
split
(
const
bezier_linear_variable_t
&
self
,
const
vectorX_t
&
times
);
}
//namespace spline.
#endif //_VARIABLES_PYTHON_BINDINGS
python/spline_python.cpp
View file @
c2ed8387
...
...
@@ -6,6 +6,8 @@
#include
"hpp/spline/bezier_polynom_conversion.h"
#include
"hpp/spline/bernstein.h"
#include
"python_definitions.h"
#include
"python_variables.h"
#include
<vector>
...
...
@@ -15,20 +17,7 @@
#include
<boost/python.hpp>
/*** TEMPLATE SPECIALIZATION FOR PYTHON ****/
typedef
double
real
;
typedef
Eigen
::
Vector3d
point_t
;
typedef
Eigen
::
Matrix
<
double
,
6
,
1
,
0
,
6
,
1
>
point6_t
;
typedef
Eigen
::
Matrix
<
double
,
3
,
1
,
0
,
3
,
1
>
ret_point_t
;
typedef
Eigen
::
Matrix
<
double
,
6
,
1
,
0
,
6
,
1
>
ret_point6_t
;
typedef
Eigen
::
VectorXd
time_waypoints_t
;
typedef
Eigen
::
Matrix
<
real
,
3
,
Eigen
::
Dynamic
>
point_list_t
;
typedef
Eigen
::
Matrix
<
real
,
6
,
Eigen
::
Dynamic
>
point_list6_t
;
typedef
std
::
vector
<
point_t
,
Eigen
::
aligned_allocator
<
point_t
>
>
t_point_t
;
typedef
std
::
vector
<
point6_t
,
Eigen
::
aligned_allocator
<
point6_t
>
>
t_point6_t
;
typedef
std
::
pair
<
real
,
point_t
>
Waypoint
;
typedef
std
::
vector
<
Waypoint
>
T_Waypoint
;
typedef
std
::
pair
<
real
,
point6_t
>
Waypoint6
;
typedef
std
::
vector
<
Waypoint6
>
T_Waypoint6
;
using
namespace
spline
;
typedef
spline
::
bezier_curve
<
real
,
real
,
3
,
true
,
point_t
>
bezier_t
;
typedef
spline
::
bezier_curve
<
real
,
real
,
6
,
true
,
point6_t
>
bezier6_t
;
...
...
@@ -57,14 +46,6 @@ EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(spline_deriv_constraint_t)
namespace
spline
{
using
namespace
boost
::
python
;
template
<
typename
PointList
,
typename
T_Point
>
T_Point
vectorFromEigenArray
(
const
PointList
&
array
)
{
T_Point
res
;
for
(
int
i
=
0
;
i
<
array
.
cols
();
++
i
)
res
.
push_back
(
array
.
col
(
i
));
return
res
;
}
template
<
typename
Bezier
,
typename
PointList
,
typename
T_Point
>
Bezier
*
wrapBezierConstructorTemplate
(
const
PointList
&
array
,
const
real
ub
=
1.
)
...
...
@@ -260,6 +241,37 @@ BOOST_PYTHON_MODULE(hpp_spline)
/** END bezier curve**/
/** BEGIN variable points bezier curve**/
class_
<
LinearControlPointsHolder
>
(
"LinearWaypoint"
,
no_init
)
.
def_readonly
(
"A"
,
&
LinearControlPointsHolder
::
A
)
.
def_readonly
(
"b"
,
&
LinearControlPointsHolder
::
b
)
;
class_
<
LinearBezierVector
>
(
"bezierVarVector"
,
no_init
)
.
def_readonly
(
"size"
,
&
LinearBezierVector
::
size
)
.
def
(
"at"
,
&
LinearBezierVector
::
at
,
return_value_policy
<
manage_new_object
>
())
;
class_
<
bezier_linear_variable_t
>
(
"bezierVar"
,
no_init
)
.
def
(
"__init__"
,
make_constructor
(
&
wrapBezierLinearConstructor
))
.
def
(
"__init__"
,
make_constructor
(
&
wrapBezierLinearConstructorBounds
))
.
def
(
"min"
,
&
bezier_linear_variable_t
::
min
)
.
def
(
"max"
,
&
bezier_linear_variable_t
::
max
)
//.def("__call__", &bezier_linear_control_t::operator())
.
def
(
"derivate"
,
&
bezier_linear_variable_t
::
derivate
)
.
def
(
"compute_derivate"
,
&
bezier_linear_variable_t
::
compute_derivate
)
.
def
(
"compute_primitive"
,
&
bezier_linear_variable_t
::
compute_primitive
)
.
def
(
"split"
,
&
split
,
return_value_policy
<
manage_new_object
>
())
.
def
(
"waypoints"
,
&
wayPointsToLists
,
return_value_policy
<
manage_new_object
>
())
.
def_readonly
(
"degree"
,
&
bezier_linear_variable_t
::
degree_
)
.
def_readonly
(
"nbWaypoints"
,
&
bezier_linear_variable_t
::
size_
)
;
/** END variable points bezier curve**/
/** BEGIN spline curve function**/
class_
<
polynom_t
>
(
"polynom"
,
init
<
const
polynom_t
::
coeff_t
,
const
real
,
const
real
>
())
.
def
(
"__init__"
,
make_constructor
(
&
wrapSplineConstructor
))
...
...
python/test/variable/qp_traj/convex_hull.py
0 → 100644
View file @
c2ed8387
import
quadprog
from
numpy
import
array
,
dot
,
vstack
,
hstack
,
asmatrix
,
identity
,
cross
from
numpy.linalg
import
norm
from
scipy.spatial
import
ConvexHull
import
numpy
as
np
def
genConvexHullLines
(
points
):
hull
=
ConvexHull
(
points
)
lineList
=
[
points
[
el
]
for
el
in
hull
.
vertices
]
+
[
points
[
hull
.
vertices
[
0
]]]
lineList
=
[
array
(
el
[:
2
].
tolist
()
+
[
0.
])
for
el
in
lineList
]
return
[[
lineList
[
i
],
lineList
[
i
+
1
]]
for
i
in
range
(
len
(
hull
.
vertices
))],
lineList
[:
-
1
]
#now compute lines
def
getLineFromSegment
(
line
):
a
=
line
[
0
];
b
=
line
[
1
];
c
=
a
.
copy
()
;
c
[
2
]
=
1.
normal
=
cross
((
b
-
a
),(
c
-
a
))
normal
/=
norm
(
normal
)
# get inequality
dire
=
b
-
a
coeff
=
normal
rhs
=
a
.
dot
(
normal
)
return
(
coeff
,
array
([
rhs
]))
import
numpy
as
np
import
matplotlib.pyplot
as
plt
#generate right of the line
def
genFromLine
(
line
,
num_points
,
ranges
,
existing_points
=
[]):
coeff
,
rhs
=
getLineFromSegment
(
line
)
num_gen
=
0
gen
=
existing_points
+
[
line
[
0
][:
2
],
line
[
1
][:
2
]]
while
(
len
(
gen
)
<
num_points
):
pt
=
array
([
np
.
random
.
uniform
(
ranges
[
0
][
0
],
ranges
[
0
][
1
]),
np
.
random
.
uniform
(
ranges
[
1
][
0
],
ranges
[
1
][
1
])])
if
coeff
[:
2
].
dot
(
pt
)
<=
rhs
:
gen
+=
[
pt
]
return
genConvexHullLines
(
gen
)
if
__name__
==
'__main__'
:
genFromLine
([
array
([
0.5
,
0.
,
0.
]),
array
([
0.5
,
0.5
,
0.
])],
5
)
genFromLine
([
array
([
0.5
,
0.
,
0.
]),
array
([
0.5
,
-
0.5
,
0.
])],
5
)
python/test/variable/qp_traj/plot_cord.py
0 → 100644
View file @
c2ed8387
import
matplotlib.pyplot
as
plt
import
numpy
as
np
def
plotBezier
(
bez
,
color
):
step
=
100.
points1
=
np
.
array
([(
bez
(
i
/
step
*
bez
.
max
())[
0
][
0
],
bez
(
i
/
step
*
bez
.
max
())[
1
][
0
])
for
i
in
range
(
int
(
step
))])
x
=
points1
[:,
0
]
y
=
points1
[:,
1
]
plt
.
plot
(
x
,
y
,
color
,
linewidth
=
2.0
)
def
plotControlPoints
(
bez
,
color
):
wps
=
bez
.
waypoints
()
wps
=
np
.
array
([
wps
[:
2
,
i
]
for
i
in
range
(
wps
.
shape
[
1
])
])
x
=
wps
[:,
0
]
y
=
wps
[:,
1
]
plt
.
scatter
(
x
,
y
,
color
=
color
)
def
plotPoly
(
lines
,
color
):
step
=
1000.
for
line
in
lines
:
a_0
=
line
[
0
]
b_0
=
line
[
1
]
pointsline
=
np
.
array
([
a_0
*
i
/
step
+
b_0
*
(
1.
-
i
/
step
)
for
i
in
range
(
int
(
step
))])
xl
=
pointsline
[:,
0
]
yl
=
pointsline
[:,
1
]
plt
.
plot
(
xl
,
yl
,
color
,
linewidth
=
0.5
)
python/test/variable/qp_traj/qp.py
0 → 100644
View file @
c2ed8387
import
quadprog
from
numpy
import
array
,
dot
,
vstack
,
hstack
,
asmatrix
,
identity
#min (1/2)x' P x + q' x
#subject to G x <= h
#subject to C x = d
def
quadprog_solve_qp
(
P
,
q
,
G
=
None
,
h
=
None
,
C
=
None
,
d
=
None
):
qp_G
=
.
5
*
(
P
+
P
.
T
)
# make sure P is symmetric
qp_a
=
-
q
if
C
is
not
None
:
if
G
is
not
None
:
qp_C
=
-
vstack
([
C
,
G
]).
T
qp_b
=
-
hstack
([
d
,
h
])
else
:
qp_C
=
-
C
.
transpose
()
qp_b
=
-
d
meq
=
C
.
shape
[
0
]
else
:
# no equality constraint
qp_C
=
-
G
.
T
qp_b
=
-
h
meq
=
0
return
quadprog
.
solve_qp
(
qp_G
,
qp_a
,
qp_C
,
qp_b
,
meq
)[
0
]
#min ||Ax-b||**2
#subject to G x <= h
#subject to C x = d
def
solve_least_square
(
A
,
b
,
G
=
None
,
h
=
None
,
C
=
None
,
d
=
None
):
P
=
dot
(
A
.
T
,
A
)
#~ q = 2*dot(b, A).reshape(b.shape[0])
q
=
2
*
dot
(
b
,
A
)
return
quadprog_solve_qp
(
P
,
q
,
G
,
h
)
#min q' x
#subject to G x <= h
#subject to C x = d
def
solve_lp
(
q
,
G
=
None
,
h
=
None
,
C
=
None
,
d
=
None
):
qp_G
=
identity
(
q
.
shape
[
0
])
*
0.000001
qp_a
=
-
q
if
C
is
not
None
:
if
G
is
not
None
:
qp_C
=
-
vstack
([
C
,
G
]).
T
qp_b
=
-
hstack
([
d
,
h
])
else
:
qp_C
=
-
C
.
transpose
()
qp_b
=
-
d
meq
=
C
.
shape
[
0
]
else
:
# no equality constraint
qp_C
=
-
G
.
T
qp_b
=
-
h
meq
=
0
return
quadprog
.
solve_qp
(
qp_G
,
qp_a
,
qp_C
,
qp_b
,
meq
)[
0
]
if
__name__
==
'__main__'
:
from
numpy.linalg
import
norm
A
=
array
([[
1.
,
2.
,
0.
],
[
-
8.
,
3.
,
2.
],
[
0.
,
1.
,
1.
]])
b
=
array
([
3.
,
2.
,
3.
])
P
=
dot
(
A
.
T
,
A
)
q
=
2
*
dot
(
b
,
A
).
reshape
((
3
,))
G
=
array
([[
1.
,
2.
,
1.
],
[
2.
,
0.
,
1.
],
[
-
1.
,
2.
,
-
1.
]])
h
=
array
([
3.
,
2.
,
-
2.
]).
reshape
((
3
,))
res2
=
solve_least_square
(
A
,
b
,
G
,
h
)
res1
=
quadprog_solve_qp
(
P
,
q
,
G
,
h
)
print
res1
print
res2
python/test/variable/qp_traj/qp_cord.py
0 → 100644
View file @
c2ed8387
from
spline
import
*
from
varBezier
import
varBezier
from
convex_hull
import
*
from
qp
import
solve_lp
,
quadprog_solve_qp
from
numpy
import
matrix
,
array
,
zeros
,
ones
,
diag
,
cross
,
vstack
,
identity
from
numpy.linalg
import
norm
__EPS
=
1e-6
#### helpers for stacking matrices ####
def
concat
(
m1
,
m2
):
if
(
type
(
m1
)
==
type
(
None
)):
return
m2
return
vstack
([
m1
,
m2
]).
reshape
([
-
1
,
m2
.
shape
[
-
1
]])
def
concatvec
(
m1
,
m2
):
if
(
type
(
m1
)
==
type
(
None
)):
return
m2
return
array
(
m1
.
tolist
()
+
m2
.
tolist
())
#### helpers for stacking matrices ####
#constraint to lie between 2 extremities of a segment
def
segmentConstraint
(
varBez
,
a
,
b
,
wpIndex
,
constraintVarIndex
,
totalAddVarConstraints
):
mat
,
vec
=
varBez
.
matrixFromWaypoints
(
wpIndex
)
vec
=
b
-
vec
resmat
=
zeros
([
mat
.
shape
[
0
],
mat
.
shape
[
1
]
+
totalAddVarConstraints
])
resmat
[:,:
mat
.
shape
[
1
]]
=
mat
resmat
[:,
mat
.
shape
[
1
]
+
constraintVarIndex
-
1
]
=
b
-
a
return
(
resmat
,
vec
)
#constraint to lie one side of a line
def
lineConstraint
(
varBez
,
C
,
d
,
totalAddVarConstraints
):
resmat
=
None
resvec
=
None
for
wpIndex
in
range
(
varBez
.
bezier
.
nbWaypoints
):
mat
,
vec
=
varBez
.
matrixFromWaypoints
(
wpIndex
)
mat
=
C
.
dot
(
mat
)
vec
=
d
-
C
.
dot
(
vec
)
resmat
=
concat
(
resmat
,
mat
)
resvec
=
concatvec
(
resvec
,
vec
)
augmented
=
zeros
([
resmat
.
shape
[
0
],
resmat
.
shape
[
1
]
+
totalAddVarConstraints
])
augmented
[:,:
resmat
.
shape
[
1
]]
=
resmat
return
(
augmented
,
resvec
)
##### cost function integrals #####
#compute bezier primitive of variable waypoints given these waypoints
def
compute_primitive
(
wps
):
new_degree
=
(
len
(
wps
)
-
1
+
1
)
current_sum
=
[
zeros
(
wps
[
0
][
0
].
shape
),
0
]
n_wp
=
[(
current_sum
[
0
],
0.
)]
for
wp
in
wps
:
current_sum
[
0
]
=
current_sum
[
0
]
+
wp
[
0
]
current_sum
[
1
]
=
current_sum
[
1
]
+
wp
[
1
]
n_wp
+=
[(
current_sum
[
0
]
/
new_degree
,
current_sum
[
1
]
/
new_degree
)]
return
n_wp
#cost function for analytical integral cost of squared acceleration
def
accelerationcost
(
bezVar
):
acc
=
bezVar
.
bezier
.
compute_derivate
(
2
)
hacc
=
varBezier
();
hacc
.
bezier
=
acc
wps_i
=
[[],[],[]]
for
i
in
range
(
3
):
#integrate for each axis
for
j
in
range
(
acc
.
nbWaypoints
):
A_i
=
hacc
.
matrixFromWaypoints
(
j
)[
0
][
i
,:].
reshape
([
1
,
-
1
])
b_i
=
hacc
.
matrixFromWaypoints
(
j
)[
1
][
i
]
wps_i
[
i
]
+=
[(
A_i
.
transpose
().
dot
(
A_i
),
b_i
*
b_i
)
]
# now integrate each bezier curve
for
i
,
wps
in
enumerate
(
wps_i
):
wps_i
[
i
]
=
compute_primitive
(
wps
)
resmat
=
wps_i
[
0
][
-
1
][
0
];
resvec
=
wps_i
[
0
][
-
1
][
1
]
for
i
in
range
(
1
,
3
):
resmat
=
resmat
+
wps_i
[
i
][
-
1
][
0
]
resvec
=
resvec
+
wps_i
[
i
][
-
1
][
1
]
return
(
resmat
,
resvec
)
##### cost function integrals #####
python/test/variable/qp_traj/test_cord.py
0 → 100644
View file @
c2ed8387
from
spline
import
*
from
numpy
import
matrix
,
array
,
zeros
,
ones
,
diag
,
cross
from
numpy.linalg
import
norm
__EPS
=
1e-6
from
varBezier
import
varBezier
from
qp_cord
import
*
from
plot_cord
import
*
idxFile
=
0
import
string
import
uuid
;
uuid
.
uuid4
().
hex
.
upper
()[
0
:
6
]
######################## generate problems ########################
def
genBezierInput
(
numvars
=
3
):
valDep
=
array
([
np
.
random
.
uniform
(
0.
,
1.
),
np
.
random
.
uniform
(
0.
,
5.
),
0.
])
valEnd
=
array
([
np
.
random
.
uniform
(
5.
,
10.
),
np
.
random
.
uniform
(
0.
,
5.
),
0.
])
return
varBezier
([
valDep
]
+
[
""
for
_
in
range
(
numvars
)]
+
[
valEnd
],
1.
)
def
genSplit
(
numCurves
):
splits
=
[]
lastval
=
np
.
random
.
uniform
(
0.
,
1.
)
for
i
in
range
(
numCurves
):
splits
+=
[
lastval
]
lastval
+=
np
.
random
.
uniform
(
0.
,
1.
)
return
[
el
/
lastval
for
el
in
splits
[:
-
1
]]
def
getRightMostLine
(
ptList
):
pt1
=
array
([
0.
,
0.
,
0.
])
pt2
=
array
([
0.
,
0.
,
0.
])
for
pt
in
ptList
:
if
pt
[
0
]
>
pt1
[
0
]:
pt1
=
pt
elif
pt
[
0
]
>
pt2
[
0
]:
pt2
=
pt
if
pt1
[
1
]
<
pt2
[
1
]:
return
[
pt2
,
pt1
]
else
:
return
[
pt1
,
pt2
]
######################## generate problems ########################
######################## solve a given problem ########################
#inequality constraint from line
def
getLineFromSegment
(
line
):
a
=
line
[
0
];
b
=
line
[
1
];
c
=
a
.
copy
()
;
c
[