def default_fit(degree, ptsTime, dim=3, totalTime=1., constraintFlag=constraint_flag.NONE, curveConstraints=None):
pD = None
if curveConstraints is None:
pD = problem_definition(dim)
else:
pD = problem_definition(curveConstraints)
# set initial and goal positions to the ones of the list (might not be used if constraints do not correspond)
pD.init_pos = array([ptsTime[0][0]]).T
pD.end_pos = array([ptsTime[-1][0]]).T
# assign curve constraints
pD.totalTime = totalTime
pD.degree = degree
pD.flag = constraintFlag
problem = setup_control_points(pD)
bezierLinear = problem.bezier()
pD.totalTime = totalTime
pD.degree = degree
problem = setup_control_points(pD)
bezierLinear = problem.bezier()
allsEvals = [(bezierLinear(time), pt) for (pt, time) in ptsTime]
allLeastSquares = [to_least_square(el.B(), el.c() + pt) for (el, pt) in allsEvals]
Ab = [sum(x) for x in zip(*allLeastSquares)]
res = quadprog_solve_qp(Ab[0] + identity(problem.numVariables * dim) * 0.0001, Ab[1])
return bezierLinear.evaluate(res.reshape((-1, 1)))
def test_problem_definition(self):
pD = problem_definition(3)
pD.init_pos
pD.init_vel = matrix([0., 1., 1.]).transpose()
pD.end_vel = matrix([0., 1., 1.]).transpose()
pD.init_acc = matrix([0., 1., -1.]).transpose()
pD.end_acc = matrix([0., 0., 0]).transpose()
pD.init_pos = array([[0., 0., 0.]]).T
pD.end_pos = array([[1., 1., 1.]]).T
pD.flag = constraint_flag.INIT_VEL | constraint_flag.INIT_POS
generate_integral_problem(pD, integral_cost_flag.ACCELERATION)
# generate problem
problem = setup_control_points(pD)
bezierLinear = problem.bezier()
bezierFixed = bezierLinear.evaluate(array([zeros(12)]).T)
self.assertTrue(bezierFixed.nbWaypoints == pD.degree + 1)
self.assertTrue(norm(bezierFixed(0.) - pD.init_pos) <= 0.001)
self.assertTrue(
norm(bezierFixed.derivate(0.0, 1) - pD.init_vel) <= 0.001)
def MakeBeizerConvexhull2D(in_LB):
#dimension of our problem (here 3 as our curve is 3D)
dim = 2 # dimension = 3, 3D
refDegree = 3 # degree = 3, polynomial we have to get 4 control points (start,end,c1,c2)
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.flag = constraint_flag.INIT_POS | constraint_flag.END_POS
#set initial position
pD.init_pos = array([in_LB.point_x[0], in_LB.point_y[0]])
#set end position
pD.end_pos = array(
[in_LB.point_x[-1], in_LB.point_y[-1]]
) # -1 refers to the last index, -2 refers to the second last index and so on
problem = setup_control_points(pD)
#generates the variable bezier curve with the parameters of problemDefinition
variableBezier = problem.bezier()
#We describe a degree 3 curve as a Bezier curve with 4 control points
distance = in_LB.point_x[-1] - in_LB.point_x[0]
distance_x = abs(distance)
distance = in_LB.point_y[-1] - in_LB.point_y[0]
distance_y = abs(distance)
waypoints = array([
[in_LB.point_x[0], in_LB.point_y[0]],
[in_LB.point_x[0] + distance_x, in_LB.point_y[0]],
[in_LB.point_x[0], in_LB.point_y[0] + distance_y],
[in_LB.point_x[-1], in_LB.point_y[-1]]
]).transpose() #[in_LB.point_x[0],in_LB.point_y[0]+distance_y],
ref = bezier(waypoints)
#plotting sampled points
numSamples = 10
fNumSamples = float(numSamples)
ptsTime = [(ref(float(t) / fNumSamples), float(t) / fNumSamples)
for t in range(numSamples + 1)]
#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
] # el.c() + pt ?
#and finally sum the costs
Ab = [sum(x) for x in zip(*allLeastSquares)]
return Ab[0], Ab[1]
A, b = genCost(variableBezier, ptsTime)
costAb = generate_integral_problem(pD, integral_cost_flag.ACCELERATION)
problemSize = problem.numVariables * dim # what is this? dimension * degree ?
convexhull_1_ineq = array(in_LB.convexhull1.equations)
convexhull_2_ineq = array(in_LB.convexhull2.equations)
nConvexEq = convexhull_1_ineq.shape[0]
ineqMatrix = zeros((nConvexEq * 4 + nConvexEq * 4 * 2 + nConvexEq * 4,
problemSize)) # C1,C2[4*2*2]
ineqVector = zeros(nConvexEq * 4 + nConvexEq * 4 * 2 + nConvexEq * 4)
# ineqMatrix = zeros((convexhull_1_ineq.shape[0] * 4 * 2, problemSize)) # C1,C2[4*2*2]
# ineqVector = zeros(convexhull_1_ineq.shape[0] * 4 * 2)
array_CB = np.array([], dtype=float).reshape(0, 4)
vec_CB = []
piecewiseCurve = variableBezier.split(array([[0.3, 0.6]]).T)
# left side
constrainedCurve = piecewiseCurve.curve_at_index(0)
array_C_cur = array(convexhull_1_ineq)[:, 0:2]
vec_C_cur = -array(convexhull_1_ineq)[:, 2]
for k in range(4):
mat_Info_Point_i = constrainedCurve.waypointAtIndex(k)
vec_C_temp = vec_C_cur - array_C_cur.dot(mat_Info_Point_i.c())
arr_C_temp = dot(array_C_cur, mat_Info_Point_i.B())
array_CB = np.vstack([array_CB, arr_C_temp])
vec_CB = np.hstack([vec_CB, vec_C_temp])
# overlapped space
constrainedCurve = piecewiseCurve.curve_at_index(1)
for i in range(2):
if i == 0:
array_C_cur = array(convexhull_1_ineq)[:, 0:2]
vec_C_cur = -array(convexhull_1_ineq)[:, 2]
else:
array_C_cur = array(convexhull_2_ineq)[:, 0:2]
vec_C_cur = -array(convexhull_2_ineq)[:, 2]
for k in range(4):
print array_C_cur
mat_Info_Point_i = constrainedCurve.waypointAtIndex(k)
vec_C_temp = vec_C_cur - array_C_cur.dot(mat_Info_Point_i.c())
arr_C_temp = dot(array_C_cur, mat_Info_Point_i.B())
array_CB = np.vstack([array_CB, arr_C_temp])
vec_CB = np.hstack([vec_CB, vec_C_temp])
# right side
constrainedCurve = piecewiseCurve.curve_at_index(2)
array_C_cur = array(convexhull_2_ineq)[:, 0:2]
vec_C_cur = -array(convexhull_2_ineq)[:, 2]
for k in range(4):
mat_Info_Point_i = constrainedCurve.waypointAtIndex(k)
vec_C_temp = vec_C_cur - array_C_cur.dot(mat_Info_Point_i.c())
arr_C_temp = dot(array_C_cur, mat_Info_Point_i.B())
array_CB = np.vstack([array_CB, arr_C_temp])
vec_CB = np.hstack([vec_CB, vec_C_temp])
ineqMatrix = array_CB
ineqVector = vec_CB
res = quadprog_solve_qp(costAb.cost.A,
costAb.cost.b,
G=ineqMatrix,
h=ineqVector)
fitBezier = variableBezier.evaluate(res.reshape((-1, 1)))
#now plotting the obtained curve, in red the concerned part
piecewiseFit = fitBezier.split(array([[0.3, 0.6]]).T)
plotBezier2D(piecewiseFit.curve_at_index(0), ax=ax_mouse, color="b")
plotBezier2D(piecewiseFit.curve_at_index(1), ax=ax_mouse, color="r")
plotBezier2D(piecewiseFit.curve_at_index(2), ax=ax_mouse, color="b")
plt.draw()
def CreateBeizierChull(p_s, p_g, list_Chull):
#problem setting
dim = 2 # dimension = 3, 3D
refDegree = 3 # degree = 3, polynomial we have to get 4 control points (start,end,c1,c2)
pD = problem_definition(dim)
pD.degree = refDegree
pD.init_pos = p_s
pD.end_pos = p_g
pD.flag = constraint_flag.INIT_POS | constraint_flag.END_POS #| constraint_flag.INIT_VEL #| constraint_flag.END_VEL
# cost_Ab.cost.A & cost_Ab.cost.b
cost_Ab = generate_integral_problem(pD, integral_cost_flag.ACCELERATION)
problem = setup_control_points(pD)
variableBezier = problem.bezier()
problemSize = problem.numVariables * dim # dimension * control points
# convex hulls
n_Chulls = len(list_Chull)
n_Splits = n_Chulls - 1
nConvexEq = list_Chull[0].equations.shape[0]
print nConvexEq
# split the original curve
arr_T_intv = makeTimeInterval(n_Chulls, n_Splits)
piecewiseCurve = variableBezier.split(arr_T_intv)
# Equality Equation Matrix
# n_eq_rows = n_Chulls*nConvexEq*(refDegree+1) + refDegree*dim
# n_eq_cols = problemSize
# ineqMatrix = zeros((n_eq_rows,n_eq_cols))
# ineqVector = zeros(n_eq_rows)
array_CB = np.array([], dtype=float).reshape(0, problemSize)
vec_CB = []
# assign the convexhull constraints
for i in range(n_Chulls):
k = i
constrainedCurve = piecewiseCurve.curve_at_index(k)
array_CB, vec_CB = stackInEqConstOfChull(array_CB, vec_CB,
constrainedCurve,
list_Chull[i].equations)
# assign the derivative cosntraints
dev_Bezier = variableBezier.compute_derivate(1)
print 'B_t'
print variableBezier.nbWaypoints
print 'B_dot_t'
print dev_Bezier.nbWaypoints
vecDesC = zeros(2)
des_dev_x = 100.0
des_dev_y = 100.0
for i in range(refDegree):
matBc = dev_Bezier.waypointAtIndex(i)
if (i == 0):
des_dev_x = 4.0
# if(i == 1):
# des_dev_x = -0.1
if (i == 2):
des_dev_x = 2.0
array_CB = np.vstack([array_CB, matBc.B()])
vecDesC[0] = des_dev_x - matBc.c()[0]
vecDesC[1] = des_dev_y - matBc.c()[1]
vec_CB = np.hstack([vec_CB, vecDesC])
# assign final matrix
ineqMatrix = array_CB
ineqVector = vec_CB
#
res = quadprog_solve_qp(cost_Ab.cost.A,
cost_Ab.cost.b,
G=ineqMatrix,
h=ineqVector)
fitBezier = variableBezier.evaluate(res.reshape((-1, 1)))
#now plotting the obtained curve, in red the concerned part
piecewiseFit = fitBezier.split(arr_T_intv)
plotBezier2D(piecewiseFit.curve_at_index(0),
ax=ax_mouse,
color="b",
showControlPoints=True)
plotBezier2D(piecewiseFit.curve_at_index(1),
ax=ax_mouse,
color="r",
showControlPoints=True)
plotBezier2D(piecewiseFit.curve_at_index(2),
ax=ax_mouse,
color="b",
showControlPoints=True)
#
# ax_dev = fig.
dev_fitBezier = fitBezier.compute_derivate(1)
plotBezier2D(dev_fitBezier, ax=ax_dev, color="g", showControlPoints=True)
plt.show()
def createBezierChullwithSlack(p_s, p_g, list_Chull, list_Chull_dev):
# problem setting
dim = len(p_s) # dimension = 3, 3D
refDegree = 3 # degree = 3, polynomial we have to get 4 control points (start,end,c1,c2)
pD = problem_definition(dim)
pD.degree = refDegree
pD.init_pos = p_s
pD.end_pos = p_g
# pD.flag = constraint_flag.INIT_POS | constraint_flag.END_POS #| constraint_flag.INIT_VEL #| constraint_flag.END_VEL
#cost_Ab.cost.A & cost_Ab.cost.b
cost_Ab = generate_integral_problem(pD, integral_cost_flag.ACCELERATION)
problem = setup_control_points(pD)
variableBezier = problem.bezier()
problemSize = problem.numVariables * dim # dimension * control points
# #changing to solve slack variable
num_slack = 1
mat_origin = cost_Ab.cost.A
vec_origin = cost_Ab.cost.b
newCostP = np.zeros(
(mat_origin.shape[0] + num_slack, mat_origin.shape[1] + num_slack))
newCostP[0:mat_origin.shape[0], :mat_origin.shape[1]] = mat_origin
newCostP[newCostP.shape[0] - num_slack,
newCostP.shape[1] - num_slack] = 1e-4
newCostq = np.zeros(len(vec_origin) + num_slack)
newCostq[:len(vec_origin)] = vec_origin
newCostq[-1] = -1e+5
# # convex hulls
n_Chulls = len(list_Chull)
n_Splits = n_Chulls - 1
# split the original curve
arr_T_intv = makeTimeInterval(n_Chulls, n_Splits)
piecewiseCurve = variableBezier.split(arr_T_intv)
print arr_T_intv
# set Inequality Constraints
array_CB = np.array([], dtype=float).reshape(0, problemSize)
vec_CB = []
# print array_CB.shape
# assign the convexhull constraints
for i in range(n_Chulls):
k = i
constrainedCurve = piecewiseCurve.curve_at_index(k)
array_CB, vec_CB = stackInEqConstOfChull(array_CB, vec_CB,
constrainedCurve,
list_Chull[i].equations)
array_CB, vec_CB = normalizeMatAndVec(array_CB, vec_CB)
# #slack vector
# vec_slack = np.zeros((array_CB.shape[0],1))
# vec_slack.fill(0)
# vec_slack[36:48] =1
# vec_slack[48:60] =1
# array_CB = np.hstack([array_CB,vec_slack])
# vec_slack2 = np.zeros((array_CB.shape[1]))
# vec_slack2[-1] = -1
# array_CB = np.vstack([array_CB,vec_slack2])
# new_vec_CB = np.zeros((array_CB.shape[0]))
# new_vec_CB[:len(vec_CB)] = vec_CB
# new_vec_CB[-1] = 0
# vec_CB = new_vec_CB
# print array_CB.shape
# print vec_CB.shape
#assign final matrix
ineqMatrix = array_CB
ineqVector = vec_CB
# # set equality constraints
array_CE = np.array([], dtype=float).reshape(0, problemSize)
vec_CE = []
matInfo_start = variableBezier.waypointAtIndex(0)
vec_norm = np.zeros((3, 1))
vec_norm[2] = 1
vec_norm = np.transpose(vec_norm)
print vec_norm.shape
print matInfo_start.B().shape
out3 = vec_norm.dot(matInfo_start.B())
print out3
array_CE = np.vstack([array_CE, out3])
vec_temp = -1 * vec_norm.dot(matInfo_start.c())
vec_CE = np.hstack([vec_CE, vec_temp])
# matInfo_end = variableBezier.waypointAtIndex(variableBezier.nbWaypoints-1)
# array_CE = np.vstack([array_CE,matInfo_end.B()])
# vec_temp = p_g - matInfo_end.c()
# vec_CE = np.hstack([vec_CE,vec_temp])
# array_CE, vec_CE = normalizeMatAndVec(array_CE,vec_CE)
# print array_CE.shape
# print vec_CE.shape
# #
# # constrainedCurve = piecewiseCurve.curve_at_index(0)
# # array_C_cur = np.array(list_Chull[0].equations)[:,0:(dim)]
# # vec_C_cur = -np.array(list_Chull[0].equations)[:,(dim)]
# # mat_Info_Point_i = constrainedCurve.waypointAtIndex(2)
# # arr_C_temp,vec_C_temp = createInEqConst(mat_Info_Point_i,False,array_C_cur,vec_C_cur)
# # array_CE = np.vstack([array_CE,arr_C_temp])
# # vec_CE = np.hstack([vec_CE,vec_C_temp])
# # constrainedCurve = piecewiseCurve.curve_at_index(1)
# # array_C_cur = np.array(list_Chull[1].equations)[:,0:(dim)]
# # vec_C_cur = -np.array(list_Chull[1].equations)[:,(dim)]
# # mat_Info_Point_i = constrainedCurve.waypointAtIndex(0)
# # arr_C_temp,vec_C_temp = createInEqConst(mat_Info_Point_i,False,array_C_cur,vec_C_cur)
# # array_CE = np.vstack([array_CE,arr_C_temp])
# # vec_CE = np.hstack([vec_CE,vec_C_temp])
# print array_CE.shape
# print vec_CE.shape
# # array_CE, vec_CE = normalizeMatAndVec(array_CE,vec_CE)
# vec_slack = np.zeros((array_CE.shape[0],1))
# vec_slack.fill(0)
# for i in range(6):
# vec_slack[i]=0
# array_CE = np.hstack([array_CE,vec_slack])
eqMatrix = array_CE
eqVector = vec_CE
array_C_cur = np.array(list_Chull[0].equations)[:, 0:(dim)]
vec_C_cur = -np.array(list_Chull[0].equations)[:, (dim)]
print array_C_cur
print vec_C_cur
# #
res = quadprog_solve_qp(mat_origin,
vec_origin,
G=ineqMatrix,
h=ineqVector,
C=eqMatrix,
d=eqVector)
fitBezier = variableBezier.evaluate(res.reshape((-1, 1)))
piecewiseFit = fitBezier.split(arr_T_intv)
plotBezier(piecewiseFit.curve_at_index(0),
ax=ax,
linewidth=4.,
color="b",
showControlPoints=True)
plotBezier(piecewiseFit.curve_at_index(1),
ax=ax,
linewidth=4.,
color="r",
showControlPoints=True)
def createBezierChull(p_s, p_g, list_Chull, list_Chull_dev):
# problem setting
dim = len(p_s) # dimension = 3, 3D
refDegree = 3 # degree = 3, polynomial we have to get 4 control points (start,end,c1,c2)
pD = problem_definition(dim)
pD.degree = refDegree
#cost_Ab.cost.A & cost_Ab.cost.b
cost_Ab = generate_integral_problem(pD, integral_cost_flag.ACCELERATION)
problem = setup_control_points(pD)
variableBezier = problem.bezier()
problemSize = problem.numVariables * dim # dimension * control points
# convex hulls
n_Chulls = len(list_Chull)
n_Splits = n_Chulls - 1
# split the original curve
arr_T_intv = makeTimeInterval(n_Chulls, n_Splits)
piecewiseCurve = variableBezier.split(arr_T_intv)
print arr_T_intv
# set Inequality Constraints
array_CB = np.array([], dtype=float).reshape(0, problemSize)
vec_CB = []
# assign the convexhull constraints
for i in range(n_Chulls):
k = i
constrainedCurve = piecewiseCurve.curve_at_index(k)
array_CB, vec_CB = stackInEqConstOfChull(array_CB, vec_CB,
constrainedCurve,
list_Chull[i].equations)
# assign the derivative cosntraints
n_Chulls_dev = len(list_Chull_dev)
dev_Bezier = variableBezier.compute_derivate(1)
# array_CB, vec_CB = stackInEqConstOfChull_realtive(array_CB,vec_CB,variableBezier,p_s,list_Chull_dev[0].equations)
#assign final matrix
ineqMatrix = array_CB
ineqVector = vec_CB
# set equality constraints
array_CE = np.array([], dtype=float).reshape(0, problemSize)
vec_CE = []
matInfo_start = variableBezier.waypointAtIndex(0)
array_CE = np.vstack([array_CE, matInfo_start.B()])
vec_temp = p_s - matInfo_start.c()
vec_CE = np.hstack([vec_CE, vec_temp])
matInfo_end = variableBezier.waypointAtIndex(variableBezier.nbWaypoints -
1)
array_CE = np.vstack([array_CE, matInfo_end.B()])
vec_temp = p_g - matInfo_end.c()
vec_CE = np.hstack([vec_CE, vec_temp])
eqMatrix = array_CE
eqVector = vec_CE
#
res = quadprog_solve_qp(cost_Ab.cost.A,
cost_Ab.cost.b,
C=eqMatrix,
d=eqVector,
G=ineqMatrix,
h=ineqVector)
fitBezier = variableBezier.evaluate(res.reshape((-1, 1)))
piecewiseFit = fitBezier.split(arr_T_intv)
plotBezier(piecewiseFit.curve_at_index(0), ax=ax, linewidth=4., color="b")
plotBezier(piecewiseFit.curve_at_index(1), ax=ax, linewidth=4., color="r")
y = np.array([wp[1] for wp in points_sampled])
z = np.array([wp[2] for wp in points_sampled])
ax.scatter(x, y, z, color="r")
#---------------
#-2.-----optimizing the beizer curve following sampled points
from curves.optimization import (problem_definition, setup_control_points)
#dimension of our problem (here 3 as our curve is 3D)
dim = 3 # dimension = 3, 3D
refDegree = 3 # degree = 3, polynomial we have to get 4 control points (start,end,c1,c2)
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()
#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(),