Esempio n. 1
0
    def set_data(self, data, weights=None):
        """ Attach experimental measurement data.

        data : a pd.DataFrame object
            Data should have columns corresponding to the state labels in
            self.boundary_species, with an index corresponding to the measurement
            times.

        """

        # Should raise an error if no state name is present
        df = data.loc[:, self.boundary_species]

        # Rename columns with state indicies
        df.columns = np.arange(self.nx)

        # Remove empty (nonmeasured) states
        self.data = df.loc[:, ~pd.isnull(df).all(0)]

        if weights is None:
            weights = self.data.max()

        obj_list = []
        for ((ti, state), xi) in self.data.stack().iteritems():
            obj_list += [(self._get_interp(ti, [state]) - xi) / weights[state]]

        obj_resid = cs.sum_square(cs.vertcat(obj_list))
        self.objective_sx += obj_resid
Esempio n. 2
0
def create_plan_fc(b0, N_sim):
    # Degrees of freedom for the optimizer
    V = cat.struct_symSX([
            (
                cat.entry('X',repeat=N_sim+1,struct=belief),
                cat.entry('U',repeat=N_sim,struct=control)
            )
        ])
    
    # Objective function
    m_bN = V['X',N_sim,'m',ca.veccat,['x_b','y_b']]
    m_cN = V['X',N_sim,'m',ca.veccat,['x_c','y_c']]
    dm_bc = m_bN - m_cN
    J = 1e2 * ca.mul(dm_bc.T, dm_bc) # m_cN -> m_bN
    J += 1e-1 * ca.trace(V['X',N_sim,'S']) # Sigma -> 0
    # Regularize controls
    J += 1e-2 * ca.sum_square(ca.veccat(V['U'])) * dt # prevent bang-bang
    
    # Multiple shooting constraints and running costs
    g = []
    for k in range(N_sim):
        # Multiple shooting
        [x_next] = BF([V['X',k], V['U',k]])
        g.append(x_next - V['X',k+1])
        
        # Penalize uncertainty
        J += 1e-1 * ca.trace(V['X',k,'S']) * dt
    g = ca.vertcat(g)
    
    # log-probability, doesn't work with full collocation
    #Sb = V['X',N_sim,'S',['x_b','y_b'], ['x_b','y_b']]
    #J += ca.mul([ dm_bc.T, ca.inv(Sb + 1e-8 * ca.SX.eye(2)), dm_bc ]) + \
    #     ca.log(ca.det(Sb + 1e-8 * ca.SX.eye(2)))
    
    # 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)
    
    # 0 <= v <= v_max
    lbx['U',:,'v'] = 0; ubx['U',:,'v'] = v_max
    # -w_max <= w <= w_max
    lbx['U',:,'w'] = -w_max; ubx['U',:,'w'] = w_max
    
    # m(t=0) = m0
    lbx['X',0,'m'] = ubx['X',0,'m'] = b0['m']
    # S(t=0) = S0
    lbx['X',0,'S'] = ubx['X',0,'S'] = b0['S']
    
    # Solve the NLP
    sol = solver(x0=0, lbg=0, ubg=0, lbx=lbx, ubx=ubx)
    return V(sol['x'])
Esempio n. 3
0
    def _set_objective_from_data(self, data, weights):

        obj_list = []
        for ((ti, state), xi) in data.stack().iteritems():
            obj_list += [(self._get_interp(ti, [state]) - xi) / weights[state]]

        obj_resid = cs.sum_square(cs.vertcat(obj_list))
        self.objective_sx += obj_resid
Esempio n. 4
0
def create_simple_plan(x0, 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)
            )
        ])

    # Objective function
    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
    J = 1e1 * ca.mul(dx_bc.T, dx_bc) # x_cN -> x_bN
    
    # Regularize controls
    J += 1e-1 * ca.sum_square(ca.veccat(V['U'])) * dt # prevent bang-bang

    # Multiple shooting constraints and non-linear control constraints
    g = []
    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])        
    g = ca.vertcat(g)

    # Formulate the NLP
    nlp = ca.SXFunction('nlp', ca.nlpIn(x=V), ca.nlpOut(f=J,g=g))

    opts = {}
    #opts['hessian_approximation'] = 'limited-memory'
    opts['linear_solver'] = 'ma57'
    # Create a solver
    solver = ca.NlpSolver('solver', 'ipopt', nlp, opts)
    
    # Define box constraints
    lbx = V(-ca.inf)
    ubx = V(ca.inf)
    
    # 0 <= v <= v_max
    lbx['U',:,'v'] = 0; ubx['U',:,'v'] = v_max
    # -w_max <= w <= w_max
    lbx['U',:,'w'] = -w_max; ubx['U',:,'w'] = w_max
    
    # m(t=0) = m0
    lbx['X',0] = ubx['X',0] = x0
    
    # Solve the NLP
    sol = solver(x0=0, lbg=0, ubg=0, lbx=lbx, ubx=ubx)
    return V(sol['x'])
Esempio n. 5
0
    def set_data(self, data):
        """ Attach experimental measurement data.

        data : a pd.DataFrame object
            Data should have columns corresponding to the state labels in
            self.state_names, with an index corresponding to the measurement
            times.

        """

        # Should raise an error if no state name is present
        df = data.loc[:, self.state_names]

        # Rename columns with state indicies
        df.columns = np.arange(self.NEQ)

        # Remove empty (nonmeasured) states
        self.data = df.loc[:, ~pd.isnull(df).all(0)]

        obj_list = []
        for ((ti, state), xi) in self.data.stack().iteritems():
            obj_list += [(self._get_interp(ti, [state]) - xi) / self.data[state].max()]

        obj_resid = cs.sum_square(cs.vertcat(obj_list))

        # ts = data.index

        # Define objective function
        # obj_resid = cs.sum_square(cs.vertcat(
        #     [(self._get_interp(ti, self._states_indicies) - xi)/ xs.max(0)
        #      for ti, xi in zip(ts, xs)]))

        alpha = cs.SX.sym("alpha")
        obj_lasso = alpha * cs.sumRows(cs.fabs(self._P))

        self._obj = obj_resid + obj_lasso

        # Create the solver object
        self._nlp = cs.SXFunction("nlp", cs.nlpIn(x=self._V, p=alpha), cs.nlpOut(f=self._obj, g=self._g))
Esempio n. 6
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'])
Esempio n. 7
0
    
    # 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))

        # 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]))
    g = ca.vertcat(g)
Esempio n. 8
0
import numpy as np
from optistack import *
import casadi as C

A = optivar(3,3)

# inverse
B = np.array([[1, 0, -2],[3, 1, -1],[2, 1, 0]])

optisolve(C.sum_square(mtimes(A,B)-np.eye(3)))

As = optival(A)

assert(np.max(np.max(np.fabs(As-np.linalg.inv(B)))<1e-7))

A = optivar(4,3);
# moore-penrose pseudo-inverse
B = np.array([[1, 0, -2, 3],[ 3, 1, -1, 1],[2, 1, 0, 9]])

optisolve(C.sum_square(mtimes(A,B)-np.eye(4)))

As = optival(A)

assert(np.max(np.max(np.fabs(As-np.linalg.pinv(B)))<1e-7))






Esempio n. 9
0
def create_plan_pc(b0, 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 means
    m_bN = V['X',N_sim,ca.veccat,['x_b','y_b']]
    m_cN = V['X',N_sim,ca.veccat,['x_c','y_c']]
    dm_bc = m_bN - m_cN
    
    # 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,'v'] - 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(dm_bc.T, dm_bc) + ca.trace(bk_next['S']))
    
    # 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)
    
    # 0 <= v
    lbx['U',:,'v'] = 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']
    
    # Solve the NLP
    sol = solver(x0=0, lbg=lbg, ubg=ubg, lbx=lbx, ubx=ubx)
    return V(sol['x'])