diff --git a/rawe/models/aero.py b/rawe/models/aero.py index d62c366..fd6352f 100644 --- a/rawe/models/aero.py +++ b/rawe/models/aero.py @@ -167,6 +167,14 @@ def aeroForcesTorques(dae, conf, v_bw_f, v_bw_b, w_bn_b, (eTe_f_1, eTe_f_2, eTe_ t_b_2 = rho_sref_v2*cref*dae['cm_small'] t_b_3 = rho_sref_v2*bref*dae['cn_small'] + if 'useScaledDisturbanceForcesAndTorques' in conf and conf['useScaledDisturbanceForcesAndTorques']: + f_f[0] += rho_sref_v2*dae['f1_disturbance_scaled'] + f_f[1] += rho_sref_v2*dae['f2_disturbance_scaled'] + f_f[2] += rho_sref_v2*dae['f3_disturbance_scaled'] + t_b_1 += rho_sref_v2*dae['t1_disturbance_scaled'] + t_b_2 += rho_sref_v2*dae['t2_disturbance_scaled'] + t_b_3 += rho_sref_v2*dae['t3_disturbance_scaled'] + dae['aero_fx'] = f_f[0] dae['aero_fy'] = f_f[1] dae['aero_fz'] = f_f[2] diff --git a/rawe/models/arianne_conf.py b/rawe/models/arianne_conf.py index 9c1e862..c777b5c 100644 --- a/rawe/models/arianne_conf.py +++ b/rawe/models/arianne_conf.py @@ -32,7 +32,9 @@ def makeConf(): 'cD_A': 0.027310, 'cD_A2': 1.948549, 'cD_B2': -0.013039, - 'cD0': 0.006600, + #'cD0': 0.006600, + #'cD0': 0.495, + 'cD0': 0.107, 'cY_B': -0.131871, @@ -71,7 +73,7 @@ def makeConf(): #[kite] - 'mass': 0.626, # mass of the kite # [ kg] + 'mass': 0.685, # mass of the kite # [ kg] 'tether_mass': 0.0, # use aerodynamic approximations instead of triginometry @@ -85,8 +87,8 @@ def makeConf(): 'bref': 0.96, #sqrt(sref*AR) 'cref': 0.128, #sqrt(sref/AR) - 'zt': 0.01, - 'xt': 0.018, # quick fix + 'zt': 0.025, + 'xt': 0.0, # quick fix #INERTIA MATRIX (Kurt's direct measurements) 'j1': 0.0163, diff --git a/rawe/models/carousel.py b/rawe/models/carousel.py index 23d4ee2..cb4e01d 100644 --- a/rawe/models/carousel.py +++ b/rawe/models/carousel.py @@ -145,14 +145,51 @@ def getWind(): , dy + ddelta*(rA + x) , dz ]) - C.veccat([dae['cos_delta']*wind_x, dae['sin_delta']*wind_x, 0]) + if 'useDisturbanceTurbulence' in conf and conf['useDisturbanceTurbulence']: + Psi_1 = dae.addU('Psi_1') + Psi_2 = dae.addU('Psi_2') + Psi_3 = dae.addU('Psi_3') + Omega_1 = dae.addU('Omega_1') + Omega_2 = dae.addU('Omega_2') + Omega_3 = dae.addU('Omega_3') + ## Reference to carousel frame + #R_r2c = C.blockcat([[dae['cos_delta'],dae['sin_delta'],0.0], + #[-dae['sin_delta'],dae['cos_delta'],0.0], + #[0.0,0.0,1.0]]) + # Add linear turbulence to velocity. They are assumed to be in the inertial frame + #v_bw_c -= C.mul(R_r2c, C.veccat([Psi_1, + #Psi_2, + #Psi_3])) + v_bw_c -= C.veccat([Psi_1, + Psi_2, + Psi_3]) + ## Add rotational turbulence to w_bn_b. Note that the turbulence is assumed to be in the inertial frame + dae['w_bn_b_with_disturbance'] = dae['w_bn_b'] - C.mul([dae['R_c2b'],R_r2c,C.veccat([Omega_1, + Omega_2, + Omega_3])]) + + if 'useScaledDisturbanceForcesAndTorques' in conf and conf['useScaledDisturbanceForcesAndTorques']: + f1_scaled = dae.addU('f1_disturbance_scaled') + f2_scaled = dae.addU('f2_disturbance_scaled') + f3_scaled = dae.addU('f3_disturbance_scaled') + t1_scaled = dae.addU('t1_disturbance_scaled') + t2_scaled = dae.addU('t2_disturbance_scaled') + t3_scaled = dae.addU('t3_disturbance_scaled') + # Velocity of aircraft w.r.t wind carrying frame, expressed in body frame # (needed to compute the aero forces and torques !) v_bw_b = C.mul( dae['R_c2b'], v_bw_c ) - (f1, f2, f3, t1, t2, t3) = aeroForcesTorques(dae, conf, v_bw_c, v_bw_b, - dae['w_bn_b'], - (dae['e21'], dae['e22'], dae['e23']) # y-axis of body frame in carousel coordinates - ) + if 'useDisturbanceTurbulence' in conf and conf['useDisturbanceTurbulence']: + (f1, f2, f3, t1, t2, t3) = aeroForcesTorques(dae, conf, v_bw_c, v_bw_b, + dae['w_bn_b_with_disturbance'], + (dae['e21'], dae['e22'], dae['e23']) # y-axis of body frame in carousel coordinates + ) + else: + (f1, f2, f3, t1, t2, t3) = aeroForcesTorques(dae, conf, v_bw_c, v_bw_b, + dae['w_bn_b'], + (dae['e21'], dae['e22'], dae['e23']) # y-axis of body frame in carousel coordinates + ) # f1..f3 expressed in carrousel coordinates # t1..t3 expressed in body coordinates @@ -166,6 +203,34 @@ def getWind(): t2 = t2 * gamma_homotopy + dae.addZ('t2_homotopy') * (1 - gamma_homotopy) t3 = t3 * gamma_homotopy + dae.addZ('t3_homotopy') * (1 - gamma_homotopy) + # if we want disturbance forces and torques, add them as control inputs and add them to the forces and torques + if 'useDisturbanceForcesAndTorques' in conf and conf['useDisturbanceForcesAndTorques']: + f1 += dae.addU('f1_disturbance') + f2 += dae.addU('f2_disturbance') + f3 += dae.addU('f3_disturbance') + t1 += dae.addU('t1_disturbance') + t2 += dae.addU('t2_disturbance') + t3 += dae.addU('t3_disturbance') + + if 'useDummyAccelerationControls' in conf and conf['useDummyAccelerationControls']: + u_a1 = dae.addU('dummyAcceleration_1') + u_a2 = dae.addU('dummyAcceleration_2') + u_a3 = dae.addU('dummyAcceleration_3') + + if 'kinematicIMUAccelerationModel' in conf and conf['kinematicIMUAccelerationModel']: + if not('polynomialApproximationForIMUAcceleration' in conf and conf['polynomialApproximationForIMUAcceleration']): + IMUAcceleration1 = dae.addU('IMUAcceleration1') + IMUAcceleration2 = dae.addU('IMUAcceleration2') + IMUAcceleration3 = dae.addU('IMUAcceleration3') + dx_IMU = dae.addX('dx_IMU') + dy_IMU = dae.addX('dy_IMU') + dz_IMU = dae.addX('dz_IMU') + + if 'useIMUAccelerationBias' in conf and conf['useIMUAccelerationBias']: + IMUAccelerationBias1 = dae.addX('IMUAccelerationBias1') + IMUAccelerationBias2 = dae.addX('IMUAccelerationBias2') + IMUAccelerationBias3 = dae.addX('IMUAccelerationBias3') + # mass matrix mm = C.SXMatrix(8, 8) mm[0,0] = jCarousel + m*rA*rA + m*x*x + m*y*y + 2*m*rA*x @@ -252,7 +317,6 @@ def getWind(): , t3 + w2*(j1*w1 + j31*w3) - j2*w1*w2 , ddr*r-(zt*w1*(e11*x+e12*y+e13*z+zt*e11*e31+zt*e12*e32+zt*e13*e33)+zt*w2*(e21*x+e22*y+e23*z+zt*e21*e31+zt*e22*e32+zt*e23*e33))*(w3-ddelta*e33)-dx*(dx-zt*e21*(w1-ddelta*e13)+zt*e11*(w2-ddelta*e23))-dy*(dy-zt*e22*(w1-ddelta*e13)+zt*e12*(w2-ddelta*e23))-dz*(dz-zt*e23*(w1-ddelta*e13)+zt*e13*(w2-ddelta*e23))+dr*dr+(w1-ddelta*e13)*(e21*(zt*dx-zt2*e21*(w1-ddelta*e13)+zt2*e11*(w2-ddelta*e23))+e22*(zt*dy-zt2*e22*(w1-ddelta*e13)+zt2*e12*(w2-ddelta*e23))+zt*e23*(dz+zt*e13*w2-zt*e23*w1)+zt*e33*(w1*z+zt*e33*w1+ddelta*e11*x+ddelta*e12*y+zt*ddelta*e11*e31+zt*ddelta*e12*e32)+zt*e31*(x+zt*e31)*(w1-ddelta*e13)+zt*e32*(y+zt*e32)*(w1-ddelta*e13))-(w2-ddelta*e23)*(e11*(zt*dx-zt2*e21*(w1-ddelta*e13)+zt2*e11*(w2-ddelta*e23))+e12*(zt*dy-zt2*e22*(w1-ddelta*e13)+zt2*e12*(w2-ddelta*e23))+zt*e13*(dz+zt*e13*w2-zt*e23*w1)-zt*e33*(w2*z+zt*e33*w2+ddelta*e21*x+ddelta*e22*y+zt*ddelta*e21*e31+zt*ddelta*e22*e32)-zt*e31*(x+zt*e31)*(w2-ddelta*e23)-zt*e32*(y+zt*e32)*(w2-ddelta*e23)) ] ) - dRexp = C.SXMatrix(3,3) dRexp[0,0] = e21*(w3 - ddelta*e33) - e31*(w2 - ddelta*e23) @@ -303,7 +367,7 @@ def getWind(): dae['c'] = c dae['cdot'] = cdot dae['cddot'] = cddot - return (mm, rhs, dRexp) + return (mm, rhs, dRexp, dae) def carouselModel(conf): ''' @@ -420,7 +484,7 @@ def carouselModel(conf): -(dae['e31']*dae['x'] + dae['e32']*dae['y'] + dae['e33']*dae['z']) / C.sqrt(dae['x']**2 + dae['y']**2 + dae['z']**2) dae['line_angle_deg'] = C.arccos(dae['cos_line_angle'])*180.0/C.pi - (massMatrix, rhs, dRexp) = setupModel(dae, conf) + (massMatrix, rhs, dRexp, dae) = setupModel(dae, conf) if 'stabilize_invariants' in conf and conf['stabilize_invariants'] == True: RotPole = 0.5 @@ -449,6 +513,143 @@ def carouselModel(conf): if 'flaps' in dae: ode = C.veccat([ode, dae.ddt('flaps') - dae['dflaps']]) + if 'polynomialApproximationForIMUAngularVelocity' in conf and conf['polynomialApproximationForIMUAngularVelocity']:#{{{ + #Add the control inputs for the polynomial coefficients. That's all we have to do here. The rest is taken care of by the constraints in the MHE formulation + Npoly_omega = conf['PolynomialOrder_omega'] + polynomialCoefficients_AngularVelocity = C.SXMatrix(3,Npoly_omega+1) + for k in range(Npoly_omega+1): + polynomialCoefficients_AngularVelocity[0,k] = dae.addU('polynomialCoefficient_AngularVelocity_x_'+str(k)) + polynomialCoefficients_AngularVelocity[1,k] = dae.addU('polynomialCoefficient_AngularVelocity_y_'+str(k)) + polynomialCoefficients_AngularVelocity[2,k] = dae.addU('polynomialCoefficient_AngularVelocity_z_'+str(k)) + + dae.addX('IMUAngularVelocity_dummy_x') + dae.addX('IMUAngularVelocity_dummy_y') + dae.addX('IMUAngularVelocity_dummy_z') + + # Define th polynomial #{{{ + Nimu = conf['NumberOfIMUMeasurementsBetweenAbsoluteMeasurements'] + m = Nimu-1 + tt = C.ssym('tt') + gram = C.SXMatrix(Npoly_omega+1,1) + gram[0,0] = 1.0/C.sqrt(m+1.0) + if Npoly_omega>0: + #Also do the linear function manually, cause it relies on gammanm = 1/infinity = 0, but which is not allowed + n=0.0 + alphanm = m/(n+1.0)*C.sqrt((4.0*(n+1.0)**2-1.0)/((m+1.0)**2-(n+1.0)**2)) + gram[1,0] = alphanm*tt*gram[0,0] + gammanm = 0.0 + for k in range(2,Npoly_omega+1): + n = k-1.0 + alphanm = m/(n+1.0)*C.sqrt((4.0*(n+1.0)**2-1.0)/((m+1.0)**2-(n+1.0)**2)) + n = n-1.0 + alphanmm1 = m/(n+1.0)*C.sqrt((4.0*(n+1.0)**2-1.0)/((m+1.0)**2-(n+1.0)**2)) + gammanm = alphanm/alphanmm1 + gram[k,0] = alphanm*tt*gram[k-1,0]-gammanm*gram[k-2,0] + Gram = C.SXFunction([tt],[gram]) + Gram.init() + Gram.setInput(-1.0) + Gram.evaluate() + polyEvalStart = Gram.getOutput() + Gram.setInput(-1.0+2.0*Nimu/m) + Gram.evaluate() + polyEvalEnd = Gram.getOutput()#}}} + + #{{{ + w_bn_imu_start_x = 0.0 + w_bn_imu_start_y = 0.0 + w_bn_imu_start_z = 0.0 + + w_bn_imu_end_x = 0.0 + w_bn_imu_end_y = 0.0 + w_bn_imu_end_z = 0.0 + for k in range(Npoly_omega+1): + w_bn_imu_start_x += dae['polynomialCoefficient_AngularVelocity_x_'+str(k)]*polyEvalStart[k] + w_bn_imu_start_y += dae['polynomialCoefficient_AngularVelocity_y_'+str(k)]*polyEvalStart[k] + w_bn_imu_start_z += dae['polynomialCoefficient_AngularVelocity_z_'+str(k)]*polyEvalStart[k] + + w_bn_imu_end_x += dae['polynomialCoefficient_AngularVelocity_x_'+str(k)]*polyEvalEnd[k] + w_bn_imu_end_y += dae['polynomialCoefficient_AngularVelocity_y_'+str(k)]*polyEvalEnd[k] + w_bn_imu_end_z += dae['polynomialCoefficient_AngularVelocity_z_'+str(k)]*polyEvalEnd[k]#}}} + + ode = C.veccat([ode,dae.ddt('IMUAngularVelocity_dummy_x')-(w_bn_imu_end_x-w_bn_imu_start_x)/conf['Ts'], + dae.ddt('IMUAngularVelocity_dummy_y')-(w_bn_imu_end_y-w_bn_imu_start_y)/conf['Ts'], + dae.ddt('IMUAngularVelocity_dummy_z')-(w_bn_imu_end_z-w_bn_imu_start_z)/conf['Ts']]) + IMUAngularVelocity_dummy = C.vertcat([dae['IMUAngularVelocity_dummy_x'],dae['IMUAngularVelocity_dummy_y'],dae['IMUAngularVelocity_dummy_z']]) + + BodyAngularVelocity_dummy = C.mul(conf['RIMU'].T,IMUAngularVelocity_dummy) + dae['BodyAngularVelocity_dummy_x'] = BodyAngularVelocity_dummy[0,0] + dae['BodyAngularVelocity_dummy_y'] = BodyAngularVelocity_dummy[1,0] + dae['BodyAngularVelocity_dummy_z'] = BodyAngularVelocity_dummy[2,0] + + dae['w_bn_imu_start_x'] = w_bn_imu_start_x + dae['w_bn_imu_start_y'] = w_bn_imu_start_y + dae['w_bn_imu_start_z'] = w_bn_imu_start_z + + dae['w_bn_imu_end_x'] = w_bn_imu_end_x + dae['w_bn_imu_end_y'] = w_bn_imu_end_y + dae['w_bn_imu_end_z'] = w_bn_imu_end_z + + dae['Output1'] = dae['w_bn_b_x'] - dae['BodyAngularVelocity_dummy_x'] + dae['Output2'] = dae['w_bn_b_y'] - dae['BodyAngularVelocity_dummy_y'] + dae['Output3'] = dae['w_bn_b_z'] - dae['BodyAngularVelocity_dummy_z'] + +#}}} + + if 'kinematicIMUAccelerationModel' in conf and conf['kinematicIMUAccelerationModel']:#{{{ + #dddelta = xDotSol['ddelta'] + # TODO effect of pIMU and dddelta + dddelta = 0.0 + R = dae['R_c2b'] + RIMU = conf['RIMU'] + if 'polynomialApproximationForIMUAcceleration' in conf and conf['polynomialApproximationForIMUAcceleration']: + # Create the Gram polynomials, the orthogonal polynomials on a discrete grid + # We need an extra state for time, and an extra control input such that we can have time go from -1 to 1 in each control interval + t = dae.addX('t') + ode = C.veccat([ode,dae.ddt('t')-conf['dt']]) + delta_t = dae.addU('delta_t') + t_interval = t-delta_t + Nimu = conf['NumberOfIMUMeasurementsBetweenAbsoluteMeasurements'] + Npoly_acceleration = conf['PolynomialOrder_acceleration'] + m = Nimu-1 + gram = C.SXMatrix(Npoly_acceleration+1,1) + gram[0,0] = 1.0/C.sqrt(m+1.0) + if Npoly_acceleration>0: + #Also do the linear function manually, cause it relies on gammanm = 1/infinity = 0, but which is not allowed + n=0.0 + alphanm = m/(n+1.0)*C.sqrt((4.0*(n+1.0)**2-1.0)/((m+1.0)**2-(n+1.0)**2)) + gram[1,0] = alphanm*t_interval*gram[0,0] + gammanm = 0.0 + for k in range(2,Npoly_acceleration+1): + n = k-1.0 + alphanm = m/(n+1.0)*C.sqrt((4.0*(n+1.0)**2-1.0)/((m+1.0)**2-(n+1.0)**2)) + n = n-1.0 + alphanmm1 = m/(n+1.0)*C.sqrt((4.0*(n+1.0)**2-1.0)/((m+1.0)**2-(n+1.0)**2)) + gammanm = alphanm/alphanmm1 + gram[k,0] = alphanm*t_interval*gram[k-1,0]-gammanm*gram[k-2,0] + #Add the control inputs for the polynomial coefficients + polynomialCoefficients = C.SXMatrix(3,Npoly_acceleration+1) + for k in range(Npoly_acceleration+1): + polynomialCoefficients[0,k] = dae.addU('polynomialCoefficient_Acceleration_x_'+str(k)) + polynomialCoefficients[1,k] = dae.addU('polynomialCoefficient_Acceleration_y_'+str(k)) + polynomialCoefficients[2,k] = dae.addU('polynomialCoefficient_Acceleration_z_'+str(k)) + # Built the expression for the IMU Acceleration + IMUAcceleration = C.SXMatrix(3,1) + for k in range(Npoly_acceleration+1): + IMUAcceleration += polynomialCoefficients[:,k]*gram[k,0] + else: + IMUAcceleration = C.vertcat([dae['IMUAcceleration1'], dae['IMUAcceleration2'], dae['IMUAcceleration3']]) + if 'useIMUAccelerationBias' in conf and conf['useIMUAccelerationBias']: + IMUAccelerationBias = C.vertcat([dae['IMUAccelerationBias1'],dae['IMUAccelerationBias2'],dae['IMUAccelerationBias3']]) + IMUAcceleration -= IMUAccelerationBias + ode = C.veccat([ode,dae.ddt('IMUAccelerationBias1')-0.0, + dae.ddt('IMUAccelerationBias2')-0.0, + dae.ddt('IMUAccelerationBias3')-0.0]) + ddp_IMU = C.mul(R.T,C.mul(RIMU.T,IMUAcceleration)) + dae['ddelta'] ** 2 * C.vertcat([dae['x'] + conf['rArm'], dae['y'], 0]) - 2 * dae['ddelta'] * C.vertcat([-dae['dy'], dae['dx'], 0]) - dddelta * C.vertcat([-dae['y'], dae['x'] + conf['rArm'], 0]) + C.vertcat([0, 0, conf['g']]) + ode = C.veccat([ode,dae.ddt('dx_IMU')-ddp_IMU[0], + dae.ddt('dy_IMU')-ddp_IMU[1], + dae.ddt('dz_IMU')-ddp_IMU[2]]) +#}}} + if 'stabilize_invariants' in conf and conf['stabilize_invariants'] == True: cPole = 0.5 else: diff --git a/rawe/ocp/Ocp.py b/rawe/ocp/Ocp.py index f18fa21..cea3671 100644 --- a/rawe/ocp/Ocp.py +++ b/rawe/ocp/Ocp.py @@ -38,6 +38,7 @@ def __init__(self): self.add(OptDouble('LEVENBERG_MARQUARDT',default=0.0)) self.add(OptBool('CG_USE_ARRIVAL_COST',default=False)) self.add(OptBool('CG_USE_VARIABLE_WEIGHTING_MATRIX',default=False)) + self.add(OptBool('CG_COMPUTE_COVARIANCE_MATRIX',default=False)) # self.add(OptBool('CG_USE_C99',default=True)) class Ocp(object): @@ -67,6 +68,7 @@ def __init__(self, dae, N=None, ts=None): self._constraints = [] self._constraintsStart = [] self._constraintsEnd = [] + self._constraintsWhen = [] self._dbgMessages = [] @@ -227,8 +229,10 @@ def maybeAddBoxConstraint(): self._constraintsEnd.append((lhs,comparison,rhs)) elif when is "AT_START": self._constraintsStart.append((lhs,comparison,rhs)) + elif isinstance(when,int) & 0 <= when <= self._N: + self._constraintsWhen.append((lhs,comparison,rhs,when)) else: - raise Exception("\"when\" must be 'AT_START' or 'AT_END', leaving it blank means always, you put: "+str(when)) + raise Exception("\"when\" must be 'AT_START', 'AT_END' or a gridpoint, leaving it blank means always, you put: "+str(when)) def minimizeLsq(self, obj): if isinstance(obj, list): diff --git a/rawe/ocp/ocg_interface.py b/rawe/ocp/ocg_interface.py index 9e8e18b..f691729 100644 --- a/rawe/ocp/ocg_interface.py +++ b/rawe/ocp/ocg_interface.py @@ -117,6 +117,16 @@ return memcpyMat(val, acadoVariables.WN, nr, nc, ACADO_NYN, ACADO_NYN); } #endif /* ACADO_WEIGHTING_MATRICES_TYPE */ +#if ACADO_COMPUTE_COVARIANCE_MATRIX +int py_set_sigmaN(real_t * val, const int nr, const int nc){ + return memcpyMat(acadoVariables.sigmaN, val, nr, nc, ACADO_NX, ACADO_NX); } +int py_get_sigmaN(real_t * val, const int nr, const int nc){ + return memcpyMat(val, acadoVariables.sigmaN, nr, nc, ACADO_NX, ACADO_NX); } +#endif /* ACADO_COMPUTE_COVARIANCE_MATRIX */ + +int py_get_A(real_t * val, const int nr, const int nc){ + return memcpyMat(val, acadoWorkspace.A, nr, nc, 100, ACADO_NX + ACADO_NU*ACADO_N); } + #if ACADO_USE_ARRIVAL_COST == 1 int py_set_SAC(real_t * val, const int nr, const int nc){ return memcpyMat(acadoVariables.SAC, val, nr, nc, ACADO_NX, ACADO_NX); } @@ -157,6 +167,8 @@ int py_get_ACADO_QPDUNES(void){ return ACADO_QPDUNES; } /** Indicator for determining the QP solver used by the ACADO solver code. */ int py_get_ACADO_QP_SOLVER(void){ return ACADO_QP_SOLVER; } +/** Indicator for computing covariance matrix. */ +int py_get_ACADO_COMPUTE_COVARIANCE_MATRIX(void){ return ACADO_COMPUTE_COVARIANCE_MATRIX; } /** Indicator for fixed initial state. */ int py_get_ACADO_INITIAL_STATE_FIXED(void){ return ACADO_INITIAL_STATE_FIXED; } /** Indicator for type of fixed weighting matrices. */ diff --git a/rawe/ocp/ocprt.py b/rawe/ocp/ocprt.py index 3e3fe74..e3dfe8e 100644 --- a/rawe/ocp/ocprt.py +++ b/rawe/ocp/ocprt.py @@ -52,7 +52,7 @@ def blah(self,*args,**kwargs): return blah class OcpRT(object): - _canonicalNames = ['x','u','z','y','yN','x0','S','SN','SAC','xAC','WL'] + _canonicalNames = ['x','u','z','y','yN','x0','S','SN','SAC','xAC','WL','sigmaN','A'] @property def ocp(self): @@ -98,6 +98,10 @@ def __init__(self, ocp, self._lib.py_get_ACADO_NU())) self.y = numpy.zeros((self._lib.py_get_ACADO_N(), self._lib.py_get_ACADO_NY())) + if ocpOptions["CG_COMPUTE_COVARIANCE_MATRIX"]: + self.sigmaN = numpy.zeros((self._lib.py_get_ACADO_NX(), + self._lib.py_get_ACADO_NX())) + self.A = numpy.zeros((100,self._lib.py_get_ACADO_NX() + self._lib.py_get_ACADO_NU()*self._lib.py_get_ACADO_N() )) if self._lib.py_get_ACADO_NXA() > 0: self.z = numpy.zeros((self._lib.py_get_ACADO_N(), self._lib.py_get_ACADO_NXA())) @@ -201,6 +205,8 @@ def _setAll(self): self._callMat(self._lib.py_set_yN, self.yN) self._callMat(self._lib.py_set_S, self.S) self._callMat(self._lib.py_set_SN, self.SN) + if self._lib.py_get_ACADO_COMPUTE_COVARIANCE_MATRIX(): + self._callMat(self._lib.py_set_sigmaN, self.sigmaN) if self._lib.py_get_ACADO_INITIAL_STATE_FIXED(): self._callMat(self._lib.py_set_x0, self.x0) if self._lib.py_get_ACADO_USE_ARRIVAL_COST(): @@ -217,6 +223,9 @@ def _getAll(self): self._callMat(self._lib.py_get_yN, self.yN) self._callMat(self._lib.py_get_S, self.S) self._callMat(self._lib.py_get_SN, self.SN) + if self._lib.py_get_ACADO_COMPUTE_COVARIANCE_MATRIX(): + self._callMat(self._lib.py_get_sigmaN, self.sigmaN) + self._callMat(self._lib.py_get_A, self.A) if self._lib.py_get_ACADO_INITIAL_STATE_FIXED(): self._callMat(self._lib.py_get_x0, self.x0) if self._lib.py_get_ACADO_USE_ARRIVAL_COST(): diff --git a/rawe/ocp/writeAcadoOcpExport.py b/rawe/ocp/writeAcadoOcpExport.py index 3be0736..39c318f 100644 --- a/rawe/ocp/writeAcadoOcpExport.py +++ b/rawe/ocp/writeAcadoOcpExport.py @@ -40,6 +40,10 @@ def writeAcadoAlgorithm(ocp, dae): k = len(outputs) outputs.append(rhs-lhs) constraintData.append((k,comparison,'AT_START')) + for (lhs,comparison,rhs,when) in ocp._constraintsWhen: + k = len(outputs) + outputs.append(rhs-lhs) + constraintData.append((k,comparison,when)) assert len(outputs) == len(constraintData), 'the "impossible" happened' # add dae residual and objectives @@ -271,6 +275,8 @@ def generateAcadoOcp(ocp, integratorOptions, ocpOptions): whenStr = 'AT_END, ' elif when == 'AT_START': whenStr = 'AT_START, ' + elif isinstance(when,int): + whenStr = str(when)+', ' else: raise Exception('the "impossible" happened, unrecognized "when": '+str(when)) lines.append( diff --git a/rawe/sim.py b/rawe/sim.py index d016e6b..2a95c0e 100644 --- a/rawe/sim.py +++ b/rawe/sim.py @@ -18,6 +18,8 @@ import time import casadi as C import numpy +import matplotlib as mpl +mpl.use('Agg') import matplotlib.pyplot as plt def getitemMsg(d,name,msg): diff --git a/rawekite/carouselSteadyState.py b/rawekite/carouselSteadyState.py index b378877..c4d27b7 100644 --- a/rawekite/carouselSteadyState.py +++ b/rawekite/carouselSteadyState.py @@ -76,7 +76,10 @@ def constrainInvariantErrs(): 'daileron':0,'delevator':0,'drudder':0,'dflaps':0, 'nu':100,'motor_torque':0, 'dmotor_torque':0,'ddr':0, - 'dddr':0.0,'w0':0.0} + 'dddr':0.0,'w0':0.0, + 'f1_disturbance':0.0,'f2_disturbance':0.0,'f3_disturbance':0.0, + 't1_disturbance':0.0,'t2_disturbance':0.0,'t3_disturbance':0.0, + 'dummyAcceleration_1':0.0,'dummyAcceleration_2':0.0,'dummyAcceleration_3':0.0} dotGuess = {'x':0,'y':0,'z':0,'dx':0,'dy':0,'dz':0, 'r':0,'dr':0, 'e11':0,'e12':0,'e13':0, @@ -104,7 +107,10 @@ def constrainInvariantErrs(): 'daileron':(0,0),'delevator':(0,0),'drudder':(0,0),'dflaps':(0,0), 'nu':(0,3000),'motor_torque':(-1000,1000), 'ddr':(0,0), - 'dmotor_torque':(0,0),'dddr':(0,0),'w0':(0,0)} + 'dmotor_torque':(0,0),'dddr':(0,0),'w0':(0,0), + 'f1_disturbance':(0,0),'f2_disturbance':(0,0),'f3_disturbance':(0,0), + 't1_disturbance':(0,0),'t2_disturbance':(0,0),'t3_disturbance':(0,0), + 'dummyAcceleration_1':(0,0),'dummyAcceleration_2':(0,0),'dummyAcceleration_3':(0,0)} if ref_dict is not None: for name in ref_dict: bounds[name] = ref_dict[name]