Skip to content
GitLab
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
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.
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