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 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 is_verified(rho):
# initialize optimization problem
# (with Drake there is no need to specify that
# this is going to be a SOS program!)
prog = MathematicalProgram()
# SOS indeterminates
x = prog.NewIndeterminates(2, 'x')
# Lyapunov function
V = x.dot(P).dot(x)
V_dot = 2*x.dot(P).dot(f(x))
# degree of the polynomial lambda(x)
# no need to change it, but if you really want to,
# keep l_deg even (why?) and do not set l_deg greater than 10
# (otherwise optimizations will take forever)
l_deg = 4
assert l_deg % 2 == 0
# SOS Lagrange multipliers
l = prog.NewSosPolynomial(Variables(x), l_deg)[0].ToExpression()
# main condition above
eps = 1e-3 # do not change
prog.AddSosConstraint(- V_dot - l * (rho - V) - eps*x.dot(x))
# solve SOS program
# no objective function in this formulation
result = Solve(prog)
# return True if feasible, False if infeasible
return result.is_success()
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 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)
def runDircol(self,x0,xf,tf0):
N = 15 # constant
#N = np.int(tf0 * 10) # "10Hz" / samples per second
context = self.CreateDefaultContext()
dircol = DirectCollocation(self, context, num_time_samples=N,
minimum_timestep=0.05, maximum_timestep=1.0)
u = dircol.input()
# set some constraints on inputs
dircol.AddEqualTimeIntervalsConstraints()
dircol.AddConstraintToAllKnotPoints(u[1] <= self.slewmax)
dircol.AddConstraintToAllKnotPoints(u[1] >= -self.slewmax)
dircol.AddConstraintToAllKnotPoints(u[0] <= self.umax)
dircol.AddConstraintToAllKnotPoints(u[0] >= -self.umax)
# constrain the last input to be zero (at least for the u input)
#import pdb; pdb.set_trace()
dv = dircol.decision_variables()
for i in range(3, self.nX*N, 4):
alfa_state = dv[i] #u[t_end]
dircol.AddBoundingBoxConstraint(-self.alfamax, self.alfamax, alfa_state)
#final_u_decision_var = dv[self.nX*N + self.nU*N - 1] #u[t_end]
#dircol.AddLinearEqualityConstraint(final_u_decision_var, 0.0)
#first_u_decision_var = dv[self.nX*N + 1 ] #u[t_0]
#dircol.AddLinearEqualityConstraint(first_u_decision_var, 0.0)
# set some constraints on start and final pose
eps = 0.0 * np.ones(self.nX) # relaxing factor
dircol.AddBoundingBoxConstraint(x0, x0, dircol.initial_state())
dircol.AddBoundingBoxConstraint(xf-eps, \
xf+eps, dircol.final_state())
R = 1.0*np.eye(self.nU) # Cost on input "effort".
dircol.AddRunningCost( u.transpose().dot(R.dot(u)) )
# Add a final cost equal to the total duration.
dircol.AddFinalCost(dircol.time())
# guess initial trajectory
initial_x_trajectory = \
PiecewisePolynomial.FirstOrderHold([0., tf0], np.column_stack((x0, xf)))
dircol.SetInitialTrajectory(PiecewisePolynomial(), initial_x_trajectory)
# optimize
result = Solve(dircol)
print('******\nRunning trajectory optimization:')
print('w/ solver %s' %(result.get_solver_id().name()))
print(result.get_solution_result())
assert(result.is_success())
xtraj = dircol.ReconstructStateTrajectory(result)
utraj = dircol.ReconstructInputTrajectory(result)
# return nominal trajectory
return utraj,xtraj
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 runDircol(self, x0, xf, tf0):
N = 21 #np.int(tf0 * 10) # "10Hz" samples per second
context = self.CreateDefaultContext()
dircol = DirectCollocation(self,
context,
num_time_samples=N,
minimum_timestep=0.05,
maximum_timestep=1.0)
u = dircol.input()
dircol.AddEqualTimeIntervalsConstraints()
dircol.AddConstraintToAllKnotPoints(u[0] <= 0.5 * self.omegamax)
dircol.AddConstraintToAllKnotPoints(u[0] >= -0.5 * self.omegamax)
dircol.AddConstraintToAllKnotPoints(u[1] <= 0.5 * self.umax)
dircol.AddConstraintToAllKnotPoints(u[1] >= -0.5 * self.umax)
eps = 0.0
dircol.AddBoundingBoxConstraint(x0, x0, dircol.initial_state())
dircol.AddBoundingBoxConstraint(xf - np.array([eps, eps, eps]),
xf + np.array([eps, eps, eps]),
dircol.final_state())
R = 1.0 * np.eye(2) # Cost on input "effort".
dircol.AddRunningCost(u.transpose().dot(R.dot(u)))
#dircol.AddRunningCost(R*u[0]**2)
# Add a final cost equal to the total duration.
dircol.AddFinalCost(dircol.time())
initial_x_trajectory = \
PiecewisePolynomial.FirstOrderHold([0., tf0], np.column_stack((x0, xf)))
dircol.SetInitialTrajectory(PiecewisePolynomial(),
initial_x_trajectory)
result = Solve(dircol)
print(result.get_solver_id().name())
print(result.get_solution_result())
assert (result.is_success())
#import pdb; pdb.set_trace()
xtraj = dircol.ReconstructStateTrajectory(result)
utraj = dircol.ReconstructInputTrajectory(result)
return utraj, xtraj
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 min_time_bounce_kick_traj(self, p0, v0, p0_puck, v0_puck, v_puck_desired):
"""Use direct transcription to calculate player's trajectory for bounce kick. The elastic collision is enforced
when robot reaches the desired position at specified time."""
T = 1
x0 = np.concatenate((p0, v0), axis=0)
prog = DirectTranscription(self.sys, self.sys.CreateDefaultContext(), int(T/self.params.dt))
prog.AddBoundingBoxConstraint(x0, x0, prog.initial_state())
self.add_final_state_constraint_elastic_collision(prog, p0_puck, v0_puck, v_puck_desired)
self.add_input_limits(prog)
self.add_arena_limits(prog)
prog.AddFinalCost(prog.time())
result = Solve(prog)
u_sol = prog.ReconstructInputTrajectory(result)
if not result.is_success():
print("Minimum time bounce kick trajectory: optimization failed")
return False, np.zeros((2, 1))
u_values = u_sol.vector_values(u_sol.get_segment_times())
return result.is_success(), u_values
def min_time_traj_transcription(self, p0, v0, pf, vf, xlim=None, ylim=None):
"""Minimum time traj using directi transcription."""
T = 2
N = int(T/self.params.dt)
x0 = np.concatenate((p0, v0), axis=0)
xf = np.concatenate((pf, vf), axis=0)
prog = DirectTranscription(self.sys, self.sys.CreateDefaultContext(), N)
prog.AddBoundingBoxConstraint(x0, x0, prog.initial_state()) # initial states
prog.AddBoundingBoxConstraint(xf, xf, prog.final_state())
self.add_input_limits(prog)
self.add_arena_limits(prog)
prog.AddFinalCost(prog.time())
result = Solve(prog)
u_sol = prog.ReconstructInputTrajectory(result)
if not result.is_success():
print("Minimum time trajectory: optimization failed")
return False, np.zeros((2, 1))
u_values = u_sol.vector_values(u_sol.get_segment_times())
return result.is_success(), u_values
def intercepting_traj(self, p0, v0, pf, vf, T):
"""Trajectory planning given initial state, final state, and final time."""
x0 = np.concatenate((p0, v0), axis=0)
xf = np.concatenate((pf, vf), axis=0)
prog = DirectTranscription(self.sys, self.sys.CreateDefaultContext(), int(T/self.params.dt))
prog.AddBoundingBoxConstraint(x0, x0, prog.initial_state())
prog.AddBoundingBoxConstraint(xf, xf, prog.final_state())
self.add_input_limits(prog)
self.add_arena_limits(prog)
# cost
prog.AddRunningCost(prog.input()[0]*prog.input()[0] + prog.input()[1]*prog.input()[1])
# solve optimization
result = Solve(prog)
u_sol = prog.ReconstructInputTrajectory(result)
if not result.is_success():
print("Intercepting trajectory: optimization failed")
return False, np.zeros((2, 1))
u_values = u_sol.vector_values(u_sol.get_segment_times())
return result.is_success(), u_values
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 drake_trajectory_generation(file_name):
global x_cmd_drake
global u_cmd_drake
print(file_name)
Parser(plant).AddModelFromFile(file_name)
plant.Finalize()
context = plant.CreateDefaultContext()
global dircol
dircol= DirectCollocation(
plant,
context,
num_time_samples=11,
minimum_timestep=0.1,
maximum_timestep=0.4,
input_port_index=plant.get_actuation_input_port().get_index())
dircol.AddEqualTimeIntervalsConstraints()
initial_state = (0., 0., 0., 0.)
dircol.AddBoundingBoxConstraint(initial_state, initial_state,
dircol.initial_state())
final_state = (0., math.pi, 0., 0.)
dircol.AddBoundingBoxConstraint(final_state, final_state, dircol.final_state())
R = 10 # Cost on input "effort".weight
u = dircol.input()
dircol.AddRunningCost(R * u[0]**2)
# Add a final cost equal to the total duration.
dircol.AddFinalCost(dircol.time())
initial_x_trajectory = PiecewisePolynomial.FirstOrderHold(
[0., 4.], np.column_stack((initial_state, final_state))) # yapf: disable
dircol.SetInitialTrajectory(PiecewisePolynomial(), initial_x_trajectory)
dircol.AddConstraintToAllKnotPoints(dircol.input()[1] <= 0)
dircol.AddConstraintToAllKnotPoints(dircol.input()[1] >= 0)
global result
global u_values
result = Solve(dircol)
assert result.is_success()
#plotphase_portrait()
fig1, ax1 = plt.subplots()
u_trajectory = dircol.ReconstructInputTrajectory(result)
u_knots = np.hstack([
u_trajectory.value(t) for t in np.linspace(u_trajectory.start_time(),
u_trajectory.end_time(), 400)
])#here the u_knots now is 2x400
#u_trajectory = dircol.ReconstructInputTrajectory(result)
times = np.linspace(u_trajectory.start_time(), u_trajectory.end_time(), 400)
#u_lookup = np.vectorize(u_trajectory.value)
#now we have ndarray of u_values with 400 points for 4 seconds w/ 100hz pub frequency
#u_values = u_lookup(times)
#ax1.plot(times, u_values)
ax1.plot(times, u_knots[0])
ax1.plot(times, u_knots[1])
ax1.set_xlabel("time (seconds)")
ax1.set_ylabel("force (Newtons)")
ax1.set_title(' Direct collocation for Cartpole ')
print('here2')
plt.show()
print('here3')
#x_knots = np.hstack([
# x_trajectory.value(t) for t in np.linspace(x_trajectory.start_time(),
# x_trajectory.end_time(), 100)
#])
x_trajectory = dircol.ReconstructStateTrajectory(result)
x_knots = np.hstack([
x_trajectory.value(t) for t in np.linspace(x_trajectory.start_time(),
x_trajectory.end_time(), 400)
])
print(x_trajectory.start_time())
print(x_trajectory.end_time())
fig, ax = plt.subplots(4,1,figsize=(8,8))
plt.subplots_adjust(wspace =0, hspace =0.4)
#plt.tight_layout(3)#adjust total space
ax[0].set_title('state of direct collocation for Cartpole')
ax[0].plot(x_knots[0, :], x_knots[2, :], linewidth=2, color='b', linestyle='-')
ax[0].set_xlabel("state_dot(theta1_dot and t|heta2_dot)")
ax[0].set_ylabel("state(theta1 and theta2)");
ax[0].plot(x_knots[1, :], x_knots[3, :],color='r',linewidth=2,linestyle='--')
ax[0].legend(('theta1&theta1dot','theta2&theta2dot'));
ax[1].set_title('input u(t) of direct collocation for Cartpole')
# ax[1].plot(times,u_values, 'g')
ax[1].plot(times, u_knots[0])
ax[1].plot(times, u_knots[1])
ax[1].legend(('input u(t)'))
ax[1].set_xlabel("time")
ax[1].set_ylabel("u(t)")
ax[1].legend(('x joint ','thetajoint'))
ax[1].set_title('input x(t) of direct collocation for Cartpole')
ax[2].plot(times, x_knots[0, :])
ax[2].set_xlabel("time")
ax[2].set_ylabel("x(t)")
ax[2].set_title('input theta(t) of direct collocation for Cartpole')
ax[3].set_title('input theta(t) of direct collocation for Cartpole')
ax[3].plot(times, x_knots[1, :])
ax[3].set_xlabel("time")
ax[3].set_ylabel("theta(t)")
print('here4')
plt.show()
print('here5')
x_cmd_drake=x_knots
#return x_knots[0, :]#u_values
# u_cmd_drake=u_values
u_cmd_drake=u_knots
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())
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
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?
# 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
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)
from pydrake.all import MathematicalProgram, Solve
prog = MathematicalProgram()
x = prog.NewIndeterminates(2, "x")
f = [-x[0] - 2 * x[1]**2, -x[1] - x[0] * x[1] - 2 * x[1]**3]
V = x[0]**2 + 2 * x[1]**2
Vdot = V.Jacobian(x).dot(f)
prog.AddSosConstraint(-Vdot)
result = Solve(prog)
assert result.is_success()
print("Successfully verified Lyapunov candidate")
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 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
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
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
dircol.SetInitialTrajectory(PiecewisePolynomial(), initial_x_trajectory)
fig = plt.figure()
h, = plt.plot([], [], ".-")
plt.xlim((-2.5, 2.5))
plt.ylim((-3., 3.))
def draw_trajectory(t, x):
h.set_xdata(x[0, :])
h.set_ydata(x[1, :])
fig.canvas.draw()
if plt.get_backend() == u"MacOSX":
plt.pause(1e-10)
dircol.AddStateTrajectoryCallback(draw_trajectory)
result = Solve(dircol)
assert result.is_success()
x_trajectory = dircol.ReconstructStateTrajectory(result)
x_knots = np.hstack([
x_trajectory.value(t) for t in np.linspace(x_trajectory.start_time(),
x_trajectory.end_time(), 100)
])
plt.plot(x_knots[0, :], x_knots[1, :])
plt.show()
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 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))
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 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
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