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(),