Commit 11c34d91 authored by Guilhem Saurel's avatar Guilhem Saurel
Browse files

Python Format

parent ee941bdc
Pipeline #7884 failed with stage
in 4 minutes and 36 seconds
...@@ -2,4 +2,4 @@ ...@@ -2,4 +2,4 @@
# Copyright (c) 2019 CNRS # Copyright (c) 2019 CNRS
# Author : Steve Tonneau # Author : Steve Tonneau
from .curves import * from .curves import * # noqa
...@@ -2,4 +2,4 @@ ...@@ -2,4 +2,4 @@
# Copyright (c) 2019 CNRS # Copyright (c) 2019 CNRS
# Author : Steve Tonneau # Author : Steve Tonneau
from .curves.optimization import * from .curves.optimization import * # noqa
import eigenpy import eigenpy
import matplotlib.pyplot as plt import matplotlib.pyplot as plt
import numpy as np import numpy as np
from mpl_toolkits.mplot3d import Axes3D
from numpy import array from numpy import array
from .curves import bezier from .curves import bezier
......
import os
import unittest import unittest
from math import sqrt
import eigenpy import eigenpy
import numpy as np from numpy import array, dot, identity, zeros
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, # importing the bezier curve class
cubic_hermite_spline, curve_constraints, exact_cubic,polynomial, from curves import bezier
piecewise, piecewise_SE3, convert_to_polynomial,convert_to_bezier,convert_to_hermite)
eigenpy.switchToNumpyArray() eigenpy.switchToNumpyArray()
# dummy methods
#importing the bezier curve class
from curves import (bezier)
#dummy methods
def plot(*karrgs): def plot(*karrgs):
pass pass
class TestNotebook(unittest.TestCase): class TestNotebook(unittest.TestCase):
...@@ -31,98 +21,88 @@ class TestNotebook(unittest.TestCase): ...@@ -31,98 +21,88 @@ class TestNotebook(unittest.TestCase):
def test_notebook(self): def test_notebook(self):
print("test_notebook") 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() waypoints = array([[1., 2., 3.], [-4., -5., -6.], [4., 5., 6.], [7., 8., 9.]]).transpose()
ref = bezier(waypoints) ref = bezier(waypoints)
numSamples = 10
numSamples = 10; fNumSamples = float(numSamples) fNumSamples = float(numSamples)
ptsTime = [ (ref(float(t) / fNumSamples), float(t) / fNumSamples) for t in range(numSamples+1)] ptsTime = [(ref(float(t) / fNumSamples), float(t) / fNumSamples) for t in range(numSamples + 1)]
from curves.optimization import (problem_definition, setup_control_points) 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 dim = 3
refDegree = 3 refDegree = 3
pD = problem_definition(dim) 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) 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() 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): 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): def genCost(variableBezier, ptsTime):
#first evaluate variableBezier for each time sampled # first evaluate variableBezier for each time sampled
allsEvals = [(variableBezier(time), pt) for (pt,time) in ptsTime] allsEvals = [(variableBezier(time), pt) for (pt, time) in ptsTime]
#then compute the least square form of the cost for each points # 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] allLeastSquares = [to_least_square(el.B(), el.c() + pt) for (el, pt) in allsEvals]
#and finally sum the costs # and finally sum the costs
Ab = [sum(x) for x in zip(*allLeastSquares)] Ab = [sum(x) for x in zip(*allLeastSquares)]
return Ab[0], Ab[1] 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): 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) res = quadprog_solve_qp(A, b)
def evalAndPlot(variableBezier, res): def evalAndPlot(variableBezier, res):
fitBezier = variableBezier.evaluate(res.reshape((-1,1)) ) fitBezier = variableBezier.evaluate(res.reshape((-1, 1)))
return fitBezier return fitBezier
fitBezier = evalAndPlot(variableBezier, res)
fitBezier = evalAndPlot(variableBezier, res)
pD.degree = refDegree - 1 pD.degree = refDegree - 1
problem = setup_control_points(pD) problem = setup_control_points(pD)
variableBezier = problem.bezier() variableBezier = problem.bezier()
A, b = genCost(variableBezier, ptsTime) A, b = genCost(variableBezier, ptsTime)
res = quadprog_solve_qp(A, b) res = quadprog_solve_qp(A, b)
fitBezier = evalAndPlot(variableBezier, res) fitBezier = evalAndPlot(variableBezier, res)
from curves.optimization import constraint_flag from curves.optimization import constraint_flag
pD.flag = constraint_flag.INIT_POS | constraint_flag.END_POS pD.flag = constraint_flag.INIT_POS | constraint_flag.END_POS
#set initial position # set initial position
pD.init_pos = array([ptsTime[ 0][0]]).T pD.init_pos = array([ptsTime[0][0]]).T
#set end position # set end position
pD.end_pos = array([ptsTime[-1][0]]).T pD.end_pos = array([ptsTime[-1][0]]).T
problem = setup_control_points(pD) problem = setup_control_points(pD)
variableBezier = problem.bezier() variableBezier = problem.bezier()
prob = setup_control_points(pD) prob = setup_control_points(pD)
variableBezier = prob.bezier() variableBezier = prob.bezier()
A, b = genCost(variableBezier, ptsTime) A, b = genCost(variableBezier, ptsTime)
res = quadprog_solve_qp(A, b) 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_vel = array([[0., 0., 0.]]).T
pD.init_acc = array([[0., 0., 0.]]).T pD.init_acc = array([[0., 0., 0.]]).T
pD.end_vel = array([[0., 0., 0.]]).T pD.end_vel = array([[0., 0., 0.]]).T
pD.end_acc = 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 err = False
try: try:
...@@ -131,7 +111,6 @@ class TestNotebook(unittest.TestCase): ...@@ -131,7 +111,6 @@ class TestNotebook(unittest.TestCase):
err = True err = True
assert err assert err
pD.degree = refDegree + 4 pD.degree = refDegree + 4
prob = setup_control_points(pD) prob = setup_control_points(pD)
variableBezier = prob.bezier() variableBezier = prob.bezier()
...@@ -139,47 +118,44 @@ class TestNotebook(unittest.TestCase): ...@@ -139,47 +118,44 @@ class TestNotebook(unittest.TestCase):
res = quadprog_solve_qp(A, b) res = quadprog_solve_qp(A, b)
fitBezier = evalAndPlot(variableBezier, res) fitBezier = evalAndPlot(variableBezier, res)
pD.degree = refDegree + 60 pD.degree = refDegree + 60
prob = setup_control_points(pD) prob = setup_control_points(pD)
variableBezier = prob.bezier() variableBezier = prob.bezier()
A, b = genCost(variableBezier, ptsTime) A, b = genCost(variableBezier, ptsTime)
#regularization matrix # regularization matrix
reg = identity(A.shape[1]) * 0.001 reg = identity(A.shape[1]) * 0.001
res = quadprog_solve_qp(A + reg, b) res = quadprog_solve_qp(A + reg, b)
fitBezier = evalAndPlot(variableBezier, res) fitBezier = evalAndPlot(variableBezier, res)
# set initial / terminal constraints
#set initial / terminal constraints
pD.flag = constraint_flag.END_POS | constraint_flag.INIT_POS pD.flag = constraint_flag.END_POS | constraint_flag.INIT_POS
pD.degree = refDegree pD.degree = refDegree
prob = setup_control_points(pD) prob = setup_control_points(pD)
variableBezier = prob.bezier() variableBezier = prob.bezier()
#get value of the curve first order derivative at t = 0.8 # get value of the curve first order derivative at t = 0.8
t08Constraint = variableBezier.derivate(0.8,1) t08Constraint = variableBezier.derivate(0.8, 1)
target = zeros(3) target = zeros(3)
A, b = genCost(variableBezier, ptsTime) 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()) res = quadprog_solve_qp(A, b, C=t08Constraint.B(), d=target - t08Constraint.c())
fitBezier = evalAndPlot(variableBezier, res) 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) 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) piecewiseCurve = variableBezier.split(array([[0.4, 0.8]]).T)
constrainedCurve = piecewiseCurve.curve_at_index(1) constrainedCurve = piecewiseCurve.curve_at_index(1)
#find the number of variables # find the number of variables
problemSize = prob.numVariables * dim 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 nConstraints = constrainedCurve.nbWaypoints
waypoints = constrainedCurve.waypoints() waypoints = constrainedCurve.waypoints()
...@@ -187,17 +163,15 @@ class TestNotebook(unittest.TestCase): ...@@ -187,17 +163,15 @@ class TestNotebook(unittest.TestCase):
ineqMatrix = zeros((nConstraints, problemSize)) ineqMatrix = zeros((nConstraints, problemSize))
ineqVector = zeros(nConstraints) ineqVector = zeros(nConstraints)
# finding the z equation of each control point
#finding the z equation of each control point
for i in range(nConstraints): for i in range(nConstraints):
wayPoint = constrainedCurve.waypointAtIndex(i) wayPoint = constrainedCurve.waypointAtIndex(i)
ineqMatrix[i,:] = wayPoint.B()[2,:] ineqMatrix[i, :] = wayPoint.B()[2, :]
ineqVector[i] = -wayPoint.c()[2] ineqVector[i] = -wayPoint.c()[2]
res = quadprog_solve_qp(A, b, G=ineqMatrix, h = ineqVector)
fitBezier = variableBezier.evaluate(res.reshape((-1,1)) )
res = quadprog_solve_qp(A, b, G=ineqMatrix, h=ineqVector)
fitBezier = variableBezier.evaluate(res.reshape((-1, 1)))
fitBezier
if __name__ == '__main__': if __name__ == '__main__':
......
This diff is collapsed.
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