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 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 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 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 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 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 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 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 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))
# 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.')
class FourierCollocationProblem:
def __init__(self, system_dynamics, constraints):
start_time = time.time()
self.prog = MathematicalProgram()
self.N = 50 # Number of collocation points
self.M = 10 # Number of frequencies
self.system_dynamics = system_dynamics
self.psi = np.pi * (0.7) # TODO change
(
min_height,
max_height,
min_vel,
max_vel,
h0,
min_travelled_distance,
t_f_min,
t_f_max,
avg_vel_min,
avg_vel_max,
) = constraints
initial_pos = np.array([0, 0, h0])
# Add state trajectory parameters as decision variables
self.coeffs = self.prog.NewContinuousVariables(
3, self.M + 1, "c"
) # (x,y,z) for every frequency
self.phase_delays = self.prog.NewContinuousVariables(3, self.M, "eta")
self.t_f = self.prog.NewContinuousVariables(1, 1, "t_f")[0, 0]
self.avg_vel = self.prog.NewContinuousVariables(1, 1, "V_bar")[0, 0]
# Enforce initial conditions
residuals = self.get_pos_fourier(collocation_time=0) - initial_pos
for residual in residuals:
self.prog.AddConstraint(residual == 0)
# Enforce dynamics at collocation points
for n in range(self.N):
collocation_time = (n / self.N) * self.t_f
pos = self.get_pos_fourier(collocation_time)
vel = self.get_vel_fourier(collocation_time)
vel_dot = self.get_vel_dot_fourier(collocation_time)
residuals = self.continuous_dynamics(pos, vel, vel_dot)
for residual in residuals[3:6]: # TODO only these last three are residuals
self.prog.AddConstraint(residual == 0)
if True:
# Add velocity constraints
squared_vel_norm = vel.T.dot(vel)
self.prog.AddConstraint(min_vel ** 2 <= squared_vel_norm)
self.prog.AddConstraint(squared_vel_norm <= max_vel ** 2)
# Add height constraints
self.prog.AddConstraint(pos[2] <= max_height)
self.prog.AddConstraint(min_height <= pos[2])
# Add constraint on min travelled distance
self.prog.AddConstraint(min_travelled_distance <= self.avg_vel * self.t_f)
# Add constraints on coefficients
if False:
for coeff in self.coeffs.T:
lb = np.array([-500, -500, -500])
ub = np.array([500, 500, 500])
self.prog.AddBoundingBoxConstraint(lb, ub, coeff)
# Add constraints on phase delays
if False:
for etas in self.phase_delays.T:
lb = np.array([0, 0, 0])
ub = np.array([2 * np.pi, 2 * np.pi, 2 * np.pi])
self.prog.AddBoundingBoxConstraint(lb, ub, etas)
# Add constraints on final time and avg vel
self.prog.AddBoundingBoxConstraint(t_f_min, t_f_max, self.t_f)
self.prog.AddBoundingBoxConstraint(avg_vel_min, avg_vel_max, self.avg_vel)
# Add objective function
self.prog.AddCost(-self.avg_vel)
# Set initial guess
coeffs_guess = np.zeros((3, self.M + 1))
coeffs_guess += np.random.rand(*coeffs_guess.shape) * 100
self.prog.SetInitialGuess(self.coeffs, coeffs_guess)
phase_delays_guess = np.zeros((3, self.M))
phase_delays_guess += np.random.rand(*phase_delays_guess.shape) * 1e-1
self.prog.SetInitialGuess(self.phase_delays, phase_delays_guess)
self.prog.SetInitialGuess(self.avg_vel, (avg_vel_max - avg_vel_min) / 2)
self.prog.SetInitialGuess(self.t_f, (t_f_max - t_f_min) / 2)
print(
"Finished formulating the optimization problem in: {0} s".format(
time.time() - start_time
)
)
start_solve_time = time.time()
self.result = Solve(self.prog)
print("Found solution: {0}".format(self.result.is_success()))
print("Found a solution in: {0} s".format(time.time() - start_solve_time))
# TODO input costs
# assert self.result.is_success()
return
def get_solution(self):
coeffs_opt = self.result.GetSolution(self.coeffs)
phase_delays_opt = self.result.GetSolution(self.phase_delays)
t_f_opt = self.result.GetSolution(self.t_f)
avg_vel_opt = self.result.GetSolution(self.avg_vel)
sim_N = 100
dt = t_f_opt / sim_N
times = np.arange(0, t_f_opt, dt)
pos_traj = np.zeros((3, sim_N))
# TODO remove vel traj
vel_traj = np.zeros((3, sim_N))
for i in range(sim_N):
t = times[i]
pos = self.evaluate_pos_traj(
coeffs_opt, phase_delays_opt, t_f_opt, avg_vel_opt, t
)
pos_traj[:, i] = pos
vel = self.evaluate_vel_traj(
coeffs_opt, phase_delays_opt, t_f_opt, avg_vel_opt, t
)
vel_traj[:, i] = vel
# TODO move plotting out
plot_trj_3_wind(pos_traj.T, np.array([np.sin(self.psi), np.cos(self.psi), 0]))
plt.show()
breakpoint()
return
def evaluate_pos_traj(self, coeffs, phase_delays, t_f, avg_vel, t):
pos_traj = np.copy(coeffs[:, 0])
for m in range(1, self.M):
sine_terms = np.array(
[
np.sin((2 * np.pi * m * t) / t_f + phase_delays[0, m]),
np.sin((2 * np.pi * m * t) / t_f + phase_delays[1, m]),
np.sin((2 * np.pi * m * t) / t_f + phase_delays[2, m]),
]
)
pos_traj += coeffs[:, m] * sine_terms
direction_term = np.array(
[
avg_vel * np.sin(self.psi) * t,
avg_vel * np.cos(self.psi) * t,
0,
]
)
pos_traj += direction_term
return pos_traj
def evaluate_vel_traj(self, coeffs, phase_delays, t_f, avg_vel, t):
vel_traj = np.array([0, 0, 0], dtype=object)
for m in range(1, self.M):
cos_terms = np.array(
[
(2 * np.pi * m)
/ t_f
* np.cos((2 * np.pi * m * t) / t_f + phase_delays[0, m]),
(2 * np.pi * m)
/ t_f
* np.cos((2 * np.pi * m * t) / t_f + phase_delays[1, m]),
(2 * np.pi * m)
/ t_f
* np.cos((2 * np.pi * m * t) / t_f + phase_delays[2, m]),
]
)
vel_traj += coeffs[:, m] * cos_terms
direction_term = np.array(
[
avg_vel * np.sin(self.psi),
avg_vel * np.cos(self.psi),
0,
]
)
vel_traj += direction_term
return vel_traj
def get_pos_fourier(self, collocation_time):
pos_traj = np.copy(self.coeffs[:, 0])
for m in range(1, self.M):
a = (2 * np.pi * m) / self.t_f
sine_terms = np.array(
[
np.sin(a * collocation_time + self.phase_delays[0, m]),
np.sin(a * collocation_time + self.phase_delays[1, m]),
np.sin(a * collocation_time + self.phase_delays[2, m]),
]
)
pos_traj += self.coeffs[:, m] * sine_terms
direction_term = np.array(
[
self.avg_vel * np.sin(self.psi) * collocation_time,
self.avg_vel * np.cos(self.psi) * collocation_time,
0,
]
)
pos_traj += direction_term
return pos_traj
def get_vel_fourier(self, collocation_time):
vel_traj = np.array([0, 0, 0], dtype=object)
for m in range(1, self.M):
a = (2 * np.pi * m) / self.t_f
cos_terms = np.array(
[
a * np.cos(a * collocation_time + self.phase_delays[0, m]),
a * np.cos(a * collocation_time + self.phase_delays[1, m]),
a * np.cos(a * collocation_time + self.phase_delays[2, m]),
]
)
vel_traj += self.coeffs[:, m] * cos_terms
direction_term = np.array(
[
self.avg_vel * np.sin(self.psi),
self.avg_vel * np.cos(self.psi),
0,
]
)
vel_traj += direction_term
return vel_traj
def get_vel_dot_fourier(self, collocation_time):
vel_dot_traj = np.array([0, 0, 0], dtype=object)
for m in range(1, self.M):
a = (2 * np.pi * m) / self.t_f
sine_terms = np.array(
[
-(a ** 2) * np.sin(a * collocation_time + self.phase_delays[0, m]),
-(a ** 2) * np.sin(a * collocation_time + self.phase_delays[1, m]),
-(a ** 2) * np.sin(a * collocation_time + self.phase_delays[2, m]),
]
)
vel_dot_traj += self.coeffs[:, m] * sine_terms
return vel_dot_traj
def continuous_dynamics(self, pos, vel, vel_dot):
x = np.concatenate((pos, vel))
x_dot = np.concatenate((vel, vel_dot))
# TODO need to somehow implement input to make this work
return x_dot - self.system_dynamics(x, u)
dircol.AddBoundingBoxConstraint(final_state.get_value(),
final_state.get_value(),
dircol.final_state())
# dircol.AddLinearConstraint(dircol.final_state() == final_state.get_value())
R = 10 # Cost on input "effort".
dircol.AddRunningCost(R*u[0]**2)
initial_x_trajectory = \
PiecewisePolynomial.FirstOrderHold([0., 4.],
[initial_state.get_value(),
final_state.get_value()])
dircol.SetInitialTrajectory(PiecewisePolynomial(), initial_x_trajectory)
result = Solve(dircol)
assert result.is_success()
x_trajectory = dircol.ReconstructStateTrajectory(result)
fig, ax = plt.subplots(1, 2)
vis = PendulumVisualizer(ax[0])
ani = vis.animate(x_trajectory, repeat=True)
x_knots = np.hstack([x_trajectory.value(t) for t in
np.linspace(x_trajectory.start_time(),
x_trajectory.end_time(), 100)])
ax[1].plot(x_knots[0, :], x_knots[1, :])
plt.show()
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)
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 achieves_force_closure(points, normals, mu, debug=False):
"""
Determines whether the given forces achieve force closure.
Note that each of points and normals are lists of variable
length, but should be the same length.
Here's an example valid set of input args:
points = [np.asarray([0.1, 0.2]), np.asarray([0.5,-0.1])]
normals = [np.asarray([-1.0,0.0]), np.asarray([0.0, 1.0])]
mu = 0.2
achieves_force_closure(points, normals, mu)
NOTE: your normals should be normalized (be unit vectors)
:param points: a list of arrays where each array is the
position of the force points relative to the center of mass.
:type points: a list of 1-d numpy arrays of shape (2,)
:param normals: a list of arrays where each array is the normal
of the local surface of the object, pointing in towards
the object
:type normals: a list of 1-d numpy arrays of shape (2,)
:param mu: coulombic kinetic friction coefficient
:type mu: float, greater than 0.
:return: whether or not the given parameters achieves force closure
:rtype: bool (True or False)
"""
assert len(points) == len(normals)
assert mu >= 0.0
assert_unit_normals(normals)
## YOUR CODE HERE
G = get_G(points, normals)
if not is_full_row_rank(G):
print("G is not full row rank")
return False
N_points = len(points)
max_force = 1000
mp = MathematicalProgram()
## Decision vars
# force tangent to surface
forces_x = mp.NewContinuousVariables(N_points, "forces_x")
# force normal to surface
forces_z = mp.NewContinuousVariables(N_points, "forces_z")
# -1<=slack_var<=0 used to convert inequalities to strict inequalities
slack_var = mp.NewContinuousVariables(1, "slack_var")
# put force vars in single array
forces = [None] * 2 * N_points
for point_idx in range(N_points):
forces[point_idx * 2] = forces_x[point_idx]
forces[point_idx * 2 + 1] = forces_z[point_idx]
# ensure forces/moments add to zero
for row_idx in range(np.shape(G)[0]):
# 3 rows (LMB_x, LMB_z, AMB) = 0 for static system
mp.AddLinearConstraint(G[row_idx, :].dot(forces) == 0)
if debug:
print("Static force/moment constraints = 0:")
print(G[row_idx, :].dot(forces))
# ensure forces stay within friction cone and use slack to avoid trivial 0 solution
for point_idx in range(N_points):
# normal force must be positive (slack allows us to catch if it's zero)
mp.AddLinearConstraint(forces_z[point_idx] + slack_var[0] >= 0)
mp.AddLinearConstraint(
forces_x[point_idx] <= mu * forces_z[point_idx] + slack_var[0])
mp.AddLinearConstraint(
forces_x[point_idx] >= -(mu * forces_z[point_idx] + slack_var[0]))
# restrict slack
mp.AddLinearConstraint(slack_var[0] <= 0)
mp.AddLinearConstraint(slack_var[0] >= -1)
mp.AddLinearCost(slack_var[0])
# restrict solution within bounds
for force in forces:
mp.AddLinearConstraint(force >= -max_force)
mp.AddLinearConstraint(force <= max_force)
result = Solve(mp)
if (not result.is_success()):
print("solver failed to find solution")
return False
gamma = result.GetSolution(slack_var)
if (gamma < 0):
print("acheived force closure, gamma = {}".format(gamma))
if debug:
x = result.GetSolution(forces_x)
z = result.GetSolution(forces_z)
print("solution forces_x: {}".format(x))
print("solution forces_z: {}".format(z))
return True
else:
print("only trivial solution with 0 forces found")
return False
def 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
R = 10 # Cost on input "effort".
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)))
dircol.SetInitialTrajectory(PiecewisePolynomial(), initial_x_trajectory)
result = Solve(dircol)
print(result.get_solver_id().name())
print(result.get_solution_result())
assert (result.is_success())
x_trajectory = dircol.ReconstructStateTrajectory(result)
tree = RigidBodyTree(FindResource("acrobot/acrobot.urdf"),
FloatingBaseType.kFixed)
vis = PlanarRigidBodyVisualizer(tree, xlim=[-4., 4.], ylim=[-4., 4.])
ani = vis.animate(x_trajectory, repeat=True)
u_trajectory = dircol.ReconstructInputTrajectory(result)
times = np.linspace(u_trajectory.start_time(), u_trajectory.end_time(), 100)
u_lookup = np.vectorize(u_trajectory.value)
u_values = u_lookup(times)
plt.figure()
plt.plot(times, u_values)
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 diff_drive_pd_mp(
self, x,
x_des): # x_des = [x, theta, yaw, x_dot, theta_dot, yaw_dot]
m_s = 0.2 #kg
d = 0.085 #m
m_c = 0.056
I_3 = 0.000228373 #.0000989844 #kg*m^2
I_2 = .0000989844
R = 0.0333375
g = -9.81 #may need to be set as -9.81; test to see
L = 0.03
A_val_theta = (m_s * d * g *
(3 * m_c + m_s)) / (3 * m_c * I_3 +
3 * m_c * m_s * d**2 + m_s * I_3)
B_val_theta = (-m_s * d / R - 3 * m_c * m_s) / (
3 * m_c * I_3 + 3 * m_c * m_s * d**2 + m_s *
I_3) * math.pi / 180 #multiplicand for u to change in theta_dot
B_val_phi = -(2 * L / R) / (6 * m_c * L**2 + m_c * R**2 + 2 * I_2)
mp = MathematicalProgram()
k = mp.NewContinuousVariables(3, 'k_val') # [kp, kd, ky]
u = mp.NewContinuousVariables(2, 'u_val')
theta_dot_post = mp.NewContinuousVariables(
1,
'theta_dot_post_val') #estimated value of theta dot after control
phi_dot_post = mp.NewContinuousVariables(
1, 'phi_dot_post_val') #estimated value of theta dot after control
'''
kp = 1. #regulate theta (pitch) position
kd = kp / 20. #regulate theta (pitch) velocity
ky = 0.5 #regulate phi (yaw) position
'''
actuator_limit = .1 #estimate; 0.1 is probably high
#mp.AddConstraint(theta[0] == np.arcsin(2*(x[0]*x[2] - x[1]*x[3]))) #pitch
theta = np.arcsin(2 * (x[0] * x[2] - x[1] * x[3]))
phi = np.arctan2(2 * (x[0] * x[1] + x[2] * x[3]),
1 - 2 * (x[1]**2 + x[2]**2)) #yaw
theta_dot = x[
10] #Shown to be ~1.5% below true theta_dot on average in experiments
mp.AddConstraint(k[0] >= 0.)
mp.AddConstraint(k[1] >= 0.)
mp.AddConstraint(k[2] >= 0.)
mp.AddConstraint(u[0] == k[0] * (x_des[1] - theta + k[1] *
(x_des[4] - theta_dot)) + k[2] *
(x_des[2] - phi))
mp.AddConstraint(u[0] <= -actuator_limit)
mp.AddConstraint(u[0] >= -actuator_limit)
mp.AddConstraint(u[1] == -u[0])
mp.AddConstraint(theta_dot_post[0] == theta_dot +
B_val_theta * u[0] * 0.0005 +
A_val_theta * theta * .0005)
mp.AddConstraint(phi_dot_post[0] == x[11] + B_val_phi * u[0] * .0005)
theta_dot_des = -(theta - x_des[1]) / (2 * .0005)
mp.AddQuadraticCost((theta_dot_post[0] - theta_dot_des)**2)
phi_dot_des = -(phi - x_des[2]) / 2
print('theta_dot_des', theta_dot_des)
print('theta', theta)
mp.AddQuadraticCost(0.001 * (phi_dot_post[0] - phi_dot_des)**2)
result = Solve(mp)
print('Success: ', result.is_success())
print('Solver id: ', result.get_solver_id().name())
print('k: ', result.GetSolution(k))
print('u: ', result.GetSolution(u))
return result.GetSolution(u)
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 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 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
class PPTrajectory():
def __init__(self, sample_times, num_vars, degree, continuity_degree):
self.sample_times = sample_times
self.n = num_vars
self.degree = degree
self.prog = MathematicalProgram()
self.coeffs = []
for i in range(len(sample_times)):
self.coeffs.append(
self.prog.NewContinuousVariables(num_vars, degree + 1, "C"))
self.result = None
# Add continuity constraints
for s in range(len(sample_times) - 1):
trel = sample_times[s + 1] - sample_times[s]
coeffs = self.coeffs[s]
for var in range(self.n):
for deg in range(continuity_degree + 1):
# Don't use eval here, because I want left and right
# values of the same time
left_val = 0
for d in range(deg, self.degree + 1):
left_val += coeffs[var, d]*np.power(trel, d-deg) * \
math.factorial(d)/math.factorial(d-deg)
right_val = self.coeffs[s + 1][var,
deg] * math.factorial(deg)
self.prog.AddLinearConstraint(left_val == right_val)
# Add cost to minimize highest order terms
for s in range(len(sample_times) - 1):
self.prog.AddQuadraticCost(np.eye(num_vars), np.zeros(
(num_vars, 1)), self.coeffs[s][:, -1])
def eval(self, t, derivative_order=0):
if derivative_order > self.degree:
return 0
s = 0
while s < len(self.sample_times) - 1 and t >= self.sample_times[s + 1]:
s += 1
trel = t - self.sample_times[s]
if self.result is None:
coeffs = self.coeffs[s]
else:
coeffs = self.result.GetSolution(self.coeffs[s])
deg = derivative_order
val = 0 * coeffs[:, 0]
for var in range(self.n):
for d in range(deg, self.degree + 1):
val[var] += coeffs[var, d]*np.power(trel, d-deg) * \
math.factorial(d)/math.factorial(d-deg)
return val
def add_constraint(self, t, derivative_order, lb, ub=None):
'''
Adds a constraint of the form d^deg lb <= x(t) / dt^deg <= ub
'''
if ub is None:
ub = lb
assert (derivative_order <= self.degree)
val = self.eval(t, derivative_order)
self.prog.AddLinearConstraint(val, lb, ub)
def generate(self):
self.result = Solve(self.prog)
assert (self.result.is_success())
def direct_collocation_zhao_glider():
print("Running direct collocation")
plant = SlotineGlider()
context = plant.CreateDefaultContext()
N = 21
initial_guess = True
max_dt = 0.5
max_tf = N * max_dt
dircol = DirectCollocation(
plant,
context,
num_time_samples=N,
minimum_timestep=0.05,
maximum_timestep=max_dt,
)
# Constrain all timesteps, $h[k]$, to be equal, so the trajectory breaks are evenly distributed.
dircol.AddEqualTimeIntervalsConstraints()
# Add input constraints
u = dircol.input()
dircol.AddConstraintToAllKnotPoints(0 <= u[0])
dircol.AddConstraintToAllKnotPoints(u[0] <= 3)
dircol.AddConstraintToAllKnotPoints(-np.pi / 2 <= u[1])
dircol.AddConstraintToAllKnotPoints(u[1] <= np.pi / 2)
# Add state constraints
x = dircol.state()
min_speed = 5
dircol.AddConstraintToAllKnotPoints(x[0] >= min_speed)
min_height = 0.5
dircol.AddConstraintToAllKnotPoints(x[3] >= min_height)
# Add initial state
travel_angle = (3 / 2) * np.pi
h0 = 10
dir_vector = np.array([np.cos(travel_angle), np.sin(travel_angle)])
# Start at initial position
x0_pos = np.array([h0, 0, 0])
dircol.AddBoundingBoxConstraint(x0_pos, x0_pos,
dircol.initial_state()[3:6])
# Periodicity constraints
dircol.AddLinearConstraint(
dircol.final_state()[0] == dircol.initial_state()[0])
dircol.AddLinearConstraint(
dircol.final_state()[1] == dircol.initial_state()[1])
dircol.AddLinearConstraint(
dircol.final_state()[2] == dircol.initial_state()[2])
dircol.AddLinearConstraint(
dircol.final_state()[3] == dircol.initial_state()[3])
# Always end in right direction
# NOTE this assumes that we always are starting in origin
if travel_angle % np.pi == 0: # Travel along x-axis
dircol.AddConstraint(
dircol.final_state()[5] == dircol.initial_state()[5])
elif travel_angle % ((1 / 2) * np.pi) == 0: # Travel along y-axis
dircol.AddConstraint(
dircol.final_state()[4] == dircol.initial_state()[4])
else:
dircol.AddConstraint(
dircol.final_state()[5] == dircol.final_state()[4] *
np.tan(travel_angle))
# Maximize distance travelled in desired direction
p0 = dircol.initial_state()
p1 = dircol.final_state()
Q = 1
dist_travelled = np.array([p1[4], p1[5]]) # NOTE assume starting in origin
dircol.AddFinalCost(-(dir_vector.T.dot(dist_travelled)) * Q)
if True:
# Cost on input effort
R = 0.1
dircol.AddRunningCost(R * (u[0])**2 + R * u[1]**2)
# Initial guess is a straight line from x0 in direction
if initial_guess:
avg_vel_guess = 10 # Guess for initial velocity
x0_guess = np.array([avg_vel_guess, travel_angle, 0, h0, 0, 0])
guessed_total_dist_travelled = 200
xf_guess = np.array([
avg_vel_guess,
travel_angle,
0,
h0,
dir_vector[0] * guessed_total_dist_travelled,
dir_vector[1] * guessed_total_dist_travelled,
])
initial_x_trajectory = PiecewisePolynomial.FirstOrderHold(
[0.0, 4.0], np.column_stack((x0_guess, xf_guess)))
dircol.SetInitialTrajectory(PiecewisePolynomial(),
initial_x_trajectory)
# Solve direct collocation
result = Solve(dircol)
assert result.is_success()
print("Found a solution!")
# PLOTTING
N_plot = 200
# Plot trajectory
x_trajectory = dircol.ReconstructStateTrajectory(result)
times = np.linspace(x_trajectory.start_time(), x_trajectory.end_time(),
N_plot)
x_knots = np.hstack([x_trajectory.value(t) for t in times])
z = x_knots[3, :]
x = x_knots[4, :]
y = x_knots[5, :]
plot_trj_3_wind(np.vstack((x, y, z)).T, dir_vector)
# Plot input
u_trajectory = dircol.ReconstructInputTrajectory(result)
u_knots = np.hstack([u_trajectory.value(t) for t in times])
plot_input_zhao_glider(times, u_knots.T)
plt.show()
return 0
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()
# Construct an n-by-n positive semi-definite matrix as the decision
# variables.
num_states = A[0].shape[0]
P = prog.NewSymmetricContinuousVariables(num_states, "P")
prog.AddPositiveSemidefiniteConstraint(P - .01*np.identity(num_states))
# Add the common Lyapunov conditions.
for i in range(len(A)):
prog.AddPositiveSemidefiniteConstraint(-A[i].transpose().dot(P)
- P.dot(A[i])
- .01*np.identity(num_states))
# Add an objective.
prog.AddLinearCost(np.trace(P))
# Run the optimization.
result = Solve(prog)
if result.is_success():
P = result.GetSolution(P)
print("eig(P) =" + str(np.linalg.eig(P)[0]))
for i in range(len(A)):
print("eig(Pdot" + str(i) + ") = " +
str(np.linalg.eig(A[i].transpose().dot(P) + P.dot(A[i]))[0]))
else:
print('Could not find a common Lyapunov function.')
print('This is expected to occur with some probability: not all')
print('random sets of stable matrices will have a common Lyapunov')
print('function.')
def 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
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 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.
'''
#Determine a time within the middle of the range for (min time, max time)
N = 50
time_diff = ((maximum_time - minimum_time) / 2.)
if (maximum_time - minimum_time) % 2 == 1:
time_used = (time_diff - .5) + minimum_time + 3
else:
time_used = (time_diff) + minimum_time + 3
print(time_used)
time_array = np.arange(0.0, time_used, time_used / (1.0 * N))
dt = time_used / (1.0 * N)
mp = MathematicalProgram()
#Base case for state/control variables
k = 0
state = mp.NewContinuousVariables(4, "state_%d" % k) #x,y,xdot,ydot
state_over_time = state
u = mp.NewContinuousVariables(2, "u_%d" % k)
u_over_time = u
num_time_steps = N #int(time_used)
#Set Nx4 vector to represent the state variables; (N-1)x2 vector to represent the control inputs
for k in range(1, num_time_steps):
state = mp.NewContinuousVariables(4,
"state_%d" % k) #x,y,xdot,ydot
state_over_time = np.vstack((state_over_time, state))
for k in range(1, num_time_steps - 1):
u = mp.NewContinuousVariables(2, "u_%d" % k)
u_over_time = np.vstack((u_over_time, u))
#constrain initial states
print(state_initial)
mp.AddLinearConstraint(state_over_time[
0, 0] == state_initial[0]).evaluator().set_description('start [0]')
mp.AddLinearConstraint(state_over_time[
0, 1] == state_initial[1]).evaluator().set_description('start [1]')
mp.AddLinearConstraint(state_over_time[
0, 2] == state_initial[2]).evaluator().set_description('start [2]')
mp.AddLinearConstraint(state_over_time[0, 3] <= state_initial[3]
).evaluator().set_description('start [3], less')
mp.AddLinearConstraint(
state_over_time[0, 3] >= state_initial[3]).evaluator(
).set_description('start [3], greater')
#find dynamics constraints for each state to make standard direct transcription constraints
for i in range(0, num_time_steps - 1):
next_state_change = self.rocket_dynamics(state_over_time[i, :],
u_over_time[i, :])
#constrain every state via dynamics
mp.AddLinearConstraint(
state_over_time[i + 1, 0] == state_over_time[i, 0] +
next_state_change[0] *
dt).evaluator().set_description('dynamics[0]' + str(i))
mp.AddLinearConstraint(
state_over_time[i + 1, 1] == state_over_time[i, 1] +
next_state_change[1] * dt +
np.cos(u_over_time[i, 0] /
100.)).evaluator().set_description('dynamics[1]' +
str(i))
mp.AddConstraint(
state_over_time[i + 1, 2] == state_over_time[i, 2] +
next_state_change[2] * dt +
np.sin(state_over_time[i, 0] /
100.)).evaluator().set_description('dynamics[2]' +
str(i))
mp.AddConstraint(state_over_time[i + 1,
3] == state_over_time[i, 3] +
next_state_change[3] *
dt).evaluator().set_description('dynamics[3]' +
str(i))
#Add a quadratic cost for the amount of fuel used over the course of the trajectory
fuel_cost_gain = 100.0
mp.AddQuadraticCost(fuel_cost_gain *
u_over_time[:, 0].dot(u_over_time[:, 0]))
mp.AddQuadraticCost(fuel_cost_gain *
u_over_time[:, 1].dot(u_over_time[:, 1]))
#constrain final states
world_2 = self.world_2_position
mp.AddConstraint(
(state_over_time[-1, 2]**2 +
state_over_time[-1, 3]**2) <= 1.0).evaluator().set_description(
'speed constraint') #final vel < 1m/s
mp.AddConstraint(((state_over_time[-1, 0] - world_2[0])**2 +
(state_over_time[-1, 1] - world_2[1])**2) <= .5**2
).evaluator().set_description('distance constraint')
#constrain system to not stray to far from planets
world_1 = self.world_1_position
dist_between_planets = (world_1[0] - world_2[0])**2 + (world_1[1] -
world_2[1])**2
kp = 5.0
for i in range(num_time_steps):
mp.AddConstraint(((state_over_time[i, 0] - world_2[0])**2 +
(state_over_time[i, 1] -
world_2[1])**2) <= dist_between_planets * kp)
result = Solve(mp)
trajectory = result.GetSolution(state_over_time)
input_trajectory = result.GetSolution(u_over_time)
print(result.is_success())
if not result.is_success():
infeasible = GetInfeasibleConstraints(mp, result)
print "Infeasible constraints:"
for i in range(len(infeasible)):
print infeasible[i]
return trajectory, input_trajectory, time_array