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

Python Format

parent ee941bdc
Pipeline #11483 failed with stage
in 4 minutes and 1 second
......@@ -2,4 +2,4 @@
# Copyright (c) 2019 CNRS
# Author : Steve Tonneau
from .curves import *
from .curves import * # noqa
......@@ -2,4 +2,4 @@
# Copyright (c) 2019 CNRS
# Author : Steve Tonneau
from .curves.optimization import *
from .curves.optimization import * # noqa
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
......
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__':
......
This diff is collapsed.
Markdown is supported
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