Commit 8d35b0a4 authored by stevet's avatar stevet
Browse files

added notebook as test

parent 39248c8b
......@@ -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")
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()
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment