Mixed Integer programming is crazy powerful. You can with ingenuity encode many problems into it. The following is a simplification of the ideas appearing in http://groups.csail.mit.edu/robotics-center/public_papers/Dai19.pdf . They do 3d robot arms, I do 2d. I also stick to completely linear approximations.

The surface of a circle is not a convex shape. If you include the interior of a circle it is. You can build a good approximation to the circle as polygons. A polygon is the union of it’s sides, each of which is a line segment. Line sgements are convex set. Unions of convex sets are encodable using mixed integer programming. What I do is sample N regular positions on the surface of a circle. These are the vertices of my polygon. Then I build boolean indicator variables for which segment we are on. Only one of them is be nonzero . If we are on a segment, we are allowed to make positions that interpolate between the endpoints of that segment , where and . These are only allowed to be nonzero if we are on the segment, so we suppress them with the indicator variables . That’s the gist of it.

Given a point on the circle (basically sines and cosines of an angle) we can build a 2d rotation matrix from it. Then we can write down the equations connecting subsequent links on the arm. . By using global rotations with respect to the world frame, these equations stay linear. That is a subtle point. and are variables, whereas is a constant describing the geometry of the robot arm. If we instead used rotation matrices connecting frame i to i+1 these R matrices would compound nonlinearly.

All in all, pretty cool!

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 |
import cvxpy as cvx import numpy as np import matplotlib.pyplot as plt # builds a N sided polygon approximation of a circle for MIP. It is the union of the segments making up the polygon # might also be useful to directly encode arcs. for joint constraint limits. def circle(N): x = cvx.Variable() y = cvx.Variable() l = cvx.Variable(N) #interpolation variables segment = cvx.Variable(N,boolean=True) #segment indicator variables, relaxing the boolean constraint gives the convex hull of the polygon angles = np.linspace(0, 2*np.pi, N, endpoint=False) xs = np.cos(angles) #we're using a VRep ys = np.sin(angles) constraints = [] constraints += [x == l*xs, y == l*ys] # x and y are convex sum of the corner points constraints += [cvx.sum(l) == 1, l <= 1, 0 <= l] #interpolations variables. Between 0 and 1 and sum up to 1 constraints += [cvx.sum(segment) == 1] # only one indicator variable can be nonzero constraints += [l[N-1] <= segment[N-1] + segment[0]] #special wrap around case for i in range(N-1): constraints += [l[i] <= segment[i] + segment[i+1]] # interpolation variables suppressed return x, y, constraints x, y, constraints = circle(8) objective = cvx.Maximize(x-0.8*y) prob = cvx.Problem(objective, constraints) res = prob.solve(solver=cvx.GLPK_MI, verbose=True) # build a 2d rotation matrix using circle def R(N): constraints = [] c, s, constraint = circle(N) # get cosines and sines from a circle constraints += constraint r = cvx.Variable((2,2)) # build rotation matrix constraints += [r[0,0] == c, r[0,1] == s] constraints += [r[1,0] == -s, r[1,1] == c] return r, constraints # np.array([[c , s], [-s, c]]) #robot linkage of differing arm length link_lengths = [0.5,0.2,0.3,0.4] pivots = [] Rs = [] N = 8 constraints = [] origin = np.zeros(2) p1 = origin for l in link_lengths: R1, c = R(8) constraints += c p2 = cvx.Variable(2) constraints += [p2 == p1 + R1*np.array([l,0])] # R1 is global rotation with respect to world frame. This is important. It is what makes the encoding linear. p1 = p2 pivots.append(p2) Rs.append(R1) end_position = np.array([-0.5, .7]) constraints += [p2 == end_position] objective = cvx.Maximize(1) prob = cvx.Problem(objective, constraints) res = prob.solve(solver=cvx.GLPK_MI, verbose=True) print(res) #print(R(x,y)) print(p2.value) print(list(map(lambda r: r.value, Rs))) #print(y.value) p1 = origin for l, r in zip(link_lengths, Rs): p2 = p1 + r.value@np.array([l,0]) plt.plot([p1[0],p2[0]], [p1[1],p2[1]], marker='o'), p1 = p2 plt.show() ''' print(xs) print(ys) print(angles) print(l.value) print(segment.value) plt.plot(x.value, label='x') plt.plot(v.value, label= 'v') plt.plot(collision.value, label = 'collision bool') plt.legend() plt.xlabel('time') plt.show() ''' |