def SOS_compute_3(self, K, l_coeff, rho_max=10.):
prog = MathematicalProgram()
# fix u and lbda, search for V and rho
x = prog.NewIndeterminates(2, "x")
# get lbda from before
l = np.array([x[1], x[0], 1])
lbda = l.dot(np.dot(l_coeff, l))
# rho is decision variable now
rho = prog.NewContinuousVariables(1, "rho")[0]
# create lyap V
s = prog.NewContinuousVariables(4, "s")
S = np.array([[s[0], s[1]], [s[2], s[3]]])
V = x.dot(np.dot(S, x))
Vdot = Jacobian([V], x).dot(self.dynamics_K(x, K))[0]
prog.AddSosConstraint(V)
prog.AddSosConstraint(-Vdot - lbda * (rho - V))
prog.AddLinearCost(-rho)
prog.AddLinearConstraint(rho <= rho_max)
prog.Solve()
rho = prog.GetSolution(rho)
s = prog.GetSolution(s)
return s, rho
def SOS_compute_2(self, l_coeff, S, rho_max=10.):
prog = MathematicalProgram()
# fix V and lbda, searcu for u and rho
x = prog.NewIndeterminates(2, "x")
# get lbda from before
l = np.array([x[1], x[0], 1])
lbda = l.dot(np.dot(l_coeff, l))
# Define u
K = prog.NewContinuousVariables(2, "K")
# Fixed Lyapunov
V = x.dot(np.dot(S, x))
Vdot = Jacobian([V], x).dot(self.dynamics_K(x, K))[0]
# rho is decision variable now
rho = prog.NewContinuousVariables(1, "rho")[0]
prog.AddSosConstraint(-Vdot - lbda * (rho - V))
prog.AddLinearConstraint(rho <= rho_max)
prog.AddLinearCost(-rho)
prog.Solve()
rho = prog.GetSolution(rho)
K = prog.GetSolution(K)
return rho, K
def run_simple_mathematical_program():
print "\n\nsimple mathematical program"
mp = MathematicalProgram()
x = mp.NewContinuousVariables(1, "x")
mp.AddLinearCost(x[0] * 1.0)
mp.AddLinearConstraint(x[0] >= 1)
print mp.Solve()
print mp.GetSolution(x)
print "(finished) simple mathematical program"
# no need to change it, but if you really want to,
# keep l_deg even and do not set l_deg greater than 10
l_deg = 4
assert l_deg % 2 == 0
# SOS Lagrange multipliers
l = prog2.NewSosPolynomial(Variables(x), l_deg)[0].ToExpression()
# level set as optimization variable
rho = prog2.NewContinuousVariables(1, 'rho')[0]
# write here the SOS condition described in the "Not quite there yet..." section above
prog2.AddSosConstraint(x.dot(x)*(V-rho) - l*V_dot)
# insert here the objective function (maximize rho)
prog2.AddLinearCost(-rho)
# solve program only if the lines above are filled
if len(prog2.GetAllConstraints()) != 0:
# solve SOS program
result = Solve(prog2)
# get maximum rho
assert result.is_success()
rho_method_2 = result.GetSolution(rho)
# print maximum rho
print(f'Method 2 verified rho = {rho_method_2}.')
"""## Method 3: Smarter Single-Shot SOS Program
def PlanGraspPoints(self):
# First, extract the bounding geometry of the object.
# Generally, our geometry is coming from 3d models, so
# we have to do some legwork to extract 2D geometry. For
# the examples we'll use in this set, we'll assume
# that extracting the convex hull of the first visual element
# is a good representation of the object geometry. (This is
# not great! But it'll do the job for us, since we're going
# to experiment with only simple objects.)
kinsol = self.hand.doKinematics(
self.x_nom[0:self.hand.get_num_positions()])
self.manipuland_link_index = \
self.hand.FindBody(self.manipuland_link_name).get_body_index()
body = self.hand.get_body(self.manipuland_link_index)
# For projecting into XY plane
Tview = np.array([[1., 0., 0., 0.], [0., 1., 0., 0.], [0., 0., 0.,
1.]])
all_points = ExtractPlanarObjectGeometryConvexHull(body, Tview)
# For later use: precompute the fingertip positions of the
# robot in the nominal posture.
nominal_fingertip_points = np.empty((2, self.num_fingers))
for i in range(self.num_fingers):
nominal_fingertip_points[:, i] = self.hand.transformPoints(
kinsol, self.fingertip_position, self.finger_link_indices[i],
0)[0:2, 0]
# Search for an optimal grasp with N points
random.seed(42)
np.random.seed(42)
def get_random_point_and_normal_on_surface(all_points):
num_points = all_points.shape[1]
i = random.choice(range(num_points - 1))
first_point = np.asarray([all_points[0][i], all_points[1][i]])
second_point = np.asarray(
[all_points[0][i + 1], all_points[1][i + 1]])
first_to_second = second_point - first_point
# Clip to avoid interpolating close to a corner
interpolate_param = np.clip(np.random.rand(), 0.2, 0.8)
rand_point = first_point + interpolate_param * first_to_second
normal = np.array([-first_to_second[1], first_to_second[0]]) \
/ np.linalg.norm(first_to_second)
return rand_point, normal
best_conv_volume = 0
best_points = []
best_normals = []
best_finger_assignments = []
for i in range(self.n_grasp_search_iters):
grasp_points = []
normals = []
for j in range(self.num_fingers):
point, normal = \
get_random_point_and_normal_on_surface(all_points)
# check for duplicates or close points -- fingertip
# radius is 0.2, so require twice that margin to avoid
# intersection fingertips.
num_rejected = 0
while min([1.0] + [
np.linalg.norm(grasp_point - point, 2)
for grasp_point in grasp_points
]) <= 0.4:
point, normal = \
get_random_point_and_normal_on_surface(all_points)
num_rejected += 1
if num_rejected > 10000:
print "Rejected 10000 points in a row due to crowding." \
" Your object is a bit too small for your number of" \
" fingers."
break
grasp_points.append(point)
normals.append(normal)
if achieves_force_closure(grasp_points, normals, self.mu):
# Test #1: Rank in terms of convex hull volume of grasp points
volume = compute_convex_hull_volume(grasp_points)
if volume < best_conv_volume:
continue
# Test #2: Does IK work for this point?
self.grasp_points = grasp_points
self.grasp_normals = normals
# Pick optimal finger assignment that
# minimizes distance between fingertips in the
# nominal posture, and the chosen grasp points
grasp_points_world = self.transform_grasp_points_manipuland(
self.x_nom)[0:2, :]
prog = MathematicalProgram()
# We'd *really* like these to be binary variables,
# but unfortunately don't have a MIP solver available in the
# course docker container. Instead, we'll solve an LP,
# and round the result to the nearest feasible integer
# solutions. Intuitively, this LP should probably reach its
# optimal value at an extreme point (where the variables
# all take integer value). I've not yet observed this
# not occuring in practice!
assignment_vars = prog.NewContinuousVariables(
self.num_fingers, self.num_fingers, "assignment")
for grasp_i in range(self.num_fingers):
# Every row and column of assignment vars sum to one --
# each finger matches to one grasp position
prog.AddLinearConstraint(
np.sum(assignment_vars[:, grasp_i]) == 1.)
prog.AddLinearConstraint(
np.sum(assignment_vars[grasp_i, :]) == 1.)
for finger_i in range(self.num_fingers):
# If this grasp assignment is active,
# add a cost equal to the distance between
# nominal pose and grasp position
prog.AddLinearCost(
assignment_vars[grasp_i, finger_i] *
np.linalg.norm(grasp_points_world[:, grasp_i] -
nominal_fingertip_points[:,
finger_i]))
prog.AddBoundingBoxConstraint(
0., 1., assignment_vars[grasp_i, finger_i])
result = Solve(prog)
assignments = result.GetSolution(assignment_vars)
# Round assignments to nearest feasible set
claimed = [False] * self.num_fingers
for grasp_i in range(self.num_fingers):
order = np.argsort(assignments[grasp_i, :])
fill_i = self.num_fingers - 1
while claimed[order[fill_i]] is not False:
fill_i -= 1
if fill_i < 0:
raise Exception("Finger association failed. "
"Horrible bug. Tell Greg")
assignments[grasp_i, :] *= 0.
assignments[grasp_i, order[fill_i]] = 1.
claimed[order[fill_i]] = True
# Populate actual assignments
self.grasp_finger_assignments = []
for grasp_i in range(self.num_fingers):
for finger_i in range(self.num_fingers):
if assignments[grasp_i, finger_i] == 1.:
self.grasp_finger_assignments.append(
(finger_i, np.array(self.fingertip_position)))
continue
qinit, info = self.ComputeTargetPosture(
self.x_nom,
self.x_nom[(self.nq - 3):self.nq],
backoff_distance=0.2)
if info != 1:
continue
best_conv_volume = volume
best_points = grasp_points
best_normals = normals
best_finger_assignments = self.grasp_finger_assignments
if len(best_points) == 0:
print "After %d attempts, couldn't find a good grasp "\
"for this object." % self.n_grasp_search_iters
print "Proceeding with a horrible random guess."
best_points = [
np.random.uniform(-1., 1., 2) for i in range(self.num_fingers)
]
best_normals = [
np.random.uniform(-1., 1., 2) for i in range(self.num_fingers)
]
best_finger_assignments = [(i, self.fingertip_position)
for i in range(self.num_fingers)]
self.grasp_points = best_points
self.grasp_normals = best_normals
self.grasp_finger_assignments = best_finger_assignments
def MILP_compute_traj(self,
obst_idx,
x_out,
y_out,
dx,
dy,
pose_initial=[0., 0.]):
'''
Find trajectory with MILP
Outputs trajectory (waypoints) and new K for control
'''
mp = MathematicalProgram()
N = 8
k = 0
# define state traj
st = mp.NewContinuousVariables(2, "state_%d" % k)
# # binary variables for obstalces constraint
c = mp.NewBinaryVariables(4 * self.ang_discret, "c_%d" % k)
obs = c
states = st
for k in range(1, N):
st = mp.NewContinuousVariables(2, "state_%d" % k)
states = np.vstack((states, st))
c = mp.NewBinaryVariables(4 * self.ang_discret, "c_%d" % k)
obs = np.vstack((obs, c))
st = mp.NewContinuousVariables(2, "state_%d" % (N + 1))
states = np.vstack((states, st))
c = mp.NewBinaryVariables(4 * self.ang_discret, "c_%d" % k)
obs = np.vstack((obs, c))
# variables encoding max x y dist from obstacle
x_margin = mp.NewContinuousVariables(1, "x_margin")
y_margin = mp.NewContinuousVariables(1, "y_margin")
### define cost
for i in range(N):
mp.AddLinearCost(-states[i, 0]) # go as far forward as possible
mp.AddLinearCost(-states[-1, 0])
mp.AddLinearCost(-x_margin[0])
mp.AddLinearCost(-y_margin[0])
# bound x y margin so it doesn't explode
mp.AddLinearConstraint(x_margin[0] <= 3.)
mp.AddLinearConstraint(y_margin[0] <= 3.)
# x y is non ngative adn at least above robot radius
mp.AddLinearConstraint(x_margin[0] >= 0.05)
mp.AddLinearConstraint(y_margin[0] >= 0.05)
M = 1000 # slack var for integer things
# state constraint
for i in range(2): # initial state constraint x y
mp.AddLinearConstraint(states[0, i] <= pose_initial[i])
mp.AddLinearConstraint(states[0, i] >= pose_initial[i])
for i in range(N):
mp.AddQuadraticCost((states[i + 1, 1] - states[i, 1])**2)
mp.AddLinearConstraint(states[i + 1, 0] <= states[i, 0] + dx)
mp.AddLinearConstraint(states[i + 1, 0] >= states[i, 0] - dx)
mp.AddLinearConstraint(states[i + 1, 1] <= states[i, 1] + dy)
mp.AddLinearConstraint(states[i + 1, 1] >= states[i, 1] - dy)
# obstacle constraint
for j in range(self.ang_discret - 1):
mp.AddLinearConstraint(sum(obs[i, 4 * j:4 * j + 4]) <= 3)
ang_min = self.angles[j] # lower angle bound of obstacle
ang_max = self.angles[j + 1] # higher angle bound of obstaclee
if int(obst_idx[j]) < (self.rng_discret -
1): # less than max range measured
rng_min = self.ranges[int(
obst_idx[j])] # where the obst is at at this angle
rng_max = self.ranges[int(obst_idx[j] + 1)]
mp.AddLinearConstraint(
states[i, 0] <= rng_min - x_margin[0] +
M * obs[i, 4 * j]) # xi <= xobs,low + M*c
mp.AddLinearConstraint(
states[i, 0] >= rng_max + x_margin[0] -
M * obs[i, 4 * j + 1]) # xi >= xobs,high - M*c
mp.AddLinearConstraint(
states[i, 1] <=
states[i, 0] * np.tan(ang_min) - y_margin[0] +
M * obs[i, 4 * j + 2]) # yi <= xi*tan(ang,min) + M*c
mp.AddLinearConstraint(
states[i, 1] >=
states[i, 0] * np.tan(ang_max) + y_margin[0] -
M * obs[i, 4 * j + 3]) # yi >= ci*tan(ang,max) - M*c
# obstacle constraint for last state
for j in range(self.ang_discret - 1):
mp.AddLinearConstraint(sum(obs[N, 4 * j:4 * j + 4]) <= 3)
ang_min = self.angles[j] # lower angle bound of obstacle
ang_max = self.angles[j + 1] # higher angle bound of obstaclee
if int(obst_idx[j]) < (self.rng_discret -
1): # less than max range measured
rng_min = self.ranges[int(
obst_idx[j])] # where the obst is at at this angle
rng_max = self.ranges[int(obst_idx[j] + 1)]
mp.AddLinearConstraint(
states[N, 0] <= rng_min - x_margin[0] +
M * obs[N, 4 * j]) # xi <= xobs,low + M*c
mp.AddLinearConstraint(
states[N, 0] >= rng_max + x_margin[0] -
M * obs[N, 4 * j + 1]) # xi >= xobs,high - M*c
mp.AddLinearConstraint(
states[N,
1] <= states[N, 0] * np.tan(ang_min) - y_margin[0] +
M * obs[N, 4 * j + 2]) # yi <= xi*tan(ang,min) + M*c
mp.AddLinearConstraint(
states[N,
1] >= states[N, 0] * np.tan(ang_max) + y_margin[0] -
M * obs[N, 4 * j + 3]) # yi >= ci*tan(ang,max) - M*c
mp.Solve()
trajectory = mp.GetSolution(states)
xm = mp.GetSolution(x_margin)
ym = mp.GetSolution(y_margin)
x_out[:] = trajectory[:, 0]
y_out[:] = trajectory[:, 1]
return trajectory, xm[0], ym[0]
def compute_trajectory(self,
obst_idx,
x_out,
y_out,
ux_out,
uy_out,
pose_initial=[0., 0., 0., 0.],
dt=0.05):
'''
Find trajectory with MILP
input u are tyhe velocities (xd, yd)
dt 0.05 according to a rate of 20 Hz
'''
mp = MathematicalProgram()
N = 30
k = 0
# define input trajectory and state traj
u = mp.NewContinuousVariables(2, "u_%d" % k) # xd yd
input_trajectory = u
st = mp.NewContinuousVariables(4, "state_%d" % k)
# # binary variables for obstalces constraint
c = mp.NewBinaryVariables(4 * self.ang_discret, "c_%d" % k)
obs = c
states = st
for k in range(1, N):
u = mp.NewContinuousVariables(2, "u_%d" % k)
input_trajectory = np.vstack((input_trajectory, u))
st = mp.NewContinuousVariables(4, "state_%d" % k)
states = np.vstack((states, st))
c = mp.NewBinaryVariables(4 * self.ang_discret, "c_%d" % k)
obs = np.vstack((obs, c))
st = mp.NewContinuousVariables(4, "state_%d" % (N + 1))
states = np.vstack((states, st))
c = mp.NewBinaryVariables(4 * self.ang_discret, "c_%d" % k)
obs = np.vstack((obs, c))
### define cost
mp.AddLinearCost(100 *
(-states[-1, 0])) # go as far forward as possible
# mp.AddQuadraticCost(states[-1,1]*states[-1,1])
# time constraint
M = 1000 # slack var for obst costraint
# state constraint
for i in range(2): # initial state constraint x y yaw
mp.AddLinearConstraint(states[0, i] <= pose_initial[i])
mp.AddLinearConstraint(states[0, i] >= pose_initial[i])
for i in range(2): # initial state constraint xd yd yawd
mp.AddLinearConstraint(states[0, i] <= pose_initial[2 + i] + 1)
mp.AddLinearConstraint(states[0, i] >= pose_initial[2 + i] - 1)
for i in range(N):
# state update according to dynamics
state_next = self.quad_dynamics(states[i, :],
input_trajectory[i, :], dt)
for j in range(4):
mp.AddLinearConstraint(states[i + 1, j] <= state_next[j])
mp.AddLinearConstraint(states[i + 1, j] >= state_next[j])
# obstacle constraint
for j in range(self.ang_discret - 1):
mp.AddLinearConstraint(sum(obs[i, 4 * j:4 * j + 4]) <= 3)
ang_min = self.angles[j] # lower angle bound of obstacle
ang_max = self.angles[j + 1] # higher angle bound of obstaclee
if int(obst_idx[j]) < (self.rng_discret -
1): # less than max range measured
rng_min = self.ranges[int(
obst_idx[j])] # where the obst is at at this angle
rng_max = self.ranges[int(obst_idx[j] + 1)]
mp.AddLinearConstraint(
states[i, 0] <= rng_min - 0.05 +
M * obs[i, 4 * j]) # xi <= xobs,low + M*c
mp.AddLinearConstraint(
states[i, 0] >= rng_max + 0.005 -
M * obs[i, 4 * j + 1]) # xi >= xobs,high - M*c
mp.AddLinearConstraint(
states[i, 1] <= states[i, 0] * np.tan(ang_min) - 0.05 +
M * obs[i, 4 * j + 2]) # yi <= xi*tan(ang,min) + M*c
mp.AddLinearConstraint(
states[i, 1] >= states[i, 0] * np.tan(ang_max) + 0.05 -
M * obs[i, 4 * j + 3]) # yi >= ci*tan(ang,max) - M*c
# environmnt constraint, dont leave fov
mp.AddLinearConstraint(
states[i, 1] >= states[i, 0] * np.tan(-self.theta))
mp.AddLinearConstraint(
states[i, 1] <= states[i, 0] * np.tan(self.theta))
# bound the inputs
# mp.AddConstraint(input_trajectory[i,:].dot(input_trajectory[i,:]) <= 2.5*2.5) # dosnt work with multi int
mp.AddLinearConstraint(input_trajectory[i, 0] <= 2.5)
mp.AddLinearConstraint(input_trajectory[i, 0] >= -2.5)
mp.AddLinearConstraint(input_trajectory[i, 1] <= 0.5)
mp.AddLinearConstraint(input_trajectory[i, 1] >= -0.5)
mp.Solve()
input_trajectory = mp.GetSolution(input_trajectory)
trajectory = mp.GetSolution(states)
x_out[:] = trajectory[:, 0]
y_out[:] = trajectory[:, 1]
ux_out[:] = input_trajectory[:, 0]
uy_out[:] = input_trajectory[:, 1]
return trajectory, input_trajectory
def linear_program(f, A, b, C=None, d=None, tol=1.e-5, **kwargs):
"""
Solves the linear program min_x f^T x s.t. A x <= b, C x = d.
Arguments
----------
f : numpy.ndarray
Gradient of the cost function.
A : numpy.ndarray
Left-hand side of the inequality constraints.
b : numpy.ndarray
Right-hand side of the inequality constraints.
C : numpy.ndarray
Left-hand side of the equality constraints.
d : numpy.ndarray
Right-hand side of the equality constraints.
tol : float
Maximum value of a residual of an inequality to consider the related constraint active.
Returns
----------
sol : dict
Dictionary with the solution of the LP.
Fields
----------
min : float
Minimum of the LP (None if the problem is unfeasible or unbounded).
argmin : numpy.ndarray
Argument that minimizes the LP (None if the problem is unfeasible or unbounded).
active_set : list of int
Indices of the active inequallities {i | A_i argmin = b} (None if the problem is unfeasible or unbounded).
multiplier_inequality : numpy.ndarray
Lagrange multipliers for the inequality constraints (None if the problem is unfeasible or unbounded).
multiplier_equality : numpy.ndarray
Lagrange multipliers for the equality constraints (None if the problem is unfeasible or unbounded or without equality constraints).
"""
# check equalities
if (C is None) != (d is None):
raise ValueError('missing C or d.')
# problem size
n_ineq, n_x = A.shape
if C is not None:
n_eq = C.shape[0]
else:
n_eq = 0
# build program
prog = MathematicalProgram()
x = prog.NewContinuousVariables(n_x)
[prog.AddLinearConstraint(A[i].dot(x) <= b[i]) for i in range(n_ineq)]
[prog.AddLinearConstraint(C[i].dot(x) == d[i]) for i in range(n_eq)]
prog.AddLinearCost(f.dot(x))
# solve
solver = GurobiSolver()
prog.SetSolverOption(solver.solver_type(), 'OutputFlag', 0)
[prog.SetSolverOption(solver.solver_type(), parameter, value) for parameter, value in kwargs.items()]
result = prog.Solve()
# initialize output
sol = {
'min': None,
'argmin': None,
'active_set': None,
'multiplier_inequality': None,
'multiplier_equality': None
}
if result == SolutionResult.kSolutionFound:
sol['argmin'] = prog.GetSolution(x)
sol['min'] = f.dot(sol['argmin'])
sol['active_set'] = np.where(A.dot(sol['argmin']) - b > -tol)[0].tolist()
# retrieve multipliers through KKT conditions
M = A[sol['active_set']].T
if n_eq > 0:
M = np.hstack((M, C.T))
m = -np.linalg.pinv(M).dot(f)
sol['multiplier_inequality'] = np.zeros(n_ineq)
for i, j in enumerate(sol['active_set']):
sol['multiplier_inequality'][j] = m[i]
if n_eq > 0:
sol['multiplier_equality'] = m[len(sol['active_set']):]
return sol
from pydrake.all import Jacobian, MathematicalProgram, Solve
def dynamics(x):
return -x + x**3
prog = MathematicalProgram()
x = prog.NewIndeterminates(1, "x")
rho = prog.NewContinuousVariables(1, "rho")[0]
# Define the Lyapunov function.
V = x.dot(x)
Vdot = Jacobian([V], x).dot(dynamics(x))[0]
# Define the Lagrange multiplier.
lambda_ = prog.NewContinuousVariables(1, "lambda")[0]
prog.AddConstraint(lambda_ >= 0)
prog.AddSosConstraint((V - rho) * x.dot(x) - lambda_ * Vdot)
prog.AddLinearCost(-rho)
result = Solve(prog)
assert result.is_success()
print("Verified that " + str(V) + " < " + str(result.GetSolution(rho)) +
" is in the region of attraction.")
assert math.fabs(result.GetSolution(rho) - 1) < 1e-5
prog = MathematicalProgram()
# Construct an n-by-n positive semi-definite matrix as the decision
# variables.
num_states = A[0].shape[0]
P = prog.NewSymmetricContinuousVariables(num_states, "P")
prog.AddPositiveSemidefiniteConstraint(P - .01 * np.identity(num_states))
# Add the common Lyapunov conditions.
for i in range(len(A)):
prog.AddPositiveSemidefiniteConstraint(-A[i].transpose().dot(P) -
P.dot(A[i]) -
.01 * np.identity(num_states))
# Add an objective.
prog.AddLinearCost(np.trace(P))
# Run the optimization.
result = Solve(prog)
if result.is_success():
P = result.GetSolution(P)
print("eig(P) =" + str(np.linalg.eig(P)[0]))
for i in range(len(A)):
print("eig(Pdot" + str(i) + ") = " +
str(np.linalg.eig(A[i].transpose().dot(P) + P.dot(A[i]))[0]))
else:
print('Could not find a common Lyapunov function.')
print('This is expected to occur with some probability: not all')
print('random sets of stable matrices will have a common Lyapunov')
print('function.')
def achieves_force_closure(points, normals, mu):
"""
Determines whether the given forces achieve force closure.
Note that each of points and normals are lists of variable
length, but should be the same length.
Here's an example valid set of input args:
points = [np.asarray([0.1, 0.2]), np.asarray([0.5,-0.1])]
normals = [np.asarray([-1.0,0.0]), np.asarray([0.0, 1.0])]
mu = 0.2
achieves_force_closure(points, normals, mu)
NOTE: your normals should be normalized (be unit vectors)
:param points: a list of arrays where each array is the
position of the force points relative to the center of mass.
:type points: a list of 1-d numpy arrays of shape (2,)
:param normals: a list of arrays where each array is the normal
of the local surface of the object, pointing in towards
the object
:type normals: a list of 1-d numpy arrays of shape (2,)
:param mu: coulombic kinetic friction coefficient
:type mu: float, greater than 0.
:return: whether or not the given parameters achieves force closure
:rtype: bool (True or False)
"""
assert len(points) == len(normals)
assert mu >= 0.0
# print('Number of Points: ' + str(len(points)))
from pydrake.all import MathematicalProgram, Solve
mp = MathematicalProgram()
forces = mp.NewContinuousVariables(2 * len(points), "forces")
y = mp.NewContinuousVariables(1, "y")
g = get_G(points, normals)
# print('Is Full Row Rank? => ' + str(is_full_row_rank(g)))
if (not is_full_row_rank(g)):
return False
w = g.dot(forces)
mp.AddLinearConstraint(w[0] == 0)
mp.AddLinearConstraint(w[1] == 0)
mp.AddLinearConstraint(w[2] == 0)
# mp.AddLinearEqualityConstraint(g, np.zeros((3, 1)), forces)
mp.AddLinearConstraint(y[0] >= -1)
mp.AddLinearConstraint(y[0] <= 0)
for i in range(len(points)):
mp.AddLinearConstraint(mu * forces[2 * i + 1] >= forces[2 * i])
mp.AddLinearConstraint(mu * forces[2 * i + 1] >= -forces[2 * i])
mp.AddLinearConstraint(forces[2 * i + 1] >= -y[0])
mp.AddLinearCost(y[0])
result = Solve(mp)
forces_solution = result.GetSolution(forces)
y_solution = result.GetSolution(y)
# print('forces_solution: ' + str(forces_solution))
# print('w: ' + str(g.dot(forces_solution)))
# print('y_solution: ' + str(y_solution))
if y_solution == 0:
return False
else:
return True
def achieves_force_closure(points, normals, mu, debug=False):
"""
Determines whether the given forces achieve force closure.
Note that each of points and normals are lists of variable
length, but should be the same length.
Here's an example valid set of input args:
points = [np.asarray([0.1, 0.2]), np.asarray([0.5,-0.1])]
normals = [np.asarray([-1.0,0.0]), np.asarray([0.0, 1.0])]
mu = 0.2
achieves_force_closure(points, normals, mu)
NOTE: your normals should be normalized (be unit vectors)
:param points: a list of arrays where each array is the
position of the force points relative to the center of mass.
:type points: a list of 1-d numpy arrays of shape (2,)
:param normals: a list of arrays where each array is the normal
of the local surface of the object, pointing in towards
the object
:type normals: a list of 1-d numpy arrays of shape (2,)
:param mu: coulombic kinetic friction coefficient
:type mu: float, greater than 0.
:return: whether or not the given parameters achieves force closure
:rtype: bool (True or False)
"""
assert len(points) == len(normals)
assert mu >= 0.0
assert_unit_normals(normals)
## YOUR CODE HERE
G = get_G(points, normals)
if not is_full_row_rank(G):
print("G is not full row rank")
return False
N_points = len(points)
max_force = 1000
mp = MathematicalProgram()
## Decision vars
# force tangent to surface
forces_x = mp.NewContinuousVariables(N_points, "forces_x")
# force normal to surface
forces_z = mp.NewContinuousVariables(N_points, "forces_z")
# -1<=slack_var<=0 used to convert inequalities to strict inequalities
slack_var = mp.NewContinuousVariables(1, "slack_var")
# put force vars in single array
forces = [None] * 2 * N_points
for point_idx in range(N_points):
forces[point_idx * 2] = forces_x[point_idx]
forces[point_idx * 2 + 1] = forces_z[point_idx]
# ensure forces/moments add to zero
for row_idx in range(np.shape(G)[0]):
# 3 rows (LMB_x, LMB_z, AMB) = 0 for static system
mp.AddLinearConstraint(G[row_idx, :].dot(forces) == 0)
if debug:
print("Static force/moment constraints = 0:")
print(G[row_idx, :].dot(forces))
# ensure forces stay within friction cone and use slack to avoid trivial 0 solution
for point_idx in range(N_points):
# normal force must be positive (slack allows us to catch if it's zero)
mp.AddLinearConstraint(forces_z[point_idx] + slack_var[0] >= 0)
mp.AddLinearConstraint(
forces_x[point_idx] <= mu * forces_z[point_idx] + slack_var[0])
mp.AddLinearConstraint(
forces_x[point_idx] >= -(mu * forces_z[point_idx] + slack_var[0]))
# restrict slack
mp.AddLinearConstraint(slack_var[0] <= 0)
mp.AddLinearConstraint(slack_var[0] >= -1)
mp.AddLinearCost(slack_var[0])
# restrict solution within bounds
for force in forces:
mp.AddLinearConstraint(force >= -max_force)
mp.AddLinearConstraint(force <= max_force)
result = Solve(mp)
if (not result.is_success()):
print("solver failed to find solution")
return False
gamma = result.GetSolution(slack_var)
if (gamma < 0):
print("acheived force closure, gamma = {}".format(gamma))
if debug:
x = result.GetSolution(forces_x)
z = result.GetSolution(forces_z)
print("solution forces_x: {}".format(x))
print("solution forces_z: {}".format(z))
return True
else:
print("only trivial solution with 0 forces found")
return False
def linear_program(f, A, b, C=None, d=None, tol=1.e-5):
"""
Solves the linear program min_x f^T x s.t. A x <= b, C x = d.
Arguments
----------
f : numpy.ndarray
Gradient of the cost function.
A : numpy.ndarray
Left-hand side of the inequality constraints.
b : numpy.ndarray
Right-hand side of the inequality constraints.
C : numpy.ndarray
Left-hand side of the equality constraints.
d : numpy.ndarray
Right-hand side of the equality constraints.
tol : float
Maximum value of a residual of an inequality to consider the related constraint active.
Returns
----------
sol : dict
Dictionary with the solution of the LP.
Fields
----------
min : float
Minimum of the LP (None if the problem is unfeasible or unbounded).
argmin : numpy.ndarray
Argument that minimizes the LP (None if the problem is unfeasible or unbounded).
active_set : list of int
Indices of the active inequallities {i | A_i argmin = b} (None if the problem is unfeasible or unbounded).
multiplier_inequality : numpy.ndarray
Lagrange multipliers for the inequality constraints (None if the problem is unfeasible or unbounded).
multiplier_equality : numpy.ndarray
Lagrange multipliers for the equality constraints (None if the problem is unfeasible or unbounded or without equality constraints).
"""
# check equalities
if (C is None) != (d is None):
raise ValueError('missing C or d.')
# problem size
n_ineq, n_x = A.shape
if C is not None:
n_eq = C.shape[0]
else:
n_eq = 0
# reshape inputs
if len(f.shape) == 2:
f = np.reshape(f, f.shape[0])
# build program
prog = MathematicalProgram()
x = prog.NewContinuousVariables(n_x)
inequalities = []
for i in range(n_ineq):
lhs = A[i, :] + 1.e-20 * np.random.rand(
(n_x)
) # drake raises a RuntimeError if the in the expression x does not appear (e.g.: 0 x <= 1)
rhs = b[i] + 1.e-15 * np.random.rand(
1
) # in case the constraint is 0 x <= 0 the previous trick ends up adding the constraint x <= 0 to the program...
inequalities.append(prog.AddLinearConstraint(lhs.dot(x) <= rhs))
for i in range(n_eq):
prog.AddLinearConstraint(C[i, :].dot(x) == d[i])
prog.AddLinearCost(f.dot(x))
# solve
solver = GurobiSolver()
prog.SetSolverOption(solver.solver_type(), "OutputFlag", 0)
result = prog.Solve()
# initialize output
sol = {
'min': None,
'argmin': None,
'active_set': None,
'multiplier_inequality': None,
'multiplier_equality': None
}
if result == SolutionResult.kSolutionFound:
sol['argmin'] = prog.GetSolution(x).reshape(n_x, 1)
sol['min'] = f.dot(sol['argmin'])[0]
sol['active_set'] = np.where(
A.dot(sol['argmin']) - b > -tol)[0].tolist()
# retrieve multipliers through KKT conditions
M = A[sol['active_set'], :].T
if n_eq > 0:
M = np.hstack((M, C.T))
m = np.linalg.pinv(M).dot(-f.reshape(n_x, 1))
sol['multiplier_inequality'] = np.zeros((n_ineq, 1))
for i, j in enumerate(sol['active_set']):
sol['multiplier_inequality'][j, 0] = m[i, :]
if n_eq > 0:
sol['multiplier_equality'] = m[len(sol['active_set']):, :]
return sol
def achieves_force_closure(points, normals, mu):
"""
Determines whether the given forces achieve force closure.
Note that each of points and normals are lists of variable
length, but should be the same length.
Here's an example valid set of input args:
points = [np.asarray([0.1, 0.2]), np.asarray([0.5,-0.1])]
normals = [np.asarray([-1.0,0.0]), np.asarray([0.0, 1.0])]
mu = 0.2
achieves_force_closure(points, normals, mu)
NOTE: your normals should be normalized (be unit vectors)
:param points: a list of arrays where each array is the
position of the force points relative to the center of mass.
:type points: a list of 1-d numpy arrays of shape (2,)
:param normals: a list of arrays where each array is the normal
of the local surface of the object, pointing in towards
the object
:type normals: a list of 1-d numpy arrays of shape (2,)
:param mu: coulombic static friction coefficient
:type mu: float, greater than or equal to 0
:return: whether or not the given parameters achieves force closure
:rtype: bool (True or False)
"""
assert len(points) == len(normals)
assert mu >= 0.0
G = get_G(points, normals)
if np.linalg.matrix_rank(G) != 3:
return False
from pydrake.all import MathematicalProgram
from pydrake.solvers import mathematicalprogram as mpa
mp = MathematicalProgram()
f = mp.NewContinuousVariables(2 * len(points), "f")
gamma = mp.NewContinuousVariables(1, "gamma")
mp.AddLinearConstraint(gamma[0] <= 0)
mp.AddLinearConstraint(gamma[0] >= -1)
for i in range(len(points)):
mp.AddLinearConstraint(f[2 * i] <= mu * f[2 * i + 1] + gamma[0])
mp.AddLinearConstraint(-f[2 * i] <= mu * f[2 * i + 1] + gamma[0])
mp.AddLinearConstraint(f[2 * i] <= 1000)
mp.AddLinearConstraint(f[2 * i] >= -1000)
mp.AddLinearConstraint(f[2 * i + 1] + gamma[0] >= 0)
mp.AddLinearConstraint(f[2 * i + 1] <= 1000)
gf = G.dot(f)
#print(gf.shape)
assert (gf.shape == (3, ))
for i in range(3):
mp.AddLinearConstraint(gf[i] == 0)
mp.AddLinearCost(gamma[0])
#mp.AddQuadraticCost(np.sum(f**2))
#print(mp)
x = mp.Solve()
#print(mp.Solve())
if x == mpa.SolutionResult.kSolutionFound:
#print(mp.GetSolution(f))
g = mp.GetSolution(gamma)
#print(g)
#print(g[0] < 0)
return g[0] < 0
return False
