def initial_guess(self, t, tf_init, params):
x_guess = self.xd()
inclination = 30.0 * ca.pi / 180.0 # the other angle than the one you're thinking of
dcmInclination = np.array(
[[ca.cos(inclination), 0.0, -ca.sin(inclination)], [0.0, 1.0, 0.0],
[ca.sin(inclination), 0.0,
ca.cos(inclination)]])
dtheta = 2.0 * ca.pi / tf_init
theta = t * dtheta
r = 0.25 * params['l']
angle = ca.SX.sym('angle')
x_cir = ca.sqrt(params['l']**2 - r**2)
y_cir = r * ca.cos(angle)
z_cir = r * ca.sin(angle)
init_pos_fun = ca.Function(
'init_pos', [angle],
[ca.mtimes(dcmInclination, ca.veccat(x_cir, y_cir, z_cir))])
init_vel_fun = init_pos_fun.jacobian()
ret = {}
ret['q'] = init_pos_fun(theta)
ret['dq'] = init_vel_fun(theta, 0) * dtheta
ret['w'] = ca.veccat(0.0, 0.0, dtheta)
norm_vel = ca.norm_2(ret['dq'])
norm_pos = ca.norm_2(ret['q'])
R0 = ret['dq'] / norm_vel
R2 = ret['q'] / norm_pos
R1 = ca.cross(ret['q'] / norm_pos, ret['dq'] / norm_vel)
ret['R'] = ca.vertcat(R0.T, R1.T, R2.T).T
return ret
def matrix_norm_2_generalized(a, b_inv, x=None, n_iter=None):
""" Get largest generalized eigenvalue of the pair inv_a^{-1},b
get the largest eigenvalue of the generalized eigenvalue problem
a x = \lambda b x
<=> b x = (1/\lambda) a x
Let \omega := 1/lambda
We solve the problem
b x = \omega a x
using the inverse power iteration which converges to
the smallest generalized eigenvalue \omega_\min
Hence we can get \lambda_\max = 1/\omega_\min,
the largest eigenvalue of a x = \lambda b x
"""
n, _ = a.shape
if x is None:
x = np.eye(n, 1)
x /= norm_2(x)
if n_iter is None:
n_iter = 2 * n**2
y = mtimes(b_inv, mtimes(a, x))
for i in range(n_iter):
x = y / norm_2(y)
y = mtimes(b_inv, mtimes(a, x))
return mtimes(y.T, x)
def test_so3():
r = ca.DM([0.1, 0.2, 0.3, 0])
v = ca.DM([0.1, 0.2, 0.3])
R = Dcm.from_euler(ca.DM([0.1, 0.2, 0.3]))
q1 = Quat.from_euler(ca.DM([0.1, 0.2, 0.3]))
assert ca.norm_2(Dcm.log(Dcm.exp(v)) - v) < eps
assert ca.norm_2(Quat.log(Quat.exp(v)) - v) < eps
assert ca.norm_2(Mrp.log(Mrp.exp(v)) - v) < eps
assert ca.norm_2(so3.vee(so3.wedge(v)) - v) < eps
assert ca.norm_2(Quat.from_dcm(Dcm.from_quat(q1)) - q1) < eps
assert ca.norm_2(Quat.from_mrp(Mrp.from_quat(q1)) - q1) < eps
assert ca.norm_2(Quat.from_euler(Euler.from_quat(q1)) - q1) < eps
assert ca.norm_2(Mrp.from_dcm(Dcm.from_mrp(r)) - r) < eps
assert ca.norm_2(Mrp.from_quat(Quat.from_mrp(r)) - r) < eps
assert ca.norm_2(Mrp.from_euler(Euler.from_mrp(r)) - r) < eps
assert ca.norm_fro(Dcm.from_quat(Quat.from_dcm(R)) - R) < eps
assert ca.norm_fro(Dcm.from_mrp(Mrp.from_dcm(R)) - R) < eps
assert ca.norm_fro(Dcm.from_euler(Euler.from_dcm(R)) - R) < eps
R = ca.DM([[0, -1, 0], [1, 0, 0], [0, 0, 1]])
assert ca.norm_fro(Dcm.from_quat(Quat.from_dcm(R)) - R) < eps
assert ca.norm_fro(Dcm.from_mrp(Mrp.from_dcm(R)) - R) < eps
assert ca.norm_fro(Dcm.from_euler(Euler.from_dcm(R)) - R) < eps
def get_in_tangent_cone_function_multidim(self, cnstr):
"""Returns a casadi function for the SetConstraint instance when the
SetConstraint is multidimensional."""
if not isinstance(cnstr, SetConstraint):
raise TypeError("in_tangent_cone is only available for" +
" SetConstraint")
time_var = self.skill_spec.time_var
robot_var = self.skill_spec.robot_var
list_vars = [time_var, robot_var]
list_names = ["time_var", "robot_var"]
robot_vel_var = self.skill_spec.robot_vel_var
opt_var = [robot_vel_var]
opt_var_names = ["robot_vel_var"]
virtual_var = self.skill_spec.virtual_var
virtual_vel_var = self.skill_spec.virtual_vel_var
input_var = self.skill_spec.input_var
expr = cnstr.expression
set_min = cnstr.set_min
set_max = cnstr.set_max
dexpr = cs.jacobian(expr, time_var)
dexpr += cs.jtimes(expr, robot_var, robot_vel_var)
if virtual_var is not None:
list_vars += [virtual_var]
list_names += ["virtual_var"]
opt_var += [virtual_vel_var]
opt_var_names += ["virtual_vel_var"]
dexpr += cs.jtimes(expr, virtual_var, virtual_vel_var)
if input_var is not None:
list_vars += [input_var]
list_vars += ["input_var"]
le = expr - set_min
ue = expr - set_max
le_good = le >= 1e-12
ue_good = ue <= 1e-12
above = cs.dot(le_good - 1, le_good - 1) == 0
below = cs.dot(ue_good - 1, ue_good - 1) == 0
inside = cs.logic_and(above, below)
out_dir = (cs.sign(le) + cs.sign(ue)) / 2.0
# going_in = cs.dot(out_dir, dexpr) <= 0.0
same_signs = cs.sign(le) == cs.sign(ue)
corner = cs.dot(same_signs - 1, same_signs - 1) == 0
dists = (cs.norm_2(dexpr) + 1e-10) * cs.norm_2(out_dir)
corner_handler = cs.if_else(
cs.dot(out_dir, dexpr) < 0.0,
cs.fabs(cs.dot(-out_dir, dexpr)) / dists < cs.np.cos(cs.np.pi / 4),
False, True)
going_in = cs.if_else(corner, corner_handler,
cs.dot(out_dir, dexpr) < 0.0, True)
in_tc = cs.if_else(
inside, # Are we inside?
True, # Then true.
going_in, # If not, check if we're "going_in"
True)
return cs.Function("in_tc_" + cnstr.label.replace(" ", "_"),
list_vars + opt_var, [in_tc],
list_names + opt_var_names,
["in_tc_" + cnstr.label])
def exp(cls, v):
assert v.shape == (3, 1) or q.shape == (3, )
q = ca.SX(4, 1)
theta = ca.norm_2(v)
q[0] = ca.cos(theta / 2)
c = ca.sin(theta / 2)
n = ca.norm_2(v)
q[1] = c * v[0] / n
q[2] = c * v[1] / n
q[3] = c * v[2] / n
return ca.if_else(n > eps, q, ca.SX([1, 0, 0, 0]))
def compute_maximum_eigenvalues(a, x=None, iters=None):
r, c = a.shape
if x is None:
x = np.eye(c, 1)
x /= norm_2(x)
if iters is None:
iters = 2 * c**2
y = mtimes(a, x)
for _ in range(iters):
x = y / norm_2(y)
y = mtimes(a, x)
return mtimes(y.T, x)
def add_target_orbit_constraint(mocp, p, phase_insertion):
start = lambda a: mocp.start(a)
end = lambda a: mocp.end(a)
# Target orbit - soft constraints
# 3 parameter orbit: SMA,ECC,INC
#
# h_target_magnitude - h_final_magnitude == 0
# h_target_z - h_final_z == 0
# c_target - c_final == 0
r_final_ECEF = casadi.vertcat(end(phase_insertion.rx),
end(phase_insertion.ry),
end(phase_insertion.rz))
v_final_ECEF = casadi.vertcat(end(phase_insertion.vx),
end(phase_insertion.vy),
end(phase_insertion.vz))
# Convert to ECI (z rotation is omitted, because LAN is free)
r_f = r_final_ECEF
v_f = (
v_final_ECEF +
casadi.cross(casadi.SX([0, 0, p.body_rotation_speed]), r_final_ECEF))
h_final = casadi.cross(r_f, v_f)
h_final_mag = casadi.norm_2(h_final)
h_final_z = h_final[2]
c_final = casadi.sumsqr(v_f) / 2 - 1.0 / casadi.norm_2(r_f)
defect_h_magnitude = h_final_mag - p.target_orbit_h_magnitude
defect_h_z = h_final_z - p.target_orbit_h_z
defect_c = c_final - p.target_orbit_c
slack_h_magnitude = mocp.add_variable('slack_h_magnitude', init=3.0)
slack_h_z = mocp.add_variable('slack_h_z', init=3.0)
slack_c = mocp.add_variable('slack_c', init=3.0)
mocp.add_constraint(slack_h_magnitude > 0)
mocp.add_constraint(slack_h_z > 0)
mocp.add_constraint(slack_c > 0)
mocp.add_constraint(defect_h_magnitude < slack_h_magnitude)
mocp.add_constraint(defect_h_magnitude > -slack_h_magnitude)
mocp.add_constraint(defect_h_z < slack_h_z)
mocp.add_constraint(defect_h_z > -slack_h_z)
mocp.add_constraint(defect_c < slack_c)
mocp.add_constraint(defect_c > -slack_c)
mocp.add_objective(100.0 * slack_h_magnitude)
mocp.add_objective(100.0 * slack_h_z)
mocp.add_objective(100.0 * slack_c)
def test_direct_product():
G = DirectProduct([R3, R3])
v1 = ca.SX([1, 2, 3, 4, 5, 6])
v2 = G.product(v1, v1)
assert ca.norm_2(v2 - 2 * v1) < eps
G = DirectProduct([Mrp, R3])
a = ca.SX([0.1, 0.2, 0.3, 0, 5, 6, 7])
b = ca.SX([0, 0, 0, 0, 1, 2, 3])
c = ca.SX([0.1, 0.2, 0.3, 0, 6, 8, 10])
assert ca.norm_2(c - G.product(a, b)) < eps
v = ca.SX([0.1, 0.2, 0.3, 4, 5, 6])
assert ca.norm_2(v - G.log(G.exp(v))) < eps
def heightMapNormalVector(self, x):
tangent_vector = ca.MX.ones(2, 1)
tangent_vector[1, 0] = self.terrain_factor * ca.cos(x)
normal_vector = ca.MX.ones(2, 1)
normal_vector[0, 0] = -tangent_vector[1, 0]
normal_vector = normal_vector / ca.norm_2(normal_vector)
return normal_vector
def exp(cls, v):
assert v.shape == (3, 1) or v.shape == (3, )
angle = ca.norm_2(v)
res = ca.SX(4, 1)
res[:3] = ca.tan(angle / 4) * v / angle
res[3] = 0
return ca.if_else(angle > eps, res, ca.SX([0, 0, 0, 0]))
def heightMapNormalVector(self, x):
tangent_vector = ca.MX.ones(2, 1)
tangent_vector[1, 0] = self.df(x=x)['jac']
normal_vector = ca.MX.ones(2, 1)
normal_vector[0, 0] = -tangent_vector[1, 0]
normal_vector = normal_vector / ca.norm_2(normal_vector)
return normal_vector
def norm(v):
"""
:type v: Matrix
:return: |v|_2
:rtype: Union[float, Symbol]
"""
return ca.norm_2(v)
def asTwist(self):
acosarg = (casadi.trace(self.R) - 1) / 2
theta = casadi.acos(casadi.if_else(casadi.fabs(acosarg) >= 1., 1., acosarg))
w = casadi.horzcat((self.R[2, 1] - self.R[1, 2]),
(self.R[0, 2] - self.R[2, 0]),
(self.R[1, 0] - self.R[0, 1]))
return casadi.horzcat(casadi.reshape(self.t, 1, 3), theta * w / casadi.norm_2(w))
def pseudorange_rate(x, params=None):
""" x = [x, y, z, b, xd, yd, zd, alpha, ...] where alpha is the receiver
clock bias rate term, assumes sat vel and xd are in same
coordinate representation.
y = [pseudorange rate] """
r = params["sat_pos"] - x[:3]
LoS = r / norm_2(r)
return dot(params["sat_vel"] - x[4:7], LoS) + x[7]
def norm(x):
"""
Returns the L2-norm of a vector x.
"""
try:
return onp.linalg.norm(x)
except Exception:
return cas.norm_2(x)
def test_point(self, element, fx_svals):
svals = fx_svals
# test against parent's implementation
for sval in svals:
p_ = element['base'].point(sval)
assert cas.norm_2(element['obj'].point(sval) -
cas.DM([p_.real, p_.imag])) < EPSILON
def __symbolic_setup(self):
# build composite symbolic expression
self._calc_lengths()
expr = cas.MX.nan(2, 1)
for i in range(len(self._segments) - 1, -1, -1):
s_max = self._fractions[i]
s_min = self._fractions[i - 1] if i > 0 else 0
s_loc = (self._s - s_min) / self._lengths[i]
expr = cas.if_else(self._s <= s_max,
self._segments[i].point(s_loc), expr)
self._expr = expr
self._point = cas.Function('point', [self._s], [self._expr])
dp_ds = cas.jacobian(self._expr, self._s)
# unit tangent vector
self._tangent_expr = cas.if_else(
cas.norm_2(dp_ds) > 0, dp_ds / cas.norm_2(dp_ds), cas.DM([0, 0]))
self._tangent = cas.Function('tangent', [self._s],
[self._tangent_expr])
dt_ds = cas.jacobian(self._tangent_expr, self._s)
# unit normal vector
self._normal_expr = cas.if_else(
cas.norm_2(dt_ds) > 0, dt_ds / cas.norm_2(dt_ds),
cas.vertcat(self._tangent_expr[1], -self._tangent_expr[0]))
self._normal = cas.Function('normal', [self._s], [self._normal_expr])
# curvature value
self._curvature_expr = cas.if_else(
cas.norm_2(dp_ds) > 0,
cas.norm_2(dt_ds) / cas.norm_2(dp_ds), cas.DM(0))
self._curvature = cas.Function('curvature', [self._s],
[self._curvature_expr])
def inv(cls, q):
assert q.shape == (4, 1) or q.shape == (4, )
qi = ca.SX(4, 1)
n = ca.norm_2(q)
qi[0] = q[0] / n
qi[1] = -q[1] / n
qi[2] = -q[2] / n
qi[3] = -q[3] / n
return qi
def export_chain_mass_model(n_mass, m, D, L):
model_name = 'chain_mass_' + str(n_mass)
x0 = np.array([0, 0, 0]) # fix mass (at wall)
M = n_mass - 2 # number of intermediate masses
nx = (2 * M + 1) * 3 # differential states
nu = 3 # control inputs
xpos = SX.sym('xpos', (M + 1) * 3, 1) # position of fix mass eliminated
xvel = SX.sym('xvel', M * 3, 1)
u = SX.sym('u', nu, 1)
xdot = SX.sym('xdot', nx, 1)
f = SX.zeros(3 * M, 1) # force on intermediate masses
for i in range(M):
f[3 * i + 2] = -9.81
for i in range(M + 1):
if i == 0:
dist = xpos[i * 3:(i + 1) * 3] - x0
else:
dist = xpos[i * 3:(i + 1) * 3] - xpos[(i - 1) * 3:i * 3]
scale = D / m * (1 - L / norm_2(dist))
F = scale * dist
# mass on the right
if i < M:
f[i * 3:(i + 1) * 3] -= F
# mass on the left
if i > 0:
f[(i - 1) * 3:i * 3] += F
# parameters
p = []
x = vertcat(xpos, xvel)
# dynamics
f_expl = vertcat(xvel, u, f)
f_impl = xdot - f_expl
model = AcadosModel()
model.f_impl_expr = f_impl
model.f_expl_expr = f_expl
model.x = x
model.xdot = xdot
model.u = u
model.p = p
model.name = model_name
return model
def testSE3():
'''
Make sure that SE3 is working correctly.
'''
v = ca.vertcat(0.1, 0.2, 0.3, 45, 50, 75)
assert ca.norm_2(vee(wedge(v)) - v) < eps
# print statements should print v
print(vee(wedge(v))) # test vee and wedge operators
print(vee(SE3Dcm.log(SE3Dcm.exp(wedge(v))))) # test log and exp maps
def errorAtTime(gt_traj, sym_traj, time, gt_pose=None):
if gt_pose is None:
gt_pose = gt_traj.sample(time)
sym_pose = sym_traj.sample(time)
if gt_pose is not None and sym_pose is not None:
relpose = casadi_pose.mulNumCas(gt_pose.inverse(), sym_pose)
return rotationAngle(relpose.R), casadi.norm_2(
relpose.t), gt_pose, sym_pose
else:
return None, None, None, None
def _optimization_constraints(self):
self.opti.subject_to(
self.state_predict[:, 0] == self.x_current[:]
) # initial condition
for i in range(1, self.horizon):
# state prediction
self.opti.subject_to(
self.state_predict[:6, i]
== self.state_derivative(
self.state_predict[0:6, i - 1],
self.state_predict[:, 8],
self.state_predict[:, 9],
)
)
# traj velocity prediction
self.opti.subject_to(
self.state_predict[6, i]
== (
self.state_predict[6, i - 1]
+ casadi.dot(
(self.path_ref[:, i] - self.path_ref[:, i - 1])
/ casadi.norm_2(self.path_ref[:, i] - self.path_ref[:, i - 1]),
(self.state_predict[3:5, i])
/ casadi.norm_2(self.state_predict[3:5, i]),
)
/ self.time
)
)
# control efforts state
self.opti.subject_to(
self.state_predict[7:, i]
== (self.state_predict[7:, i - 1] + self.delta_u[:, i - 1] / self.time)
)
self.opti.subject_to(
(self.state_predict(0, i) - self.path_ref[0, i]) ** 2
+ (self.state_predict(1, i) - self.path_ref[1, i]) ** 2
<= self.R ** 2
)
def log(cls, q):
assert q.shape == (4, 1) or q.shape == (4, )
v = ca.SX(3, 1)
norm_q = ca.norm_2(q)
theta = 2 * ca.acos(q[0])
c = ca.sin(theta / 2)
v[0] = theta * q[1] / c
v[1] = theta * q[2] / c
v[2] = theta * q[3] / c
return ca.if_else(ca.fabs(c) > eps, v, ca.SX([0, 0, 0]))
def test_point(self, element, fx_svals):
svals = fx_svals
Basics.test_point(self, element, svals)
for sval in svals:
# test natural parametrization
p_ = element['base'].point(
float(element['obj']._rescaler(sval, sval)))
element['obj'].set_natural_parametrization(True)
assert cas.norm_2(element['obj'].point(sval) -
cas.DM([p_.real, p_.imag])) < EPSILON
def test_translate(self, element, fx_dx, fx_dy, fx_svals):
dx, dy, svals = fx_dx, fx_dy, fx_svals
orig = copy.copy(element['obj'])
delta = cas.DM([dx, dy])
element['obj'].translate(dx, dy)
for sval in svals:
assert cas.norm_2(element['obj'].point(sval) -
(orig.point(sval) + delta)) < EPSILON
element['obj'].translate(-dx, -dy)
assert element['obj'] == orig
def assert_model_equivalent_numeric(self, A, B, tol=1e-9):
self.assertEqual(len(A.states), len(B.states))
self.assertEqual(len(A.der_states), len(B.der_states))
self.assertEqual(len(A.inputs), len(B.inputs))
self.assertEqual(len(A.outputs), len(B.outputs))
self.assertEqual(len(A.constants), len(B.constants))
self.assertEqual(len(A.constant_values), len(B.constant_values))
self.assertEqual(len(A.parameters), len(B.parameters))
self.assertEqual(len(A.equations), len(B.equations))
for a, b in zip(A.constant_values, B.constant_values):
delta = ca.vec(a - b)
for i in range(delta.size1()):
test = float(delta[i]) <= tol
self.assertTrue(test)
this = A.get_function()
that = B.get_function()
this_mx = this.mx_in()
that_mx = that.mx_in()
this_in = [e.name() for e in this_mx]
that_in = [e.name() for e in that_mx]
that_from_this = []
this_mx_dict = dict(zip(this_in, this_mx))
for e in that_in:
self.assertTrue(e in this_in)
that_from_this.append(this_mx_dict[e])
that = ca.Function('f', this_mx, that.call(that_from_this))
np.random.seed(0)
args_in = []
for i in range(this.n_in()):
sp = this.sparsity_in(0)
r = ca.DM(sp, np.random.random(sp.nnz()))
args_in.append(r)
this_out = this.call(args_in)
that_out = that.call(args_in)
for i, (a, b) in enumerate(zip(this_out, that_out)):
test = float(ca.norm_2(ca.vec(a - b))) <= tol
if not test:
print("Expr mismatch")
print("A: ", A.equations[i], a)
print("B: ", B.equations[i], b)
self.assertTrue(test)
return True
def _create_observation_covariance_function(self, N_min, N_max):
d = ca.veccat([ca.cos(self.x['psi']) * ca.cos(self.x['phi']),
ca.cos(self.x['psi']) * ca.sin(self.x['phi']),
ca.sin(self.x['psi'])])
r = ca.veccat([self.x['x_b'] - self.x['x_c'],
self.x['y_b'] - self.x['y_c'],
self.x['z_b']])
r_cos_omega = ca.mul(d.T, r)
cos_omega = r_cos_omega / (ca.norm_2(r) + 1e-6)
# Look at the ball
N = self.z.squared(ca.SX.zeros(self.nz, self.nz))
# variance = N_max * (1 - cos_omega) + N_min
# variance = ca.mul(r.T, r) * (N_max * (1 - cos_omega) + N_min)
variance = ca.norm_2(r) * (N_max * (1 - cos_omega) + N_min)
N['x_b', 'x_b'] = variance
N['y_b', 'y_b'] = variance
N['z_b', 'z_b'] = variance
op = {'input_scheme': ['x'],
'output_scheme': ['N']}
return ca.SXFunction('Observation covariance',
[self.x], [N], op)
def observation_covariance(state, observation):
d = ca.veccat([ca.cos(state["phi"]), ca.sin(state["phi"])])
r = ca.veccat([state["x_b"] - state["x_c"], state["y_b"] - state["y_c"]])
r_cos_theta = ca.mul(d.T, r)
cos_theta = r_cos_theta / (ca.norm_2(r_cos_theta) + 1e-2)
# Look at the ball and be close to the ball
nz = observation.size
R = observation.squared(ca.SX.zeros(nz, nz))
R["x_b", "x_b"] = ca.mul(r.T, r) * (1 - cos_theta) + 1e-2
R["y_b", "y_b"] = ca.mul(r.T, r) * (1 - cos_theta) + 1e-2
# state -> R
return ca.SXFunction("Observation covariance", [state], [R])
def arclength(self, s):
if s == 0:
return 0.0
length = cas.integrator(
'integrator', 'rk', {
'x': cas.MX.sym('null'),
't': self._s,
'ode': 0,
'quad': cas.norm_2(cas.jacobian(self._expr, self._s))
}, {
't0': 0.0,
'tf': s
}).call({})['qf']
return length
def observation_covariance(state, observation):
d = ca.veccat([ca.cos(state['phi']), ca.sin(state['phi'])])
r = ca.veccat([state['x_b']-state['x_c'], state['y_b']-state['y_c']])
r_cos_theta = ca.mul(d.T,r)
cos_theta = r_cos_theta / (ca.norm_2(r_cos_theta) + 1e-2)
# Look at the ball and be close to the ball
nz = observation.size
R = observation.squared(ca.SX.zeros(nz,nz))
R['x_b','x_b'] = ca.mul(r.T,r) * (1 - cos_theta) + 1e-2
R['y_b','y_b'] = ca.mul(r.T,r) * (1 - cos_theta) + 1e-2
# state -> R
return ca.SXFunction('Observation covariance',[state],[R])
def solve_launch_direction_problem(p):
# Find the approximate launch direction vector.
# The launch direction vector is horizontal (orthogonal to the launch site position vector).
# The launch direction vector points (approximately) in the direction of the orbit insertion velocity.
mocp = MOCP()
rx = mocp.add_variable('rx', init=p.launch_site_x)
ry = mocp.add_variable('ry', init=p.launch_site_y)
rz = mocp.add_variable('rz', init=p.launch_site_z)
vx = mocp.add_variable('vx', init=0.01)
vy = mocp.add_variable('vy', init=0.01)
vz = mocp.add_variable('vz', init=0.01)
mocp.add_objective((rx - p.launch_site_x)**2)
mocp.add_objective((ry - p.launch_site_y)**2)
mocp.add_objective((rz - p.launch_site_z)**2)
r = casadi.vertcat(rx, ry, rz)
v = casadi.vertcat(vx, vy, vz)
h = casadi.cross(r, v)
h_mag = casadi.norm_2(h)
h_z = h[2]
c = casadi.sumsqr(v) / 2 - 1.0 / casadi.norm_2(r)
mocp.add_objective((h_mag - p.target_orbit_h_magnitude)**2)
mocp.add_objective((h_z - p.target_orbit_h_z)**2)
mocp.add_objective((c - p.target_orbit_c)**2)
mocp.solve()
v_soln = DM([mocp.get_value(vx), mocp.get_value(vy), mocp.get_value(vz)])
launch_direction = normalize(v_soln)
up_direction = normalize(
DM([p.launch_site_x, p.launch_site_y, p.launch_site_z]))
launch_direction = launch_direction - (
launch_direction.T @ up_direction) * up_direction
return launch_direction
def test_rotate(self, element, fx_theta, fx_x, fx_y, fx_svals):
theta, x, y, svals = fx_theta, fx_x, fx_y, fx_svals
orig = copy.copy(element['obj'])
delta = cas.DM([x, y])
T = cas.DM(
[[+cas.cos(theta / 180 * cas.pi), -cas.sin(theta / 180 * cas.pi)],
[+cas.sin(theta / 180 * cas.pi), +cas.cos(theta / 180 * cas.pi)]])
element['obj'].rotate(theta, x, y)
for sval in svals:
p = T @ (orig.point(sval) - delta) + delta
assert cas.norm_2(element['obj'].point(sval) - p) < EPSILON
element['obj'].rotate(-theta, x, y)
assert element['obj'] == orig
def test_scale(self, element, fx_x, fx_y, fx_svals):
x, y, svals = fx_x, fx_y, fx_svals
orig = copy.copy(element['obj'])
element_ = copy.deepcopy(orig)
if x == y:
element_.scale(x=x, y=None)
else:
element_.scale(x, y)
for sval in svals:
assert cas.norm_2(
element_.point(sval) -
orig.point(sval) * cas.DM([x, y])) < EPSILON
if x > 0 and y > 0:
element_.scale(1 / x, 1 / y)
assert element_ == orig
element_ = copy.deepcopy(orig)
element_.scale(-x, y)
for sval in svals:
assert cas.norm_2(
element_.point(sval) -
orig.point(sval) * cas.DM([-x, y])) < EPSILON
if x > 0 and y > 0:
element_.scale(-1 / x, 1 / y)
assert element_ == orig
element_ = copy.deepcopy(orig)
element_.scale(x, -y)
for sval in svals:
assert cas.norm_2(
element_.point(sval) -
orig.point(sval) * cas.DM([x, -y])) < EPSILON
if x > 0 and y > 0:
element_.scale(1 / x, -1 / y)
assert element_ == orig
def estimate_BtoAng(theta_0,phal,joint,s,b):
theta_x = cs.SX.sym('theta',3)
print "theta_0", theta_0
print "phal",phal
measB = [float(i) for i in b.tolist()]
print "b",measB
f = cs.norm_2(angToB(theta_x,phal,joint,s)-measB)
# f = cs.norm_2(testFun(theta_x)-b)
nlp = cs.SXFunction( cs.nlpIn(x=theta_x), cs.nlpOut(f=f))
solver = cs.NlpSolver("ipopt", nlp)
solver.init()
solver.setInput(theta_0,'x0')
solver.setInput(0.,'lbx')
solver.setInput([np.pi/2,np.pi/(180/110),np.pi/2],'ubx')
solver.evaluate()
x = solver.getOutput('x')
# val = solver.getOutput('f')
return x
def _create_objective_function(model, V, warm_start):
[final_cost] = model.cl([V['X', model.n]])
running_cost = 0
for k in range(model.n):
[stage_cost] = model.c([V['X', k], V['U', k]])
# Encourage looking at the ball
d = ca.veccat([ca.cos(V['X', k, 'psi'])*ca.cos(V['X', k, 'phi']),
ca.cos(V['X', k, 'psi'])*ca.sin(V['X', k, 'phi']),
ca.sin(V['X', k, 'psi'])])
r = ca.veccat([V['X', k, 'x_b'] - V['X', k, 'x_c'],
V['X', k, 'y_b'] - V['X', k, 'y_c'],
V['X', k, 'z_b']])
r_cos_omega = ca.mul(d.T, r)
if warm_start:
cos_omega = r_cos_omega / (ca.norm_2(r) + 1e-6)
stage_cost += 1e-1 * (1 - cos_omega)
else:
stage_cost -= 1e-1 * r_cos_omega * model.dt
running_cost += stage_cost
return final_cost + running_cost
def plot_heuristics(model, x_all, u_all, n_last=2):
n_interm_points = 301
n = len(x_all[:])
t_all = np.linspace(0, (n - 1) * model.dt, n)
t_all_dense = np.linspace(t_all[0], t_all[-1], n_interm_points)
fig, ax = plt.subplots(2, 2, figsize=(10, 10))
# ---------------- Optic acceleration cancellation ----------------- #
oac = []
for k in range(n):
x_b = x_all[k, ca.veccat, ['x_b', 'y_b']]
x_c = x_all[k, ca.veccat, ['x_c', 'y_c']]
r_bc_xy = ca.norm_2(x_b - x_c)
z_b = x_all[k, 'z_b']
tan_phi = ca.arctan2(z_b, r_bc_xy)
oac.append(tan_phi)
# Fit a line for OAC
fit_oac = np.polyfit(t_all[:-n_last], oac[:-n_last], 1)
fit_oac_fn = np.poly1d(fit_oac)
# Plot OAC
ax[0, 0].plot(t_all[:-n_last], oac[:-n_last],
label='Simulation', lw=3)
ax[0, 0].plot(t_all, fit_oac_fn(t_all), '--k', label='Linear fit')
# ------------------- Constant bearing angle ----------------------- #
cba = []
d = ca.veccat([ca.cos(model.m0['phi']),
ca.sin(model.m0['phi'])])
for k in range(n):
x_b = x_all[k, ca.veccat, ['x_b', 'y_b']]
x_c = x_all[k, ca.veccat, ['x_c', 'y_c']]
r_cb = x_b - x_c
cos_gamma = ca.mul(d.T, r_cb) / ca.norm_2(r_cb)
cba.append(np.rad2deg(np.float(ca.arccos(cos_gamma))))
# Fit a const for CBA
fit_cba = np.polyfit(t_all[:-n_last], cba[:-n_last], 0)
fit_cba_fn = np.poly1d(fit_cba)
# Smoothen the trajectory
t_part_dense = np.linspace(t_all[0], t_all[-n_last-1], 301)
cba_smooth = spline(t_all[:-n_last], cba[:-n_last], t_part_dense)
ax[1, 0].plot(t_part_dense, cba_smooth, lw=3, label='Simulation')
# Plot CBA
# ax[1, 0].plot(t_all[:-n_last], cba[:-n_last],
# label='$\gamma \\approx const$')
ax[1, 0].plot(t_all, fit_cba_fn(t_all), '--k', label='Constant fit')
# ---------- Generalized optic acceleration cancellation ----------- #
goac_smooth = spline(t_all,
model.m0['phi'] - x_all[:, 'phi'],
t_all_dense)
n_many_last = n_last *\
n_interm_points / (t_all[-1] - t_all[0]) * model.dt
# Delta
ax[0, 1].plot(t_all_dense[:-n_many_last],
np.rad2deg(goac_smooth[:-n_many_last]), lw=3,
label=r'Rotation angle $\delta$')
# Gamma
ax[0, 1].plot(t_all[:-n_last], cba[:-n_last], '--', lw=2,
label=r'Bearing angle $\gamma$')
# ax[0, 1].plot([t_all[0], t_all[-1]], [30, 30], 'k--',
# label='experimental bound')
# ax[0, 1].plot([t_all[0], t_all[-1]], [-30, -30], 'k--')
# ax[0, 1].yaxis.set_ticks(range(-60, 70, 30))
# Derivative of delta
# ax0_twin = ax[0, 1].twinx()
# ax0_twin.step(t_all,
# np.rad2deg(np.array(ca.veccat([0, u_all[:, 'w_phi']]))),
# 'g-', label='derivative $\mathrm{d}\delta/\mathrm{d}t$')
# ax0_twin.set_ylim(-90, 90)
# ax0_twin.yaxis.set_ticks(range(-90, 100, 30))
# ax0_twin.set_ylabel('$\mathrm{d}\delta/\mathrm{d}t$, deg/s')
# ax0_twin.yaxis.label.set_color('g')
# ax0_twin.legend(loc='lower right')
# -------------------- Linear optic trajectory --------------------- #
lot_beta = []
x_b = model.m0[ca.veccat, ['x_b', 'y_b']]
for k in range(n):
x_c = x_all[k, ca.veccat, ['x_c', 'y_c']]
d = ca.veccat([ca.cos(x_all[k, 'phi']),
ca.sin(x_all[k, 'phi'])])
r = x_b - x_c
cos_beta = ca.mul(d.T, r) / ca.norm_2(r)
beta = ca.arccos(cos_beta)
tan_beta = ca.tan(beta)
lot_beta.append(tan_beta)
# lot_beta.append(np.rad2deg(np.float(ca.arccos(cos_beta))))
# lot_alpha = np.rad2deg(np.array(x_all[:, 'psi']))
lot_alpha = ca.tan(x_all[:, 'psi'])
# Fit a line for LOT
fit_lot = np.polyfit(lot_alpha[model.n_delay:-n_last],
lot_beta[model.n_delay:-n_last], 1)
fit_lot_fn = np.poly1d(fit_lot)
# Plot
ax[1, 1].scatter(lot_alpha[model.n_delay:-n_last],
lot_beta[model.n_delay:-n_last],
label='Simulation')
ax[1, 1].plot(lot_alpha[model.n_delay:-n_last],
fit_lot_fn(lot_alpha[model.n_delay:-n_last]),
'--k', label='Linear fit')
fig.tight_layout()
return fig, ax
def create_plan_pc(b0, BF, dt, N_sim):
# Degrees of freedom for the optimizer
V = cat.struct_symSX([
(
cat.entry('X',repeat=N_sim+1,struct=state),
cat.entry('U',repeat=N_sim,struct=control)
)
])
# Final coordinate
x_bN = V['X',N_sim,ca.veccat,['x_b','y_b']]
x_cN = V['X',N_sim,ca.veccat,['x_c','y_c']]
dx_bc = x_bN - x_cN
# Final velocity
v_bN = V['X',N_sim,ca.veccat,['vx_b','vy_b']]
v_cN = V['X',N_sim,ca.veccat,['vx_c','vy_c']]
dv_bc = v_bN - v_cN
# Angle between gaze and ball velocity
theta = V['X',N_sim,'theta']
phi = V['X',N_sim,'phi']
d = ca.veccat([ ca.cos(theta)*ca.cos(phi), ca.cos(theta)*ca.sin(phi), ca.sin(theta) ])
r = V['X',N_sim,ca.veccat,['vx_b','vy_b','vz_b']]
r_unit = r / (ca.norm_2(r) + 1e-2)
d_gaze = d - r_unit
# Regularize controls
J = 1e-2 * ca.sum_square(ca.veccat(V['U'])) * dt # prevent bang-bang
# Multiple shooting constraints and running costs
bk = cat.struct_SX(belief)
bk['S'] = b0['S']
g, lbg, ubg = [], [], []
for k in range(N_sim):
# Belief propagation
bk['m'] = V['X',k]
[bk_next] = BF([ bk, V['U',k] ])
bk_next = belief(bk_next) # simplify indexing
# Multiple shooting
g.append(bk_next['m'] - V['X',k+1])
lbg.append(ca.DMatrix.zeros(nx))
ubg.append(ca.DMatrix.zeros(nx))
# Control constraints
g.append(V['U',k,'F'] - a - b * ca.cos(V['U',k,'psi']))
lbg.append(-ca.inf)
ubg.append(ca.DMatrix([0]))
# Advance time
bk = bk_next
g = ca.vertcat(g)
lbg = ca.veccat(lbg)
ubg = ca.veccat(ubg)
# Simple cost
J += 1e1 * ca.mul(dx_bc.T, dx_bc) # coordinate
J += 1e0 * ca.mul(dv_bc.T, dv_bc) # velocity
#J += 1e0 * ca.mul(d_gaze.T, d_gaze) # gaze antialigned with ball velocity
J += 1e1 * ca.trace(bk_next['S']) # uncertainty
# log-probability cost
#Sb = bk_next['S', ['x_b','y_b'], ['x_b','y_b']]
#J += 1e1 * (ca.mul([ dm_bc.T, ca.inv(Sb), dm_bc ]) + ca.log(ca.det(Sb)))
# Formulate the NLP
nlp = ca.SXFunction('nlp', ca.nlpIn(x=V), ca.nlpOut(f=J,g=g))
# Create solver
opts = {}
opts['linear_solver'] = 'ma97'
#opts['hessian_approximation'] = 'limited-memory'
solver = ca.NlpSolver('solver', 'ipopt', nlp, opts)
# Define box constraints
lbx = V(-ca.inf)
ubx = V(ca.inf)
# State constraints
# catcher can look within the upper hemisphere
lbx['X',:,'theta'] = 0; ubx['X',:,'theta'] = theta_max
# Control constraints
# 0 <= F
lbx['U',:,'F'] = 0
# -w_max <= w <= w_max
lbx['U',:,'w'] = -w_max; ubx['U',:,'w'] = w_max
# -pi <= psi <= pi
lbx['U',:,'psi'] = -ca.pi; ubx['U',:,'psi'] = ca.pi
# m(t=0) = m0
lbx['X',0] = ubx['X',0] = b0['m']
# Take care of the time
#lbx['X',:,'T'] = 0.5; ubx['X',:,'T'] = ca.inf
# Simulation ends when the ball touches the ground
#lbx['X',N_sim,'z_b'] = ubx['X',N_sim,'z_b'] = 0
# Solve the NLP
sol = solver(x0=0, lbg=lbg, ubg=ubg, lbx=lbx, ubx=ubx)
return V(sol['x'])
dx_bc = x_bN - x_cN
# Final velocity
v_bN = V['X',N_sim,ca.veccat,['vx_b','vy_b']]
v_cN = V['X',N_sim,ca.veccat,['vx_c','vy_c']]
dv_bc = v_bN - v_cN
# Angle between gaze and ball velocity
theta = V['X',N_sim,'theta']
phi = V['X',N_sim,'phi']
d = ca.veccat([ ca.cos(theta)*ca.cos(phi), ca.cos(theta)*ca.sin(phi), ca.sin(theta) ])
r = V['X',N_sim,ca.veccat,['vx_b','vy_b','vz_b']]
?????
delta = ca.arctan2(ca.norm_2(ca.cross()))
r_unit = r / (ca.norm_2(r) + 1e-2)
d_gaze = d - r_unit
# Regularize controls
J = 1e-1 * ca.sum_square(ca.veccat(V['U'])) * dt
# Multiple shooting constraints and non-linear control constraints
g, lbg, ubg = [], [], []
for k in range(N_sim):
# Multiple shooting
[x_next] = F([V['X',k], V['U',k], dt])
g.append(x_next - V['X',k+1])
lbg.append(ca.DMatrix.zeros(nx))
ubg.append(ca.DMatrix.zeros(nx))
