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
8d35b0a4
Commit
8d35b0a4
authored
Jan 09, 2020
by
stevet
Browse files
added notebook as test
parent
39248c8b
Changes
2
Hide whitespace changes
Inline
Side-by-side
python/CMakeLists.txt
View file @
8d35b0a4
...
...
@@ -36,3 +36,4 @@ install (FILES deploy/optimization/__init__.py
ADD_PYTHON_UNIT_TEST
(
"python-curves"
"python/test/test.py"
"python"
)
ADD_PYTHON_UNIT_TEST
(
"python-optimization"
"python/test/optimization.py"
"python"
)
ADD_PYTHON_UNIT_TEST
(
"python-notebook"
"python/test/notebook.py"
"python"
)
python/test/notebook.py
0 → 100644
View file @
8d35b0a4
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
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
)
eigenpy
.
switchToNumpyArray
()
#importing the bezier curve class
from
curves
import
(
bezier
)
#dummy methods
def
plot
(
*
karrgs
):
pass
class
TestNotebook
(
unittest
.
TestCase
):
# def print_str(self, inStr):
# print inStr
# return
def
test_notebook
(
self
):
print
(
"test_notebook"
)
#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
)]
from
curves.optimization
import
(
problem_definition
,
setup_control_points
)
#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
#generates the variable bezier curve with the parameters of problemDefinition
problem
=
setup_control_points
(
pD
)
#for now we only care about the curve itself
variableBezier
=
problem
.
bezier
()
linearVariable
=
variableBezier
(
0.
)
#least square form of ||Ax-b||**2
def
to_least_square
(
A
,
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
Ab
=
[
sum
(
x
)
for
x
in
zip
(
*
allLeastSquares
)]
return
Ab
[
0
],
Ab
[
1
]
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
])
res
=
quadprog_solve_qp
(
A
,
b
)
def
evalAndPlot
(
variableBezier
,
res
):
fitBezier
=
variableBezier
.
evaluate
(
res
.
reshape
((
-
1
,
1
))
)
return
fitBezier
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
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
)
#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
err
=
False
try
:
prob
=
setup_control_points
(
pD
)
except
RuntimeError
,
e
:
err
=
True
assert
err
pD
.
degree
=
refDegree
+
4
prob
=
setup_control_points
(
pD
)
variableBezier
=
prob
.
bezier
()
A
,
b
=
genCost
(
variableBezier
,
ptsTime
)
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
reg
=
identity
(
A
.
shape
[
1
])
*
0.001
res
=
quadprog_solve_qp
(
A
+
reg
,
b
)
fitBezier
=
evalAndPlot
(
variableBezier
,
res
)
#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
)
A
,
b
=
genCost
(
variableBezier
,
ptsTime
)
#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
piecewiseCurve
=
ref
.
split
(
array
([[
0.6
]]).
T
)
#displaying the obtained curves
#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
problemSize
=
prob
.
numVariables
*
dim
#find the number of constraints, as many as waypoints
nConstraints
=
constrainedCurve
.
nbWaypoints
waypoints
=
constrainedCurve
.
waypoints
()
ineqMatrix
=
zeros
((
nConstraints
,
problemSize
))
ineqVector
=
zeros
(
nConstraints
)
#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
))
)
if
__name__
==
'__main__'
:
unittest
.
main
()
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