def CheckLevelSet(self, prev_x, x0, Vs, Vsdot, rho, multiplier_degree):
prog = MathematicalProgram()
x = prog.NewIndeterminates(len(prev_x),'x')
V = Vs.Substitute(dict(zip(prev_x, x)))
Vdot = Vsdot.Substitute(dict(zip(prev_x, x)))
slack = prog.NewContinuousVariables(1,'a')[0]
#mapping = dict(zip(x, np.ones(len(x))))
#V_norm = 0.0*V
#for i in range(len(x)):
# basis = np.ones(len(x))
# V_norm = V_norm + V.Substitute(dict(zip(x, basis)))
#V = V/V_norm
#Vdot = Vdot/V_norm
#prog.AddConstraint(V_norm == 0)
# in relative state (lambda(xbar)
Lambda = prog.NewSosPolynomial(Variables(x), multiplier_degree)[0].ToExpression()
Lambda = Lambda.Substitute(dict(zip(x, x-x0))) # switch to relative state (lambda(xbar)
prog.AddSosConstraint(-Vdot + Lambda*(V - rho) - slack*V)
prog.AddCost(-slack)
#import pdb; pdb.set_trace()
result = Solve(prog)
if(not result.is_success()):
print('%s, %s' %(result.get_solver_id().name(),result.get_solution_result()) )
print('slack = %f' %(result.GetSolution(slack)) )
print('Rho = %f' %(rho))
#assert result.is_success()
return -1.0
return result.GetSolution(slack)
def FixedLyapunovMaximizeLevelSet(self, prog, x, V, Vdot, multiplier_degree=None): ''' Assumes V>0. Vdot >= 0 => V >= rho (or x=0) via maximize rho subject to (V-rho)*x'*x - Lambda*Vdot is SOS, Lambda is SOS. ''' if multiplier_degree is None: # There are no guarantees... this is a reasonable guess: multiplier_degree = Polynomial(Vdot).TotalDegree() print "Using a degree " + str(multiplier_degree) + " multiplier for the S-procedure" # TODO(russt): implement numerical "balancing" from matlab version. Lambda = prog.NewSosPolynomial(Variables(x), multiplier_degree)[0].ToExpression() rho = prog.NewContinuousVariables(1, "rho")[0] prog.AddSosConstraint((V-rho)*(x.dot(x)) - Lambda*Vdot) prog.AddCost(-rho) # mosek = MosekSolver() # mosek.set_stream_logging(True, 'mosek.out') # result = mosek.Solve(prog, None, None) result = Solve(prog) print "Using " + result.get_solver_id().name() assert result.is_success() assert result.GetSolution(rho) > 0, "Optimization failed (rho <= 0)." return V/result.GetSolution(rho)
def findLyapunovFunctionSOS(self, x0, deg_V, deg_L):
prog = MathematicalProgram()
# Declare the "indeterminates", x. These are the variables which define the polynomials
z = prog.NewIndeterminates(nZ,'z')
x = prog.NewIndeterminates(1, 'x')[0]
y = prog.NewIndeterminates(1, 'y')[0]
thetadot = prog.NewIndeterminates(1, 'thetadot')[0]
X = np.array([s, c, thetadot])
sym_system = self.ToSymbolic()
sym_context = sym_system.CreateDefaultContext()
sym_context.SetContinuousState(z0+z)
sym_context.FixInputPort(0, u0+ucon )
f = sym_system.EvalTimeDerivatives(sym_context).CopyToVector() # - dztrajdt.value(t).transpose()
# Construct a polynomial V that contains all monomials with s,c,thetadot up to degree n.
V = prog.NewFreePolynomial(Variables(X), deg_V).ToExpression()
eps = 1e-4
constraint1 = prog.AddSosConstraint(V - eps*(X-x0).dot(X-x0)) # constraint to enforce that V is strictly positive away from x0.
Vdot = V.Jacobian(X).dot(f) # Construct the polynomial which is the time derivative of V
L = prog.NewFreePolynomial(Variables(X), deg_L).ToExpression() # Construct a polynomial L representing the "Lagrange multiplier".
constraint2 = prog.AddSosConstraint(-Vdot - L*(x**2+y**2-1) -
eps*(X-x0).dot(X-x0)*y**2) # Add a constraint that Vdot is strictly negative away from x0
# Add V(0) = 0 constraint
constraint3 = prog.AddLinearConstraint(V.Substitute({y: 0, x: 1, thetadot: 0}) == 0)
# Add V(theta=xxx) = 1, just to set the scale.
constraint4 = prog.AddLinearConstraint(V.Substitute({y: 1, x: 0, thetadot: 0}) == 1)
# Call the solver.
result = Solve(prog)
Vsol = Polynomial(result.GetSolution(V))
return Vsol
def test_method_3(self):
"""Test level set for Method 3"""
# test level set value up to fourth digit
# (note that the degree of the polynomial does not affect precision
# in this particular problem)
rho_method_3 = self.notebook_locals['rho_method_3']
self.assertEqual(hash(self.round(rho_method_3, 4) * 2),
1404258392611139588,
"The level set from Method 3 is incorrect.")
# try to solve again the optimization problem
# and test that the optimal value of rho is the same
# to retrieve rho, find the only optimization variable in the cost
prog = self.notebook_locals['prog3']
costs = prog.GetAllCosts()
self.assertEqual(len(costs), 1,
"prog3 has more than one cost function.")
variables = prog.GetAllCosts()[0].variables()
self.assertEqual(len(variables), 1,
"The cost function of prog3 is incorrect.")
rho = variables[0]
result = Solve(prog)
self.assertAlmostEqual(
result.GetSolution(rho),
rho_method_3,
msg="Solving prog3 we got a different value of rho_method_3.")
def solve_lcp(P, q):
prog = MathematicalProgram()
x = prog.NewContinuousVariables(q.size)
prog.AddLinearComplementarityConstraint(P, q, x)
result = Solve(prog)
status = result.is_success()
z = result.GetSolution(x)
return (z, status)
class SosProgram(SosProgramParent, MathematicalProgram):
def __init__(self):
MathematicalProgram.__init__(self)
self.result = None
def add_variables(self, size, name='c'):
return self.NewContinuousVariables(size, name)
def add_psd_variable(self, size, name='Q'):
gram = self.NewSymmetricContinuousVariables(size, name)
cons = self.AddPositiveSemidefiniteConstraint(gram)
return gram, cons
def add_linear_constraint(self, cons):
self.AddLinearConstraint(cons)
def add_linear_cost(self, expr):
self.AddLinearCost(expr)
def solve(self):
self.result = Solve(self)
def minimum(self):
if self.result is not None:
return self.result.get_optimal_cost()
def substitute_minimizer(self, expr):
if isinstance(expr, Polynomial):
p_opt = Polynomial({})
for v, c in expr:
c_opt = self.result.GetSolution(c)
if isinstance(c_opt, Expression):
c_opt = c_opt.Evaluate()
p_opt[v] = c_opt
return p_opt
else:
return self.result.GetSolution(expr)
def CheckLevelSet(prev_x, prev_V, prev_Vdot, rho, multiplier_degree): prog = MathematicalProgram() x = prog.NewIndeterminates(len(prev_x),'x') V = prev_V.Substitute(dict(zip(prev_x, x))) Vdot = prev_Vdot.Substitute(dict(zip(prev_x, x))) slack = prog.NewContinuousVariables(1)[0] Lambda = prog.NewSosPolynomial(Variables(x), multiplier_degree)[0].ToExpression() prog.AddSosConstraint(-Vdot + Lambda*(V - rho) - slack*V) prog.AddCost(-slack) result = Solve(prog) assert result.is_success() return result.GetSolution(slack)
def CheckLevelSet(prev_x, prev_V, prev_Vdot, rho, multiplier_degree):
#import pdb; pdb.set_trace()
prog = MathematicalProgram()
x = prog.NewIndeterminates(len(prev_x),'x')
V = prev_V.Substitute(dict(zip(prev_x, x)))
Vdot = prev_Vdot.Substitute(dict(zip(prev_x, x)))
slack = prog.NewContinuousVariables(1,'a')[0]
Lambda = prog.NewSosPolynomial(Variables(x), multiplier_degree)[0].ToExpression()
#print('V degree: %d' %(Polynomial(V).TotalDegree()))
#print('Vdot degree: %d' %(Polynomial(Vdot).TotalDegree()))
#print('SOS degree: %d' %(Polynomial(-Vdot + Lambda*(V - rho) - slack*V).TotalDegree()))
prog.AddSosConstraint(-Vdot + Lambda*(V - rho) - slack*V)
prog.AddCost(-slack)
result = Solve(prog)
#assert result.is_success()
return result.GetSolution(slack)
def get_ik(self, t):
epsilon = 1e-2
ik = InverseKinematics(plant=self.plant, with_joint_limits=True)
if self.q_guess is None:
context = self.plant.CreateDefaultContext()
self.q_guess = self.plant.GetPositions(context)
# This helps get the solver out of the saddle point when knee joint are locked (0.0)
self.q_guess[getJointIndexInGeneralizedPositions(
self.plant, 'l_leg_kny')] = 0.1
self.q_guess[getJointIndexInGeneralizedPositions(
self.plant, 'r_leg_kny')] = 0.1
for trajectory in self.trajectories:
position = trajectory.position_traj.value(t)
ik.AddPositionConstraint(
frameB=self.plant.GetFrameByName(trajectory.frame),
p_BQ=trajectory.position_offset,
frameA=self.plant.world_frame(),
p_AQ_upper=position + trajectory.position_tolerance,
p_AQ_lower=position - trajectory.position_tolerance)
if trajectory.orientation_traj is not None:
orientation = trajectory.orientation_traj.value(t)
ik.AddOrientationConstraint(
frameAbar=self.plant.world_frame(),
R_AbarA=RotationMatrix.Identity(),
frameBbar=self.plant.GetFrameByName(trajectory.frame),
R_BbarB=RotationMatrix(orientation),
theta_bound=trajectory.orientation_tolerance)
result = Solve(ik.prog(), self.q_guess)
if result.is_success():
return result.GetSolution(ik.q())
else:
raise Exception("Failed to find IK solution!")
def ComputeControlInput(self, x, t):
# Set up things you might want...
q = x[0:self.nq]
v = x[self.nq:]
kinsol = self.hand.doKinematics(x[0:self.hand.get_num_positions()])
M = self.hand.massMatrix(kinsol)
C = self.hand.dynamicsBiasTerm(kinsol, {}, None)
B = self.hand.B
# Assume grasp points are achieved, and calculate
# contact jacobian at those points, as well as the current
# grasp points and normals (in WORLD FRAME).
grasp_points_world_now = self.transform_grasp_points_manipuland(q)
grasp_normals_world_now = self.transform_grasp_normals_manipuland(q)
J_manipuland = jacobian(
self.transform_grasp_points_manipuland, q)
ee_points_now = self.transform_grasp_points_fingers(q)
J_manipulator = jacobian(
self.transform_grasp_points_fingers, q)
# The contact jacobian (Phi), for contact forces coming from
# point contacts, describes how the movement of the contact point
# maps into the joint coordinates of the robot. We can get this
# by combining the jacobians derived from forward kinematics to each
# of the two bodies that are in contact, evaluated at the contact point
# in each body's frame.
J_contact = J_manipuland - J_manipulator
# Cut off the 3rd dimension, which should always be 0
J_contact = J_contact[0:2, :, :]
# Given these, the manipulator equations enter as a linear constraint
# M qdd + C = Bu + J_contact lambda
# (the combined effects of lambda on the robot).
# The list of grasp points, in manipuland frame, that we'll use.
n_cf = len(self.grasp_points)
number_of_grasp_points = len(self.grasp_points)
# The evaluated desired manipuland posture.
manipuland_qdes = self.GetDesiredObjectPosition(t)
# The desired robot (arm) posture, calculated via inverse kinematics
# to achieve the desired grasp points on the object in its current
# target posture.
qdes, info = self.ComputeTargetPosture(x, manipuland_qdes)
# print('shape' + str(np.shape(qdes)))
# print('self.nq' + str(self.nq))
# print('self.nu' + str(self.nu))
if info != 1:
if not self.shut_up:
print "Warning: target posture IK solve got info %d " \
"when computing goal posture at least once during " \
"simulation. This means the grasp points was hard to " \
"achieve given the current object posture. This is " \
"occasionally OK, but indicates that your controller " \
"is probably struggling a little." % info
self.shut_up = True
# From here, it's up to you. Following the guidelines in the problem
# set, implement a controller to achieve the specified goals.
kd = 1
kp = 25
from pydrake.all import MathematicalProgram, Solve
mp = MathematicalProgram()
u = mp.NewContinuousVariables(self.nu, "u")
qdd = mp.NewContinuousVariables(self.nq, "qdd")
x_y_dimensions = 2
lambda_variable = mp.NewContinuousVariables(x_y_dimensions, number_of_grasp_points, "lambda_variable")
forces = np.zeros((self.nq))
for i in range(number_of_grasp_points):
current_forces = np.transpose(J_contact)[:,i,:].dot(lambda_variable[:,i])
forces = forces + current_forces
leftHandSide = M.dot(qdd) + C
rightHandSide = B.dot(u) + forces
for i in range(len(leftHandSide)):
mp.AddConstraint(leftHandSide[i] == rightHandSide[i])
# Calculate Normals
normals = np.array([])
for i in range(number_of_grasp_points):
current_grasp_normal = grasp_normals_world_now[0:2, i]
normals = np.vstack((normals, current_grasp_normal)) if normals.size else current_grasp_normal
# Calculate Tangeants
tangeants = np.array([])
for i in range(number_of_grasp_points):
current_grasp_tangeant = np.array([normals[i, 1], -normals[i, 0]])
tangeants = np.vstack((tangeants, current_grasp_tangeant)) if tangeants.size else current_grasp_tangeant
# Create beta variables
beta = mp.NewContinuousVariables(2, number_of_grasp_points, "b0")
# Create Grasps
for i in range(number_of_grasp_points):
c0 = normals[i] - self.mu * tangeants[i]
c1 = normals[i] + self.mu * tangeants[i]
right_hand_lambda1 = c0 * beta[0, i] + c1 * beta[1, i]
mp.AddConstraint(lambda_variable[0, i] == right_hand_lambda1[0])
mp.AddConstraint(lambda_variable[1, i] == right_hand_lambda1[1])
mp.AddConstraint(beta[0, i] >= 0)
mp.AddConstraint(beta[1, i] >= 0)
mp.AddConstraint(normals[i].dot(lambda_variable[:, i]) >= 0.25)
# Copying the control period of the constructor. Probably not supposed to do this...
next_tick_qd = v + qdd * self.cont¨rol_period
next_tick_q = q + next_tick_qd * self.control_period
q_error = qdes - next_tick_q
proportionalCost = q_error.dot(np.transpose(q_error))
qd_error = 0 - next_tick_qd
diffCost = qd_error.dot(np.transpose(qd_error))
mp.AddQuadraticCost(kp * proportionalCost + kd * diffCost)
result = Solve(mp)
u_solution = result.GetSolution(u)
u = np.zeros(self.nu)
return u_solution
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
def create_q_knots(pose_lst):
"""Convert end-effector pose list to joint position list using series of
InverseKinematics problems. Note that q is 9-dimensional because the last 2 dimensions
contain gripper joints, but these should not matter to the constraints.
@param: pose_lst (python list): post_lst[i] contains keyframe X_WG at index i.
@return: q_knots (python_list): q_knots[i] contains IK solution that will give f(q_knots[i]) \approx pose_lst[i].
"""
q_knots = []
plant, _ = CreateIiwaControllerPlant()
world_frame = plant.world_frame()
gripper_frame = plant.GetFrameByName("body")
#q_nominal = np.array([ 0., 0.6, 0., -1.75, 0., 1., 0., 0., 0.]) # nominal joint for joint-centering.
q_nominal = np.array(
[-1.57, 0.1, 0.00, -1.2, 0.00, 1.60, 0.00, 0.00, 0.00])
def AddOrientationConstraint(ik, R_WG, bounds):
"""Add orientation constraint to the ik problem. Implements an inequality
constraint where the axis-angle difference between f_R(q) and R_WG must be
within bounds. Can be translated to:
ik.prog().AddBoundingBoxConstraint(angle_diff(f_R(q), R_WG), -bounds, bounds)
"""
ik.AddOrientationConstraint(frameAbar=world_frame,
R_AbarA=R_WG,
frameBbar=gripper_frame,
R_BbarB=RotationMatrix(),
theta_bound=bounds)
def AddPositionConstraint(ik, p_WG_lower, p_WG_upper):
"""Add position constraint to the ik problem. Implements an inequality
constraint where f_p(q) must lie between p_WG_lower and p_WG_upper. Can be
translated to
ik.prog().AddBoundingBoxConstraint(f_p(q), p_WG_lower, p_WG_upper)
"""
ik.AddPositionConstraint(frameA=world_frame,
frameB=gripper_frame,
p_BQ=np.zeros(3),
p_AQ_lower=p_WG_lower,
p_AQ_upper=p_WG_upper)
for i in range(len(pose_lst)):
# if i % 100 == 0:
# print(i)
ik = inverse_kinematics.InverseKinematics(plant)
q_variables = ik.q() # Get variables for MathematicalProgram
prog = ik.prog() # Get MathematicalProgram
#### Modify here ###############################
X_WG = pose_lst[i]
p_WG = X_WG.translation()
r_WG = X_WG.rotation()
z_slack = 0
degrees_slack = 0
AddPositionConstraint(ik, p_WG - np.array([0, 0, z_slack]),
p_WG + np.array([0, 0, z_slack]))
AddOrientationConstraint(ik, r_WG, degrees_slack * np.pi / 180)
# initial guess
if i == 0:
prog.SetInitialGuess(q_variables, q_nominal)
diff = q_variables - q_nominal
else:
prog.SetInitialGuess(q_variables, q_knots[i - 1])
diff = q_variables - q_knots[i - 1]
prog.AddCost(np.sum(diff.dot(diff)))
################################################
result = Solve(prog)
if not result.is_success():
visualize_transform(meshcat,
"FAIL",
X_WG,
prefix='',
length=0.3,
radius=0.02)
print(f"Failed at {i}")
break
#raise RuntimeError
tmp = result.GetSolution(q_variables)
q_knots.append(tmp)
return q_knots
def pose_to_jointangles(T_world_robotPose):
plant, _ = CreateIiwaControllerPlant()
plant_context = plant.CreateDefaultContext()
world_frame = plant.world_frame()
gripper_frame = plant.GetFrameByName("body")
#q_nominal = np.array([ 0., 0.6, 0., -1.75, 0., 1., 0., 0., 0.]) # nominal joint for joint-centering.
q_nominal = np.array(
[-1.57, 0.1, 0.00, -1.2, 0.00, 1.60, 0.00, 0.00, 0.00])
def AddOrientationConstraint(ik, R_WG, bounds):
ik.AddOrientationConstraint(frameAbar=world_frame,
R_AbarA=R_WG,
frameBbar=gripper_frame,
R_BbarB=RotationMatrix(),
theta_bound=bounds)
def AddPositionConstraint(ik, p_WG_lower, p_WG_upper):
ik.AddPositionConstraint(frameA=world_frame,
frameB=gripper_frame,
p_BQ=np.zeros(3),
p_AQ_lower=p_WG_lower,
p_AQ_upper=p_WG_upper)
# def AddJacobianConstraint_Joint_To_Plane(ik):
# # calculate the jacobian
# J_G = plant.CalcJacobianSpatialVelocity(
# ik.context(),
# JacobianWrtVariable.kQDot,
# gripper_frame,
# [0,0,0],
# world_frame,
# world_frame
# )
# # ensure that when joints 4 and 6 move, they keep the gripper in the desired plane
# prog = ik.get_mutable_prog()
# prog.AddConstraint()
# joint_4 = J_G[:, 3]
# joint_6 = J_G[:, 5]
ik = inverse_kinematics.InverseKinematics(plant)
q_variables = ik.q() # Get variables for MathematicalProgram
prog = ik.prog() # Get MathematicalProgram
p_WG = T_world_robotPose.translation()
r_WG = T_world_robotPose.rotation()
# must be an exact solution
z_slack = 0
degrees_slack = 0
AddPositionConstraint(ik, p_WG - np.array([0, 0, z_slack]),
p_WG + np.array([0, 0, z_slack]))
AddOrientationConstraint(ik, r_WG, degrees_slack * np.pi / 180)
# todo: add some sort of constraint so that jacobian is 0 in certain directions
# (so just joints 4 and 6 move when swung)
# initial guess
prog.SetInitialGuess(q_variables, q_nominal)
diff = q_variables - q_nominal
prog.AddCost(np.sum(diff.dot(diff)))
result = Solve(prog)
if not result.is_success():
#visualize_transform(meshcat, "FAIL", X_WG, prefix='', length=0.3, radius=0.02)
assert (False) # no IK solution for target
jas = result.GetSolution(q_variables)
return jas
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 TimeVaryingLyapunovSearchRho(self, prev_x, Vs, Vdots, Ts, times, xtraj, utraj, \
rho_f, multiplier_degree=None):
C = 1.0 #8.0
#rho_f = 3.0
tries = 40
prev_rhointegral = 0.
N = len(times)-1
#Vmin = self.minimumV(prev_x, Vs) #0.05*np.ones((1, len(times))) # need to do something here instead of this!!
dt = np.diff(times)
#rho = np.flipud(rho_f*np.exp(-C*(np.array(times)-times[0])/(times[-1]-times[0])))# + np.max(Vmin)
#rho = np.linspace(0.1, rho_f, N+1)
#rho = np.linspace(rho_f/2.0, rho_f, N+1)
rho = np.linspace(rho_f*3.0, rho_f, N+1)
fig, ax = plt.subplots()
fig.suptitle('Rho progression')
ax.set(xlabel='index', ylabel='rho')
ax.grid(True)
plt.show(block = False)
need_to_break = False
for idx in range(tries):
print('starting iteration #%d with rho=' %(idx))
print(rho)
ax.plot(rho)
plt.pause(0.05)
# start of number of iterations if we want
rhodot = np.diff(rho)/dt
# sampleCheck()
Lambda_vec = []
x_vec = []
#import pdb; pdb.set_trace()
#fix rho, optimize Lagrange multipliers
for i in range(N):
prog = MathematicalProgram()
x = prog.NewIndeterminates(len(prev_x),'x')
V = Vs[i].Substitute(dict(zip(prev_x, x)))
Vdot = Vdots[i].Substitute(dict(zip(prev_x, x)))
x0 = xtraj.value(times[i]).transpose()[0]
Ttrans = np.linalg.inv(Ts[i])
x0 = Ttrans.dot(x0)
#xmin, vmin, vdmin = self.SampleCheck(x, V, Vdot)
#if(vdmin > rhodot[i]):
# print('Vdot is greater than rhodot!')
#Lambda = prog.NewSosPolynomial(Variables(x), multiplier_degree)[0].ToExpression()
Lambda = prog.NewFreePolynomial(Variables(x), multiplier_degree).ToExpression()
Lambda = Lambda.Substitute(dict(zip(x, x-x0))) # switch to relative state (lambda(xbar)
gamma = prog.NewContinuousVariables(1,'g')[0]
# Jdot-rhodot+Lambda*(rho-J) < -gamma
prog.AddSosConstraint( -gamma*V - (Vdot-rhodot[i] + Lambda*(rho[i]-V)) )
prog.AddCost(-gamma) #maximize gamma
result = Solve(prog)
if result.is_success() == False:
need_to_break = True
print('Solver could not solve anymore')
import pdb; pdb.set_trace()
break
else:
Lambda_vec.append(result.GetSolution(Lambda))
slack = result.GetSolution(gamma)
print('Slack #%d = %f' %(idx, slack))
x_vec.append(x)
if(slack < 0.0):
print('In iter#%d, found negative slack so going to end prematurely... :(' %(idx))
need_to_break = True
if(need_to_break == True):
break;
# fix Lagrange multipliers, maximize rho
rhointegral = 0.0
prog = MathematicalProgram()
xx = prog.NewIndeterminates(len(x),'x')
t = prog.NewContinuousVariables(N,'t')
#import pdb; pdb.set_trace()
#rho = np.concatenate((t,[rho_f])) + Vmin
rho_x = np.concatenate((t,[rho[-1]])) #+ rho
#import pdb; pdb.set_trace()
for i in range(N):
#prog.AddConstraint(t[i]>=0.0) # does this mean [prog,rho] = new(prog,N,'pos'); in matlab??
rhod_x = (rho_x[i+1]-rho_x[i])/dt[i]
#prog.AddConstraint(rhod_x<=0.0)
rhointegral = rhointegral+rho_x[i]*dt[i] + 0.5*rhod_x*(dt[i]**2)
V = Vs[i].Substitute(dict(zip(prev_x, xx)))
Vdot = Vdots[i].Substitute(dict(zip(prev_x, xx)))
#x0 = xtraj.value(times[i]).transpose()[0]
L1 = Lambda_vec[i].Substitute(dict(zip(x_vec[i], xx)))
#Vdot = Vdot*rho_x[i] - V*rhod_x
prog.AddSosConstraint( -(Vdot - rhod_x + L1 * ( rho_x[i]-V ) ) )
prog.AddCost(-rhointegral)
result = Solve(prog)
assert result.is_success()
rhos = result.GetSolution(rho_x)
rho = []
for r in rhos:
rho.append(r[0].Evaluate())
rhointegral = result.GetSolution(rhointegral).Evaluate()
if( (rhointegral-prev_rhointegral)/rhointegral < 1E-5): # 0.1%
print('Rho integral converged')
need_to_break = True
break;
else:
prev_rhointegral = rhointegral
print('End of iteration #%d: rhointegral=%f' %(idx, rhointegral))
if(need_to_break == True):
print('In iter#%d, found negative slack so ending prematurely... :(' %(idx))
break;
# end of iterations if we want
ax.plot(rho)
plt.pause(0.05)
print('done computing funnel.\nFinal rho= ')
print(rho)
#import pdb; pdb.set_trace()
for i in range(len(rho)):
Vs[i] = Vs[i]/rho[i]
return Vs
a = 0.5*Jacobian(V1.Jacobian(x),x)
b = 0.5*Jacobian(V2.Jacobian(x),x)
pxi = np.array([[-0.5,-0.5], [-0.5, 0.5], [0.5,0.5], [0.5,-0.5]])
for pt in pxi:
prog.AddConstraint( pt.T.dot(a).dot(pt) <= 1.0)
prog.AddConstraint( pt.T.dot(b).dot(pt) <= 1.0)
pxi = np.array([[0.0, 1.0] ])
prog.AddConstraint( pxi[-1].T.dot(b).dot(pxi[-1]) <= 1.0)
prog.AddMaximizeLogDeterminantSymmetricMatrixCost(a)
prog.AddMaximizeLogDeterminantSymmetricMatrixCost(b)
result = Solve(prog)
assert(result.is_success())
V1 = result.GetSolution(V1)
V2 = result.GetSolution(V2)
fig1, ax1 = plt.subplots()
ax1.set_xlim([-5.0, 5.0])
ax1.set_ylim([-5.0, 5.0])
ax1.set_xlabel('x')
ax1.set_ylabel('y')
sublevelset(ax1, V2, np.array([0,0]), vertices=51, color=(0.1,0.8,0.8))
sublevelset(ax1, V1, np.array([0,0]), vertices=51, color=(0.1,0.5,0.8))
plt.show(block = True)
# 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.')
# 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
The SOS program we just wrote was already a satisfying solution, but it turns out we can do even better!
From the textbook chapter "[Lyapunov analysis with convex optimization](http://underactuated.mit.edu/lyapunov.html#section2)", you know that every SOS constraint brings with it a lot of optimization variables and an SDP constraint.
So, whenever we can, removing redundant SOS requirements is always a good thing to do.
We claim that in the previous formulation we don't need $\lambda(\mathbf{x})$ to be SOS. How is this possible?
We start by noticing that, in a neighborhood of the origin, excluded the origin itself, $\dot{V}(\mathbf{x})$ is strictly negative.
(This because $V(\mathbf{x})$ is a Lyapunov function for the linearized system hence, locally, it works also for the nonlinear system.)
In light of the latter observation, instead of asking that $\dot{V}(\mathbf{x})$ is negative for all $\mathbf{x} \neq 0$ in $L(\rho)$, we can equivalently ask that all the points $\mathbf{x} \neq 0$ where $\dot{V}(\mathbf{x}) = 0$ must be outside the level set $L(\rho)$.
def ComputeControlInput(self, x, t):
# Set up things you might want...
q = x[0:self.nq]
v = x[self.nq:]
kinsol = self.hand.doKinematics(x[0:self.hand.get_num_positions()])
M = self.hand.massMatrix(kinsol)
C = self.hand.dynamicsBiasTerm(kinsol, {}, None)
B = self.hand.B
# Assume grasp points are achieved, and calculate
# contact jacobian at those points, as well as the current
# grasp points and normals (in WORLD FRAME).
grasp_points_world_now = self.transform_grasp_points_manipuland(q)
grasp_normals_world_now = self.transform_grasp_normals_manipuland(q)
J_manipuland = jacobian(self.transform_grasp_points_manipuland, q)
ee_points_now = self.transform_grasp_points_fingers(q)
J_manipulator = jacobian(self.transform_grasp_points_fingers, q)
# The contact jacobian (Phi), for contact forces coming from
# point contacts, describes how the movement of the contact point
# maps into the joint coordinates of the robot. We can get this
# by combining the jacobians derived from forward kinematics to each
# of the two bodies that are in contact, evaluated at the contact point
# in each body's frame.
J_contact = J_manipuland - J_manipulator
# Cut off the 3rd dimension, which should always be 0
J_contact = J_contact[0:2, :, :]
# Given these, the manipulator equations enter as a linear constraint
# M qdd + C = Bu + J_contact lambda
# (the combined effects of lambda on the robot).
# The list of grasp points, in manipuland frame, that we'll use.
n_cf = len(self.grasp_points)
# The evaluated desired manipuland posture.
manipuland_qdes = self.GetDesiredObjectPosition(t)
# The desired robot (arm) posture, calculated via inverse kinematics
# to achieve the desired grasp points on the object in its current
# target posture.
qdes, info = self.ComputeTargetPosture(x, manipuland_qdes)
if info != 1:
if not self.shut_up:
print "Warning: target posture IK solve got info %d " \
"when computing goal posture at least once during " \
"simulation. This means the grasp points was hard to " \
"achieve given the current object posture. This is " \
"occasionally OK, but indicates that your controller " \
"is probably struggling a little." % info
self.shut_up = True
# From here, it's up to you. Following the guidelines in the problem
# set, implement a controller to achieve the specified goals.
mp = MathematicalProgram()
#control variables = u = self.nu x 1
#ddot{q} as variable w/ q's shape
controls = mp.NewContinuousVariables(self.nu, "controls")
qdd = mp.NewContinuousVariables(q.shape[0], "qdd")
#make variables for lambda's and beta's
k = 0
b = mp.NewContinuousVariables(2, "betas_%d" % k)
betas = b
for i in range(len(self.grasp_points)):
b = mp.NewContinuousVariables(
2, "betas_%d" % k) #pair of betas for each contact point
betas = np.vstack((betas, b))
mp.AddLinearConstraint(
betas[i, 0] >= 0).evaluator().set_description(
"beta_0 greater than 0 for %d" % i)
mp.AddLinearConstraint(
betas[i, 1] >= 0).evaluator().set_description(
"beta_1 greater than 0 for %d" % i)
#c_0=normals+mu*tangent
#c1 = normals-mu*tangent
n = grasp_normals_world_now.T[:, 0:2] #row=contact point?
t = get_perpendicular2d(n[0])
c_i1 = n[0] + np.dot(self.mu, t)
c_i2 = n[0] - np.dot(self.mu, t)
l = c_i1.dot(betas[0, 0]) + c_i2.dot(betas[0, 1])
lambdas = l
for i in range(1, len(self.grasp_points)):
n = grasp_normals_world_now.T[:, 0:
2] #row=contact point? transpose then index
t = get_perpendicular2d(n[i])
c_i1 = n[i] - np.dot(self.mu, t)
c_i2 = n[i] + np.dot(self.mu, t)
l = c_i1.dot(betas[i, 0]) + c_i2.dot(betas[i, 1])
lambdas = np.vstack((lambdas, l))
control_period = 0.0333
#Constrain the dyanmics
dyn = M.dot(qdd) + C - B.dot(controls)
for i in range(q.shape[0]):
j_c = 0
for l in range(len(self.grasp_points)):
j_sub = J_contact[:, l, :].T
j_c += j_sub.dot(lambdas[l])
# print(j_c.shape)
# print(dyn.shape)
mp.AddConstraint(dyn[i] - j_c[i] == 0)
#PD controller using kinematics
Kp = 100.0
Kd = 1.0
control_period = 0.0333
next_q = q + v.dot(control_period) + np.dot(qdd.dot(control_period**2),
.5)
next_q_dot = v + qdd.dot(control_period)
mp.AddQuadraticCost(Kp * (qdes - next_q).dot((qdes - next_q).T) + Kd *
(next_q_dot).dot(next_q_dot.T))
Kp_ext = 100.
mp.AddQuadraticCost(Kp_ext * (qdes - next_q)[-3:].dot(
(qdes - next_q)[-3:].T))
res = Solve(mp)
c = res.GetSolution(controls)
return c
def bounce_pass_wall(self,
p_puck,
p_goal,
which_wall,
duration=3,
debug=True):
"""Solve for initial velocity for the puck to bounce the wall once and hit the goal."""
# initialize program
prog = MathematicalProgram()
# time periods before and after hitting the wall
h = prog.NewContinuousVariables(2, name='h')
# velocities of the ball at the start of each segment (after collision).
vel_start = prog.NewContinuousVariables(2, name='vel_start')
vel_after = prog.NewContinuousVariables(2, name='vel_after')
# sum of durations cannot exceed the specified value
prog.AddConstraint(np.sum(h) <= duration)
# Help the solver by telling it initial velocity direction
self.add_initial_vel_direction_constraint(prog, which_wall, p_goal,
vel_start)
# add dynamics constraints for the moment the ball hits the wall
p_contact = self.next_p(p_puck, vel_start, h[0])
v_contact = self.next_vel(vel_start, h[0])
# keep the same x vel, but flip the y vel
self.add_reset_map_constraint(prog, which_wall, p_contact, v_contact,
vel_after)
# in the last segment, need to specify bounds for the final position and velocity of the ball
p_end = self.next_p(p_contact, vel_after, h[1])
v_end = self.next_vel(vel_after, h[1])
self.add_goal_constraint(prog, which_wall, p_goal, p_end, v_end)
# solve for time periods h
result = Solve(prog)
if not result.is_success():
if debug:
infeasible = GetInfeasibleConstraints(prog, result)
print("Infeasible constraints in ContactOptimizer:")
for i in range(len(infeasible)):
print(infeasible[i])
# return directly
return
else:
if debug:
print("vel_start:{}".format(result.GetSolution(vel_start)))
print("vel_after: {}".format(result.GetSolution(vel_after)))
print("h: {}".format(result.GetSolution(h)))
p1 = self.next_p(p_puck, result.GetSolution(vel_start),
result.GetSolution(h[0]))
p2 = self.next_p(p1, result.GetSolution(vel_after),
result.GetSolution(h[1]))
print("p1:{}".format(p1))
print("p2:{}".format(p2))
v_end = self.next_vel(result.GetSolution(vel_after),
result.GetSolution(h[1]))
print("v_end:{}".format(v_end))
# return whether successful, and the initial puck velocity
return result.is_success(), result.GetSolution(vel_start)
# minima, two of them being global minima.
p = 4*x**2+x*y - 4*y**2 - 2.1*x**4 + 4*y**4+x**6/3
# Find the minimum value by adding a sums of squares constraint, via
# for all x, p(x) >= pmin
# which we write as
# p(x) - pmin is sos.
pmin = prog.NewContinuousVariables(1, "pmin")[0]
prog.AddSosConstraint(p-pmin)
# Maximize pmin.
prog.AddCost(-pmin)
result = Solve(prog)
assert(result.is_success())
print("Minimum value (lower bound): " + str(result.GetSolution(pmin)))
# Now, let's plot it.
fig = plt.figure(figsize=(10, 5))
ax0 = fig.add_subplot(121, projection='3d')
ax1 = fig.add_subplot(122)
xs = np.linspace(-2.2, 2.2, 51)
ys = np.linspace(-1.2, 1.2, 51)
[X, Y] = np.meshgrid(xs, ys)
P = X.copy()
for i in range(len(xs)):
for j in range(len(ys)):
P[i, j] = p.Evaluate({x: X[i, j], y: Y[i, j]})
ax0.plot_surface(X, Y, P)
ax1.contour(X, Y, P, 100)
# Add V(theta=pi) = mgl, just to set the scale.
constraint4 = prog.AddLinearConstraint(
V.Substitute({s: 1, c: 0, thetadot: 0}) == p.mass()*p.gravity()*p.length())
# Call the solver.
result = Solve(prog)
assert(result.is_success())
# Note that I've added mgl to the potential energy (relative to the textbook),
# so that it would be non-negative... like the Lyapunov function.
mgl = p.mass()*p.gravity()*p.length()
print('Mechanical Energy = ')
print(.5*p.mass()*p.length()**2*thetadot**2 + mgl*(1-c))
print('V =')
Vsol = Polynomial(result.GetSolution(V))
print(Vsol.RemoveTermsWithSmallCoefficients(1e-6))
# Plot the results as contour plots.
nq = 151
nqd = 151
q = np.linspace(-2*np.pi, 2*np.pi, nq)
qd = np.linspace(-2*mgl, 2*mgl, nqd)
Q, QD = np.meshgrid(q, qd)
Energy = .5*p.mass()*p.length()**2*QD**2 + mgl*(1-np.cos(Q))
Vplot = Q.copy()
env = {s: 0., c: 1., thetadot: 0}
for i in range(nq):
for j in range(nqd):
env[s] = np.sin(Q[i, j])
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 compute_trajectory_to_other_world(self, state_initial, minimum_time,
maximum_time):
'''
Your mission is to implement this function.
A successful implementation of this function will compute a dynamically feasible trajectory
which satisfies these criteria:
- Efficiently conserve fuel
- Reach "orbit" of the far right world
- Approximately obey the dynamic constraints
- Begin at the state_initial provided
- Take no more than maximum_time, no less than minimum_time
The above are defined more precisely in the provided notebook.
Please note there are two return args.
:param: state_initial: :param state_initial: numpy array of length 4, see rocket_dynamics for documentation
:param: minimum_time: float, minimum time allowed for trajectory
:param: maximum_time: float, maximum time allowed for trajectory
:return: three return args separated by commas:
trajectory, input_trajectory, time_array
trajectory: a 2d array with N rows, and 4 columns. See simulate_states_over_time for more documentation.
input_trajectory: a 2d array with N-1 row, and 2 columns. See simulate_states_over_time for more documentation.
time_array: an array with N rows.
'''
mp = MathematicalProgram()
max_speed = 0.99
desired_distance = 0.5
u_cost_factor = 1000.
N = 50
# trajectory = np.zeros((N+1,4))
# input_trajectory = np.ones((N,2))*10.0
time_used = (maximum_time + minimum_time) / 2.
time_step = time_used / (N + 1)
time_array = np.arange(0.0, time_used, time_step)
k = 0
# Create continuous variables for u & x
u = mp.NewContinuousVariables(2, "u_%d" % k)
x = mp.NewContinuousVariables(4, "x_%d" % k)
u_over_time = u
x_over_time = x
for k in range(1, N):
u = mp.NewContinuousVariables(2, "u_%d" % k)
x = mp.NewContinuousVariables(4, "x_%d" % k)
u_over_time = np.vstack((u_over_time, u))
x_over_time = np.vstack((x_over_time, x))
# trajectory is N+1 in size
x = mp.NewContinuousVariables(4, "x_%d" % (N + 1))
x_over_time = np.vstack((x_over_time, x))
# Add cost
# for k in range(0, N):
mp.AddQuadraticCost(u_cost_factor *
u_over_time[:, 0].dot(u_over_time[:, 0]))
mp.AddQuadraticCost(u_cost_factor *
u_over_time[:, 1].dot(u_over_time[:, 1]))
# Add constraint for initial state
mp.AddLinearConstraint(x_over_time[0, 0] >= state_initial[0])
mp.AddLinearConstraint(x_over_time[0, 0] <= state_initial[0])
mp.AddLinearConstraint(x_over_time[0, 1] >= state_initial[1])
mp.AddLinearConstraint(x_over_time[0, 1] <= state_initial[1])
mp.AddLinearConstraint(x_over_time[0, 2] >= state_initial[2])
mp.AddLinearConstraint(x_over_time[0, 2] <= state_initial[2])
mp.AddLinearConstraint(x_over_time[0, 3] >= state_initial[3])
mp.AddLinearConstraint(x_over_time[0, 3] <= state_initial[3])
# Add constraint between x & u
for k in range(1, N + 1):
next_step = self.rocket_dynamics(x_over_time[k - 1, :],
u_over_time[k - 1, :])
mp.AddConstraint(x_over_time[k, 0] == x_over_time[k - 1, 0] +
time_step * next_step[0])
mp.AddConstraint(x_over_time[k, 1] == x_over_time[k - 1, 1] +
time_step * next_step[1])
mp.AddConstraint(x_over_time[k, 2] == x_over_time[k - 1, 2] +
time_step * next_step[2])
mp.AddConstraint(x_over_time[k, 3] == x_over_time[k - 1, 3] +
time_step * next_step[3])
# Make sure it never goes too far from the planets
# for k in range(1, N):
# mp.AddConstraint(self.two_norm(x_over_time[k,0:2] - self.world_2_position[:]) <= 10)
# mp.AddConstraint(self.two_norm(x_over_time[k,0:2] - self.world_1_position[:]) <= 10)
# Make sure u never goes above a threshold
max_u = 6.
for k in range(0, N):
mp.AddLinearConstraint(u_over_time[k, 0] <= max_u)
mp.AddLinearConstraint(-u_over_time[k, 0] <= max_u)
mp.AddLinearConstraint(u_over_time[k, 1] <= max_u)
mp.AddLinearConstraint(-u_over_time[k, 1] <= max_u)
# Make sure it reaches world 2
mp.AddConstraint(
self.two_norm(x_over_time[-1, 0:2] -
self.world_2_position) <= desired_distance)
mp.AddConstraint(
self.two_norm(x_over_time[-1, 0:2] -
self.world_2_position) >= desired_distance)
# Make sure it's speed isn't too high
mp.AddConstraint(self.two_norm(x_over_time[-1, 2:4]) <= max_speed**2.)
# Get result
result = Solve(mp)
x_over_time_result = result.GetSolution(x_over_time)
u_over_time_result = result.GetSolution(u_over_time)
print("Success", result.is_success())
print("Final position", x_over_time_result[-1, :])
print(
"Final distance to world2",
self.two_norm(x_over_time_result[-1, 0:2] - self.world_2_position))
print("Final speed", self.two_norm(x_over_time_result[-1, 2:4]))
print("Fuel consumption", (u_over_time_result**2.).sum())
trajectory = x_over_time_result
input_trajectory = u_over_time_result
return trajectory, input_trajectory, time_array
def findLyapunovFunctionSOS(self, xtraj, utraj, deg_V, deg_L):
times = xtraj.get_segment_times()
prog = MathematicalProgram()
# Declare the "indeterminates", x. These are the variables which define the polynomials
x = prog.NewIndeterminates(3, 'x')
ucon = prog.NewIndeterminates(2, 'u')
sym_system = self.ToSymbolic()
sym_context = sym_system.CreateDefaultContext()
sym_context.SetContinuousState(x)
sym_context.FixInputPort(0, ucon)
f = sym_system.EvalTimeDerivativesTaylor(
sym_context).CopyToVector() # - dztrajdt.value(t).transpose()
for t in times:
x0 = xtraj.value(t).transpose()[0]
u0 = utraj.value(t).transpose()[0]
f_fb = self.EvalClosedLoopDynamics(x, ucon, f, x0, u0)
# Construct a polynomial V that contains all monomials with s,c,thetadot up to degree n.
V = prog.NewFreePolynomial(Variables(x), deg_V).ToExpression()
eps = 1e-4
constraint1 = prog.AddSosConstraint(
V - eps * (x - x0).dot(x - x0)
) # constraint to enforce that V is strictly positive away from x0.
Vdot = V.Jacobian(x).dot(
f_fb
) # Construct the polynomial which is the time derivative of V
#L = prog.NewFreePolynomial(Variables(x), deg_L).ToExpression() # Construct a polynomial L representing the
# "Lagrange multiplier".
# Add a constraint that Vdot is strictly negative away from x0
constraint2 = prog.AddSosConstraint(-Vdot[0] - eps *
(x - x0).dot(x - x0))
#import pdb; pdb.set_trace()
# Add V(0) = 0 constraint
constraint3 = prog.AddLinearConstraint(
V.Substitute({
x[0]: 0.0,
x[1]: 1.0,
x[2]: 0.0
}) == 1.0)
# Add V(theta=xxx) = 1, just to set the scale.
constraint4 = prog.AddLinearConstraint(
V.Substitute({
x[0]: 1.0,
x[1]: 0.0,
x[2]: 0.0
}) == 1.0)
# Call the solver (default).
#result = Solve(prog)
# Call the solver (specific).
#solver = CsdpSolver()
#solver = ScsSolver()
#solver = IpoptSolver()
#result = solver.Solve(prog, None, None)
result = Solve(prog)
# print out the result.
print("Success? ", result.is_success())
print(result.get_solver_id().name())
Vsol = Polynomial(result.GetSolution(V))
print('Time t=%f:\nV=' % (t))
print(Vsol.RemoveTermsWithSmallCoefficients(1e-6))
return Vsol, Vsol.Jacobian(x).dot(f_fb)
def CalcOutput(self, context, output):
# ============================ LOAD INPUTS ============================
# Torque inputs
tau_g = np.expand_dims(
np.array(self.GetInputPort("tau_g").Eval(context)), 1)
joint_centering_torque = np.expand_dims(
np.array(
self.GetInputPort("joint_centering_torque").Eval(context)), 1)
# Force inputs
# PROGRAMMING: Add zeros here?
F_GT = self.GetInputPort("F_GT").Eval(context)[0]
F_GN = self.GetInputPort("F_GN").Eval(context)[0]
# Positions
theta_L = self.GetInputPort("theta_L").Eval(context)[0]
theta_MX = self.GetInputPort("theta_MX").Eval(context)[0]
theta_MY = self.GetInputPort("theta_MY").Eval(context)[0]
theta_MZ = self.GetInputPort("theta_MZ").Eval(context)[0]
p_CT = self.GetInputPort("p_CT").Eval(context)[0]
p_CN = self.GetInputPort("p_CN").Eval(context)[0]
p_LT = self.GetInputPort("p_LT").Eval(context)[0]
p_LN = self.GetInputPort("p_LN").Eval(context)[0]
d_T = self.GetInputPort("d_T").Eval(context)[0]
d_N = self.GetInputPort("d_N").Eval(context)[0]
d_X = self.GetInputPort("d_X").Eval(context)[0]
p_MConM = np.array([self.GetInputPort("p_MConM").Eval(context)]).T
# Velocities
d_theta_L = self.GetInputPort("d_theta_L").Eval(context)[0]
d_theta_MX = self.GetInputPort("d_theta_MX").Eval(context)[0]
d_theta_MY = self.GetInputPort("d_theta_MY").Eval(context)[0]
d_theta_MZ = self.GetInputPort("d_theta_MZ").Eval(context)[0]
d_d_T = self.GetInputPort("d_d_T").Eval(context)[0]
d_d_N = self.GetInputPort("d_d_N").Eval(context)[0]
d_d_X = self.GetInputPort("d_d_X").Eval(context)[0]
# Manipulator inputs
J = np.array(self.GetInputPort("J").Eval(context)).reshape(
(6, self.nq))
J_translational = np.array(
self.GetInputPort("J_translational").Eval(context)).reshape(
(3, self.nq))
J_rotational = np.array(
self.GetInputPort("J_rotational").Eval(context)).reshape(
(3, self.nq))
Jdot_qdot = np.expand_dims(
np.array(self.GetInputPort("Jdot_qdot").Eval(context)), 1)
M = np.array(self.GetInputPort("M").Eval(context)).reshape(
(self.nq, self.nq))
Cv = np.expand_dims(np.array(self.GetInputPort("Cv").Eval(context)), 1)
# Other inputs
mu_S = self.GetInputPort("mu_S").Eval(context)[0]
hats_T = self.GetInputPort("hats_T").Eval(context)[0]
s_hat_X = self.GetInputPort("s_hat_X").Eval(context)[0]
# ============================= OTHER PREP ============================
if self.theta_MYd is None:
self.theta_MYd = theta_MY
if self.theta_MZd is None:
self.theta_MZd = theta_MZ
# Calculate desired values
dd_d_Td = 1000 * (self.d_Td - d_T) - 100 * d_d_T
dd_theta_Ld = 10 * (self.d_theta_Ld - d_theta_L)
a_MX_d = 100 * (self.d_Xd - d_X) - 10 * d_d_X
theta_MXd = theta_L
alpha_MXd = 100 * (theta_MXd - theta_MX) + 10 * (d_theta_L -
d_theta_MX)
alpha_MYd = 10 * (self.theta_MYd - theta_MY) - 5 * d_theta_MY
alpha_MZd = 10 * (self.theta_MZd - theta_MZ) - 5 * d_theta_MZ
dd_d_Nd = 0
# =========================== SOLVE PROGRAM ===========================
## 1. Define an instance of MathematicalProgram
prog = MathematicalProgram()
## 2. Add decision variables
# Contact values
F_NM = prog.NewContinuousVariables(1, 1, name="F_NM")
F_ContactMY = prog.NewContinuousVariables(1, 1, name="F_ContactMY")
F_ContactMZ = prog.NewContinuousVariables(1, 1, name="F_ContactMZ")
F_NL = prog.NewContinuousVariables(1, 1, name="F_NL")
if self.model_friction:
F_FMT = prog.NewContinuousVariables(1, 1, name="F_FMT")
F_FMX = prog.NewContinuousVariables(1, 1, name="F_FMX")
F_FLT = prog.NewContinuousVariables(1, 1, name="F_FLT")
F_FLX = prog.NewContinuousVariables(1, 1, name="F_FLX")
else:
F_FMT = np.array([[0]])
F_FMX = np.array([[0]])
F_FLT = np.array([[0]])
F_FLX = np.array([[0]])
F_ContactM_XYZ = np.array([F_FMX, F_ContactMY, F_ContactMZ])[:, :, 0]
# Object forces and torques
if not self.measure_joint_wrench:
F_OT = prog.NewContinuousVariables(1, 1, name="F_OT")
F_ON = prog.NewContinuousVariables(1, 1, name="F_ON")
tau_O = -self.sys_consts.k_J*theta_L \
- self.sys_consts.b_J*d_theta_L
# Control values
tau_ctrl = prog.NewContinuousVariables(self.nq, 1, name="tau_ctrl")
# Object accelerations
a_MX = prog.NewContinuousVariables(1, 1, name="a_MX")
a_MT = prog.NewContinuousVariables(1, 1, name="a_MT")
a_MY = prog.NewContinuousVariables(1, 1, name="a_MY")
a_MZ = prog.NewContinuousVariables(1, 1, name="a_MZ")
a_MN = prog.NewContinuousVariables(1, 1, name="a_MN")
a_LT = prog.NewContinuousVariables(1, 1, name="a_LT")
a_LN = prog.NewContinuousVariables(1, 1, name="a_LN")
alpha_MX = prog.NewContinuousVariables(1, 1, name="alpha_MX")
alpha_MY = prog.NewContinuousVariables(1, 1, name="alpha_MY")
alpha_MZ = prog.NewContinuousVariables(1, 1, name="alpha_MZ")
alpha_a_MXYZ = np.array(
[alpha_MX, alpha_MY, alpha_MZ, a_MX, a_MY, a_MZ])[:, :, 0]
# Derived accelerations
dd_theta_L = prog.NewContinuousVariables(1, 1, name="dd_theta_L")
dd_d_N = prog.NewContinuousVariables(1, 1, name="dd_d_N")
dd_d_T = prog.NewContinuousVariables(1, 1, name="dd_d_T")
ddq = prog.NewContinuousVariables(self.nq, 1, name="ddq")
## Constraints
# "set_description" calls gives us useful names for printing
prog.AddConstraint(eq(
self.sys_consts.m_L * a_LT, F_FLT + F_GT +
F_OT)).evaluator().set_description("Link tangential force balance")
prog.AddConstraint(
eq(self.sys_consts.m_L * a_LN, F_NL + F_GN +
F_ON)).evaluator().set_description("Link normal force balance")
prog.AddConstraint(eq(
self.sys_consts.I_L*dd_theta_L, \
(-self.sys_consts.w_L/2)*F_ON - (p_CN-p_LN) * F_FLT + \
(p_CT-p_LT)*F_NL + tau_O
)).evaluator().set_description("Link moment balance")
prog.AddConstraint(eq(
F_NL, -F_NM)).evaluator().set_description("3rd law normal forces")
if self.model_friction:
prog.AddConstraint(eq(F_FMT, -F_FLT)).evaluator().set_description(
"3rd law friction forces (T hat)")
prog.AddConstraint(eq(F_FMX, -F_FLX)).evaluator().set_description(
"3rd law friction forces (X hat)")
prog.AddConstraint(eq(
-dd_theta_L*(self.sys_consts.h_L/2+self.sys_consts.r) + \
d_theta_L**2*self.sys_consts.w_L/2 - a_LT + a_MT,
-dd_theta_L*d_N + dd_d_T - d_theta_L**2*d_T - 2*d_theta_L*d_d_N
)).evaluator().set_description("d_N derivative is derivative")
prog.AddConstraint(eq(
-dd_theta_L*self.sys_consts.w_L/2 - \
d_theta_L**2*self.sys_consts.h_L/2 - \
d_theta_L**2*self.sys_consts.r - a_LN + a_MN,
dd_theta_L*d_T + dd_d_N - d_theta_L**2*d_N + 2*d_theta_L*d_d_T
)).evaluator().set_description("d_N derivative is derivative")
prog.AddConstraint(eq(dd_d_N,
0)).evaluator().set_description("No penetration")
if self.model_friction:
prog.AddConstraint(
eq(F_FLT, mu_S * F_NL * self.sys_consts.mu *
hats_T)).evaluator().set_description(
"Friction relationship LT")
prog.AddConstraint(
eq(F_FLX, mu_S * F_NL * self.sys_consts.mu *
s_hat_X)).evaluator().set_description(
"Friction relationship LX")
if not self.measure_joint_wrench:
prog.AddConstraint(
eq(a_LT, -(self.sys_consts.w_L / 2) * d_theta_L**
2)).evaluator().set_description("Hinge constraint (T hat)")
prog.AddConstraint(eq(a_LN, (self.sys_consts.w_L / 2) *
dd_theta_L)).evaluator().set_description(
"Hinge constraint (N hat)")
for i in range(6):
lhs_i = alpha_a_MXYZ[i, 0]
assert not hasattr(lhs_i, "shape")
rhs_i = (Jdot_qdot + np.matmul(J, ddq))[i, 0]
assert not hasattr(rhs_i, "shape")
prog.AddConstraint(lhs_i == rhs_i).evaluator().set_description(
"Relate manipulator and end effector with joint " + \
"accelerations " + str(i))
tau_contact_trn = np.matmul(J_translational.T, F_ContactM_XYZ)
tau_contact_rot = np.matmul(J_rotational.T,
np.cross(p_MConM, F_ContactM_XYZ, axis=0))
tau_contact = tau_contact_trn + tau_contact_rot
tau_out = tau_ctrl - tau_g + joint_centering_torque
for i in range(self.nq):
M_ddq_row_i = (np.matmul(M, ddq) + Cv)[i, 0]
assert not hasattr(M_ddq_row_i, "shape")
tau_i = (tau_g + tau_contact + tau_out)[i, 0]
assert not hasattr(tau_i, "shape")
prog.AddConstraint(M_ddq_row_i == tau_i).evaluator(
).set_description("Manipulator equations " + str(i))
# Projection equations
prog.AddConstraint(
eq(a_MT,
np.cos(theta_L) * a_MY + np.sin(theta_L) * a_MZ))
prog.AddConstraint(
eq(a_MN, -np.sin(theta_L) * a_MY + np.cos(theta_L) * a_MZ))
prog.AddConstraint(
eq(F_FMT,
np.cos(theta_L) * F_ContactMY + np.sin(theta_L) * F_ContactMZ))
prog.AddConstraint(
eq(F_NM,
-np.sin(theta_L) * F_ContactMY + np.cos(theta_L) * F_ContactMZ))
prog.AddConstraint(dd_d_T[0,
0] == dd_d_Td).evaluator().set_description(
"Desired dd_d_Td constraint" + str(i))
prog.AddConstraint(dd_theta_L[0, 0] == dd_theta_Ld).evaluator(
).set_description("Desired a_LN constraint" + str(i))
prog.AddConstraint(a_MX[0, 0] == a_MX_d).evaluator().set_description(
"Desired a_MX constraint" + str(i))
prog.AddConstraint(alpha_MX[0, 0] == alpha_MXd).evaluator(
).set_description("Desired alpha_MX constraint" + str(i))
prog.AddConstraint(alpha_MY[0, 0] == alpha_MYd).evaluator(
).set_description("Desired alpha_MY constraint" + str(i))
prog.AddConstraint(alpha_MZ[0, 0] == alpha_MZd).evaluator(
).set_description("Desired alpha_MZ constraint" + str(i))
prog.AddConstraint(dd_d_N[0,
0] == dd_d_Nd).evaluator().set_description(
"Desired dd_d_N constraint" + str(i))
result = Solve(prog)
assert result.is_success()
tau_ctrl_result = []
for i in range(self.nq):
tau_ctrl_result.append(
result.GetSolution()[prog.FindDecisionVariableIndex(
tau_ctrl[i, 0])])
tau_ctrl_result = np.expand_dims(tau_ctrl_result, 1)
# ======================== UPDATE DEBUG VALUES ========================
self.debug["times"].append(context.get_time())
# control effort
self.debug["dd_d_Td"].append(dd_d_Td)
self.debug["dd_theta_Ld"].append(dd_theta_Ld)
self.debug["a_MX_d"].append(a_MX_d)
self.debug["alpha_MXd"].append(alpha_MXd)
self.debug["alpha_MYd"].append(alpha_MYd)
self.debug["alpha_MZd"].append(alpha_MZd)
self.debug["dd_d_Nd"].append(dd_d_Nd)
# decision variables
if self.model_friction:
self.debug["F_FMT"].append(
result.GetSolution()[prog.FindDecisionVariableIndex(F_FMT[0,
0])])
self.debug["F_FMX"].append(
result.GetSolution()[prog.FindDecisionVariableIndex(F_FMX[0,
0])])
self.debug["F_FLT"].append(
result.GetSolution()[prog.FindDecisionVariableIndex(F_FLT[0,
0])])
self.debug["F_FLX"].append(
result.GetSolution()[prog.FindDecisionVariableIndex(F_FLX[0,
0])])
else:
self.debug["F_FMT"].append(F_FMT)
self.debug["F_FMX"].append(F_FMX)
self.debug["F_FLT"].append(F_FLT)
self.debug["F_FLX"].append(F_FLX)
self.debug["F_NM"].append(
result.GetSolution()[prog.FindDecisionVariableIndex(F_NM[0, 0])])
self.debug["F_ContactMY"].append(
result.GetSolution()[prog.FindDecisionVariableIndex(
F_ContactMY[0, 0])])
self.debug["F_ContactMZ"].append(
result.GetSolution()[prog.FindDecisionVariableIndex(
F_ContactMZ[0, 0])])
self.debug["F_NL"].append(
result.GetSolution()[prog.FindDecisionVariableIndex(F_NL[0, 0])])
for i in range(self.nq):
self.debug["tau_ctrl_" + str(i)].append(
result.GetSolution()[prog.FindDecisionVariableIndex(
tau_ctrl[i, 0])])
self.debug["a_MX"].append(
result.GetSolution()[prog.FindDecisionVariableIndex(a_MX[0, 0])])
self.debug["a_MT"].append(
result.GetSolution()[prog.FindDecisionVariableIndex(a_MT[0, 0])])
self.debug["a_MY"].append(
result.GetSolution()[prog.FindDecisionVariableIndex(a_MY[0, 0])])
self.debug["a_MZ"].append(
result.GetSolution()[prog.FindDecisionVariableIndex(a_MZ[0, 0])])
self.debug["a_MN"].append(
result.GetSolution()[prog.FindDecisionVariableIndex(a_MN[0, 0])])
self.debug["a_LT"].append(
result.GetSolution()[prog.FindDecisionVariableIndex(a_LT[0, 0])])
self.debug["a_LN"].append(
result.GetSolution()[prog.FindDecisionVariableIndex(a_LN[0, 0])])
self.debug["alpha_MX"].append(
result.GetSolution()[prog.FindDecisionVariableIndex(alpha_MX[0,
0])])
self.debug["alpha_MY"].append(
result.GetSolution()[prog.FindDecisionVariableIndex(alpha_MY[0,
0])])
self.debug["alpha_MZ"].append(
result.GetSolution()[prog.FindDecisionVariableIndex(alpha_MZ[0,
0])])
self.debug["dd_theta_L"].append(
result.GetSolution()[prog.FindDecisionVariableIndex(
dd_theta_L[0, 0])])
self.debug["dd_d_N"].append(
result.GetSolution()[prog.FindDecisionVariableIndex(dd_d_N[0, 0])])
self.debug["dd_d_T"].append(
result.GetSolution()[prog.FindDecisionVariableIndex(dd_d_T[0, 0])])
for i in range(self.nq):
self.debug["ddq_" + str(i)].append(
result.GetSolution()[prog.FindDecisionVariableIndex(ddq[i,
0])])
self.debug["theta_MXd"].append(theta_MXd)
output.SetFromVector(tau_ctrl_result.flatten())
def static_controller(self, qref, verbose=False):
"""
Generates a controller to maintain a static pose
Arguments:
qref: (N,) numpy array, the static pose to be maintained
Return Values:
u: (M,) numpy array, actuations to best achieve static pose
f: (C,) numpy array, associated normal reaction forces
static_controller generates the actuations and reaction forces assuming the velocity and accelerations are zero. Thus, the equation to solve is:
N(q) = B*u + J*f
where N is a vector of gravitational and conservative generalized forces, B is the actuation selection matrix, and J is the contact-Jacobian transpose.
Currently, static_controller only considers the effects of the normal forces. Frictional forces are not yet supported
"""
#TODO: Solve for friction forces as well
# Check inputs
if qref.ndim > 1 or qref.shape[0] != self.multibody.num_positions():
raise ValueError(
f"Reference position mut be ({self.multibody.num_positions(),},) array"
)
# Set the context
context = self.multibody.CreateDefaultContext()
self.multibody.SetPositions(context, qref)
# Get the necessary properties
G = self.multibody.CalcGravityGeneralizedForces(context)
Jn, _ = self.GetContactJacobians(context)
phi = self.GetNormalDistances(context)
B = self.multibody.MakeActuationMatrix()
#Collect terms
A = np.concatenate([B, Jn.transpose()], axis=1)
# Create the mathematical program
prog = MathematicalProgram()
l_var = prog.NewContinuousVariables(self.num_contacts(), name="forces")
u_var = prog.NewContinuousVariables(self.multibody.num_actuators(),
name="controls")
# Ensure dynamics approximately satisfied
prog.AddL2NormCost(A=A,
b=-G,
vars=np.concatenate([u_var, l_var], axis=0))
# Enforce normal complementarity
prog.AddBoundingBoxConstraint(np.zeros(l_var.shape),
np.full(l_var.shape, np.inf), l_var)
prog.AddConstraint(phi.dot(l_var) == 0)
# Solve
result = Solve(prog)
# Check for a solution
if result.is_success():
u = result.GetSolution(u_var)
f = result.GetSolution(l_var)
if verbose:
printProgramReport(result, prog)
return (u, f)
else:
print(f"Optimization failed. Returning zeros")
if verbose:
printProgramReport(result, prog)
return (np.zeros(u_var.shape), np.zeros(l_var.shape))
x[:, N - 1] = prog.NewContinuousVariables(2, 'x' + str(N))
# Add constraints at every knot point
x0 = [-2, 0]
prog.AddBoundingBoxConstraint(x0, x0, x[:, 0])
for n in range(0, N - 1):
# Add the dynamics as an equality constraint
# prog.AddConstraint(eq(x[:,n+1], A.dot(x[:,n]) + B.dot(u[:,n])))
# Add the dynamics as a function handle constraint
prog.AddConstraint(dynamicsCstr,
lb=np.array([0., 0.]),
ub=np.array([0., 0.]),
vars=np.concatenate((x[:, n], u[:, n], x[:, n + 1]),
axis=0),
description="dynamics")
prog.AddBoundingBoxConstraint(-1, 1, u[:, n])
xf = [0, 0]
prog.AddBoundingBoxConstraint(xf, xf, x[:, N - 1])
# Solve the problem
result = Solve(prog)
x_sol = result.GetSolution(x)
print(f"Optimization successful? {result.is_success()}")
# Display the optimized trajectories
plt.figure()
plt.plot(x_sol[0, :], x_sol[1, :])
plt.xlabel('q')
plt.ylabel('qdot')
plt.show()
def compute_trajectory_to_other_world(self, state_initial, minimum_time,
maximum_time):
'''
Your mission is to implement this function.
A successful implementation of this function will compute a dynamically feasible trajectory
which satisfies these criteria:
- Efficiently conserve fuel
- Reach "orbit" of the far right world
- Approximately obey the dynamic constraints
- Begin at the state_initial provided
- Take no more than maximum_time, no less than minimum_time
The above are defined more precisely in the provided notebook.
Please note there are two return args.
:param: state_initial: :param state_initial: numpy array of length 4, see rocket_dynamics for documentation
:param: minimum_time: float, minimum time allowed for trajectory
:param: maximum_time: float, maximum time allowed for trajectory
:return: three return args separated by commas:
trajectory, input_trajectory, time_array
trajectory: a 2d array with N rows, and 4 columns. See simulate_states_over_time for more documentation.
input_trajectory: a 2d array with N-1 row, and 2 columns. See simulate_states_over_time for more documentation.
time_array: an array with N rows.
'''
# length of horizon (excluding init state)
N = 50
trajectory = np.zeros((N + 1, 4))
input_trajectory = np.ones((N, 2)) * 10.0
### My implementation of Direct Transcription
# (states and control are all decision vars using Euler integration)
mp = MathematicalProgram()
# let trajectory duration be a decision var
total_time = mp.NewContinuousVariables(1, "total_time")
dt = total_time[0] / N
# create the control decision var (m*N) and state decision var (n*[N+1])
idx = 0
u_list = mp.NewContinuousVariables(2, "u_{}".format(idx))
state_list = mp.NewContinuousVariables(4, "state_{}".format(idx))
state_list = np.vstack(
(state_list,
mp.NewContinuousVariables(4, "state_{}".format(idx + 1))))
for idx in range(1, N):
u_list = np.vstack(
(u_list, mp.NewContinuousVariables(2, "u_{}".format(idx))))
state_list = np.vstack(
(state_list,
mp.NewContinuousVariables(4, "state_{}".format(idx + 1))))
### Constraints
# set init state as constraint on stage 0 decision vars
for state_idx in range(4):
mp.AddLinearConstraint(
state_list[0, state_idx] == state_initial[state_idx])
# interstage equality constraint on state vars via Euler integration
# note: Direct Collocation could improve accuracy for same computation
for idx in range(1, N + 1):
state_new = state_list[idx - 1, :] + dt * self.rocket_dynamics(
state_list[idx - 1, :], u_list[idx - 1, :])
for state_idx in range(4):
mp.AddConstraint(state_list[idx,
state_idx] == state_new[state_idx])
# constraint on time
mp.AddLinearConstraint(total_time[0] <= maximum_time)
mp.AddLinearConstraint(total_time[0] >= minimum_time)
# constraint on final state distance (squared)to second planet
final_dist_to_world_2_sq = (
self.world_2_position[0] - state_list[-1, 0])**2 + (
self.world_2_position[1] - state_list[-1, 1])**2
mp.AddConstraint(final_dist_to_world_2_sq <= 0.25)
# constraint on final state speed (squared
final_speed_sq = state_list[-1, 2]**2 + state_list[-1, 3]**2
mp.AddConstraint(final_speed_sq <= 1)
### Cost
# equal cost on vertical/horizontal accels, weight shouldn't matter since this is the only cost
mp.AddQuadraticCost(1 * u_list[:, 0].dot(u_list[:, 0]))
mp.AddQuadraticCost(1 * u_list[:, 1].dot(u_list[:, 1]))
### Solve and parse
result = Solve(mp)
trajectory = result.GetSolution(state_list)
input_trajectory = result.GetSolution(u_list)
tf = result.GetSolution(total_time)
time_array = np.linspace(0, tf[0], N + 1)
print "optimization successful: ", result.is_success()
print "total num decision vars (x: (N+1)*4, u: 2N, total_time: 1): {}".format(
mp.num_vars())
print "solver used: ", result.get_solver_id().name()
print "optimal trajectory time: {:.2f} sec".format(tf[0])
return trajectory, input_trajectory, time_array
# Use IK to find a nonpenetrating configuration of the scene from
# which to start simulation.
q0 = mbp.GetPositions(mbp_context).copy()
ik = InverseKinematics(mbp, mbp_context)
q_dec = ik.q()
prog = ik.prog()
constraint = ik.AddMinimumDistanceConstraint(0.01)
prog.AddQuadraticErrorCost(np.eye(q0.shape[0])*1.0, q0, q_dec)
mbp.SetPositions(mbp_context, q0)
prog.SetInitialGuess(q_dec, q0)
print("Solving")
print("Initial guess: ", q0)
res = Solve(prog)
#print(prog.GetSolverId().name())
q0_proj = res.GetSolution(q_dec)
print("Final: ", q0_proj)
# Run a sim starting from whatever configuration IK figured out.
mbp.SetPositions(mbp_context, q0_proj)
simulator = Simulator(diagram, diagram_context)
simulator.set_publish_every_time_step(False)
simulator.set_target_realtime_rate(1.0)
try:
simulator.AdvanceTo(2.0)
except Exception as e:
print("Exception in sim: ", e)
scene_k -= 1