### 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, Ab 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) return zeros(P.shape) 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]]).T #set end position pD.end_pos = array([ptsTime[-1]]).T # set initial position pD.init_pos = array([ptsTime]).T # set end position pD.end_pos = array([ptsTime[-1]]).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) * 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() 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() 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