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
11c34d91
Commit
11c34d91
authored
Jan 10, 2020
by
Guilhem Saurel
Browse files
Python Format
parent
ee941bdc
Pipeline
#7884
failed with stage
in 4 minutes and 36 seconds
Changes
5
Pipelines
2
Expand all
Hide whitespace changes
Inline
Side-by-side
python/curves/__init__.py
View file @
11c34d91
...
...
@@ -2,4 +2,4 @@
# Copyright (c) 2019 CNRS
# Author : Steve Tonneau
from
.curves
import
*
from
.curves
import
*
# noqa
python/curves/optimization.py
View file @
11c34d91
...
...
@@ -2,4 +2,4 @@
# Copyright (c) 2019 CNRS
# Author : Steve Tonneau
from
.curves.optimization
import
*
from
.curves.optimization
import
*
# noqa
python/curves/plot.py
View file @
11c34d91
import
eigenpy
import
matplotlib.pyplot
as
plt
import
numpy
as
np
from
mpl_toolkits.mplot3d
import
Axes3D
from
numpy
import
array
from
.curves
import
bezier
...
...
python/test/notebook.py
View file @
11c34d91
import
os
import
unittest
from
math
import
sqrt
import
eigenpy
import
numpy
as
np
from
numpy
import
array
,
array_equal
,
isclose
,
random
,
zeros
,
identity
,
dot
,
hstack
,
vstack
from
numpy.linalg
import
norm
from
numpy
import
array
,
dot
,
identity
,
zeros
from
curves
import
(
CURVES_WITH_PINOCCHIO_SUPPORT
,
Quaternion
,
SE3Curve
,
SO3Linear
,
bezier
,
bezier3
,
cubic_hermite_spline
,
curve_constraints
,
exact_cubic
,
polynomial
,
piecewise
,
piecewise_SE3
,
convert_to_polynomial
,
convert_to_bezier
,
convert_to_hermite
)
# importing the bezier curve class
from
curves
import
bezier
eigenpy
.
switchToNumpyArray
()
#importing the bezier curve class
from
curves
import
(
bezier
)
#dummy methods
# dummy methods
def
plot
(
*
karrgs
):
pass
pass
class
TestNotebook
(
unittest
.
TestCase
):
...
...
@@ -31,98 +21,88 @@ class TestNotebook(unittest.TestCase):
def
test_notebook
(
self
):
print
(
"test_notebook"
)
#We describe a degree 3 curve as a Bezier curve with 4 control points
#
We describe a degree 3 curve as a Bezier curve with 4 control points
waypoints
=
array
([[
1.
,
2.
,
3.
],
[
-
4.
,
-
5.
,
-
6.
],
[
4.
,
5.
,
6.
],
[
7.
,
8.
,
9.
]]).
transpose
()
ref
=
bezier
(
waypoints
)
numSamples
=
10
;
fNumSamples
=
float
(
numSamples
)
ptsTime
=
[
(
ref
(
float
(
t
)
/
fNumSamples
),
float
(
t
)
/
fNumSamples
)
for
t
in
range
(
numSamples
+
1
)]
numSamples
=
10
fNumSamples
=
float
(
numSamples
)
ptsTime
=
[(
ref
(
float
(
t
)
/
fNumSamples
),
float
(
t
)
/
fNumSamples
)
for
t
in
range
(
numSamples
+
1
)]
from
curves.optimization
import
(
problem_definition
,
setup_control_points
)
#dimension of our problem (here 3 as our curve is 3D)
#
dimension of our problem (here 3 as our curve is 3D)
dim
=
3
refDegree
=
3
pD
=
problem_definition
(
dim
)
pD
.
degree
=
refDegree
#
we want to fit a curve of the same degree as the reference curve for the sanity check
pD
.
degree
=
refDegree
#
we want to fit a curve of the same degree as the reference curve for the sanity check
#generates the variable bezier curve with the parameters of problemDefinition
#
generates the variable bezier curve with the parameters of problemDefinition
problem
=
setup_control_points
(
pD
)
#for now we only care about the curve itself
#
for now we only care about the curve itself
variableBezier
=
problem
.
bezier
()
linearVariable
=
variableBezier
(
0.
)
variableBezier
(
0.
)
#least square form of ||Ax-b||**2
#
least square form of ||Ax-b||**2
def
to_least_square
(
A
,
b
):
return
dot
(
A
.
T
,
A
),
-
dot
(
A
.
T
,
b
)
return
dot
(
A
.
T
,
A
),
-
dot
(
A
.
T
,
b
)
def
genCost
(
variableBezier
,
ptsTime
):
#first evaluate variableBezier for each time sampled
allsEvals
=
[(
variableBezier
(
time
),
pt
)
for
(
pt
,
time
)
in
ptsTime
]
#then compute the least square form of the cost for each points
allLeastSquares
=
[
to_least_square
(
el
.
B
(),
el
.
c
()
+
pt
)
for
(
el
,
pt
)
in
allsEvals
]
#and finally sum the costs
#
first evaluate variableBezier for each time sampled
allsEvals
=
[(
variableBezier
(
time
),
pt
)
for
(
pt
,
time
)
in
ptsTime
]
#
then compute the least square form of the cost for each points
allLeastSquares
=
[
to_least_square
(
el
.
B
(),
el
.
c
()
+
pt
)
for
(
el
,
pt
)
in
allsEvals
]
#
and finally sum the costs
Ab
=
[
sum
(
x
)
for
x
in
zip
(
*
allLeastSquares
)]
return
Ab
[
0
],
Ab
[
1
]
A
,
b
=
genCost
(
variableBezier
,
ptsTime
)
A
,
b
=
genCost
(
variableBezier
,
ptsTime
)
def
quadprog_solve_qp
(
P
,
q
,
G
=
None
,
h
=
None
,
C
=
None
,
d
=
None
,
verbose
=
False
):
return
zeros
(
P
.
shape
[
0
])
return
zeros
(
P
.
shape
[
0
])
res
=
quadprog_solve_qp
(
A
,
b
)
def
evalAndPlot
(
variableBezier
,
res
):
fitBezier
=
variableBezier
.
evaluate
(
res
.
reshape
((
-
1
,
1
))
)
fitBezier
=
variableBezier
.
evaluate
(
res
.
reshape
((
-
1
,
1
))
)
return
fitBezier
fitBezier
=
evalAndPlot
(
variableBezier
,
res
)
fitBezier
=
evalAndPlot
(
variableBezier
,
res
)
pD
.
degree
=
refDegree
-
1
problem
=
setup_control_points
(
pD
)
variableBezier
=
problem
.
bezier
()
A
,
b
=
genCost
(
variableBezier
,
ptsTime
)
res
=
quadprog_solve_qp
(
A
,
b
)
fitBezier
=
evalAndPlot
(
variableBezier
,
res
)
from
curves.optimization
import
constraint_flag
pD
.
flag
=
constraint_flag
.
INIT_POS
|
constraint_flag
.
END_POS
#set initial position
pD
.
init_pos
=
array
([
ptsTime
[
0
][
0
]]).
T
#set end position
pD
.
end_pos
=
array
([
ptsTime
[
-
1
][
0
]]).
T
#
set initial position
pD
.
init_pos
=
array
([
ptsTime
[
0
][
0
]]).
T
#
set end position
pD
.
end_pos
=
array
([
ptsTime
[
-
1
][
0
]]).
T
problem
=
setup_control_points
(
pD
)
variableBezier
=
problem
.
bezier
()
prob
=
setup_control_points
(
pD
)
variableBezier
=
prob
.
bezier
()
A
,
b
=
genCost
(
variableBezier
,
ptsTime
)
res
=
quadprog_solve_qp
(
A
,
b
)
_
=
evalAndPlot
(
variableBezier
,
res
)
evalAndPlot
(
variableBezier
,
res
)
#values are 0 by default, so if the constraint is zero this can be skipped
#
values are 0 by default, so if the constraint is zero this can be skipped
pD
.
init_vel
=
array
([[
0.
,
0.
,
0.
]]).
T
pD
.
init_acc
=
array
([[
0.
,
0.
,
0.
]]).
T
pD
.
end_vel
=
array
([[
0.
,
0.
,
0.
]]).
T
pD
.
end_acc
=
array
([[
0.
,
0.
,
0.
]]).
T
pD
.
flag
=
constraint_flag
.
END_POS
|
constraint_flag
.
INIT_POS
|
constraint_flag
.
INIT_VEL
|
constraint_flag
.
END_VEL
|
constraint_flag
.
INIT_ACC
|
constraint_flag
.
END_ACC
pD
.
flag
=
(
constraint_flag
.
END_POS
|
constraint_flag
.
INIT_POS
|
constraint_flag
.
INIT_VEL
|
constraint_flag
.
END_VEL
|
constraint_flag
.
INIT_ACC
|
constraint_flag
.
END_ACC
)
err
=
False
try
:
...
...
@@ -131,7 +111,6 @@ class TestNotebook(unittest.TestCase):
err
=
True
assert
err
pD
.
degree
=
refDegree
+
4
prob
=
setup_control_points
(
pD
)
variableBezier
=
prob
.
bezier
()
...
...
@@ -139,47 +118,44 @@ class TestNotebook(unittest.TestCase):
res
=
quadprog_solve_qp
(
A
,
b
)
fitBezier
=
evalAndPlot
(
variableBezier
,
res
)
pD
.
degree
=
refDegree
+
60
prob
=
setup_control_points
(
pD
)
variableBezier
=
prob
.
bezier
()
A
,
b
=
genCost
(
variableBezier
,
ptsTime
)
#regularization matrix
#
regularization matrix
reg
=
identity
(
A
.
shape
[
1
])
*
0.001
res
=
quadprog_solve_qp
(
A
+
reg
,
b
)
fitBezier
=
evalAndPlot
(
variableBezier
,
res
)
#set initial / terminal constraints
# set initial / terminal constraints
pD
.
flag
=
constraint_flag
.
END_POS
|
constraint_flag
.
INIT_POS
pD
.
degree
=
refDegree
prob
=
setup_control_points
(
pD
)
variableBezier
=
prob
.
bezier
()
#get value of the curve first order derivative at t = 0.8
t08Constraint
=
variableBezier
.
derivate
(
0.8
,
1
)
target
=
zeros
(
3
)
#
get value of the curve first order derivative at t = 0.8
t08Constraint
=
variableBezier
.
derivate
(
0.8
,
1
)
target
=
zeros
(
3
)
A
,
b
=
genCost
(
variableBezier
,
ptsTime
)
#solve optimization problem with quadprog
#
solve optimization problem with quadprog
res
=
quadprog_solve_qp
(
A
,
b
,
C
=
t08Constraint
.
B
(),
d
=
target
-
t08Constraint
.
c
())
fitBezier
=
evalAndPlot
(
variableBezier
,
res
)
#returns a curve composed of the split curves, 2 in our case
#
returns a curve composed of the split curves, 2 in our case
piecewiseCurve
=
ref
.
split
(
array
([[
0.6
]]).
T
)
#displaying the obtained curves
#
displaying the obtained curves
#first, split the variable curve
#
first, split the variable curve
piecewiseCurve
=
variableBezier
.
split
(
array
([[
0.4
,
0.8
]]).
T
)
constrainedCurve
=
piecewiseCurve
.
curve_at_index
(
1
)
#find the number of variables
#
find the number of variables
problemSize
=
prob
.
numVariables
*
dim
#find the number of constraints, as many as waypoints
#
find the number of constraints, as many as waypoints
nConstraints
=
constrainedCurve
.
nbWaypoints
waypoints
=
constrainedCurve
.
waypoints
()
...
...
@@ -187,17 +163,15 @@ class TestNotebook(unittest.TestCase):
ineqMatrix
=
zeros
((
nConstraints
,
problemSize
))
ineqVector
=
zeros
(
nConstraints
)
#finding the z equation of each control point
# finding the z equation of each control point
for
i
in
range
(
nConstraints
):
wayPoint
=
constrainedCurve
.
waypointAtIndex
(
i
)
ineqMatrix
[
i
,:]
=
wayPoint
.
B
()[
2
,:]
ineqVector
[
i
]
=
-
wayPoint
.
c
()[
2
]
res
=
quadprog_solve_qp
(
A
,
b
,
G
=
ineqMatrix
,
h
=
ineqVector
)
fitBezier
=
variableBezier
.
evaluate
(
res
.
reshape
((
-
1
,
1
))
)
ineqMatrix
[
i
,
:]
=
wayPoint
.
B
()[
2
,
:]
ineqVector
[
i
]
=
-
wayPoint
.
c
()[
2
]
res
=
quadprog_solve_qp
(
A
,
b
,
G
=
ineqMatrix
,
h
=
ineqVector
)
fitBezier
=
variableBezier
.
evaluate
(
res
.
reshape
((
-
1
,
1
)))
fitBezier
if
__name__
==
'__main__'
:
...
...
python/test/test.py
View file @
11c34d91
This diff is collapsed.
Click to expand it.
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