Ejemplo n.º 1
0
    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
Ejemplo n.º 2
0
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)
Ejemplo n.º 3
0
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
Ejemplo n.º 4
0
    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])
Ejemplo n.º 5
0
 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)
Ejemplo n.º 7
0
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)
Ejemplo n.º 8
0
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
Ejemplo n.º 9
0
 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
Ejemplo n.º 10
0
 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
Ejemplo n.º 12
0
def norm(v):
    """
    :type v: Matrix
    :return: |v|_2
    :rtype: Union[float, Symbol]
    """
    return ca.norm_2(v)
Ejemplo n.º 13
0
 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))
Ejemplo n.º 14
0
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]
Ejemplo n.º 15
0
def norm(x):
    """
    Returns the L2-norm of a vector x.
    """
    try:
        return onp.linalg.norm(x)
    except Exception:
        return cas.norm_2(x)
Ejemplo n.º 16
0
    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
Ejemplo n.º 17
0
    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])
Ejemplo n.º 18
0
 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
Ejemplo n.º 19
0
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
Ejemplo n.º 20
0
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
Ejemplo n.º 21
0
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
Ejemplo n.º 22
0
    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
            )
Ejemplo n.º 23
0
 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]))
Ejemplo n.º 24
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
Ejemplo n.º 25
0
    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
Ejemplo n.º 26
0
    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
Ejemplo n.º 27
0
    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)
Ejemplo n.º 28
0
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])
Ejemplo n.º 29
0
 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
Ejemplo n.º 30
0
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])
Ejemplo n.º 31
0
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
Ejemplo n.º 32
0
    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
Ejemplo n.º 33
0
    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
Ejemplo n.º 34
0
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
Ejemplo n.º 35
0
    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
Ejemplo n.º 36
0
    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
Ejemplo n.º 37
0
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'])
Ejemplo n.º 38
0
    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))