TEST(TestASMu3D, TransferGaussPtVarsN)
{
SIM3D sim(1), sim2(1);
sim.opt.discretization = sim2.opt.discretization = ASM::LRSpline;
sim.createDefaultModel();
sim2.createDefaultModel();
ASMu3D* pch = static_cast<ASMu3D*>(sim.getPatch(1));
ASMu3D* pchNew = static_cast<ASMu3D*>(sim2.getPatch(1));
pchNew->uniformRefine(0, 1);
RealArray oldAr(3*3*3), newAr;
std::iota(oldAr.begin(), oldAr.end(), 1);
pchNew->transferGaussPtVarsN(pch->getVolume(), oldAr, newAr, 3);
static RealArray refAr = {{ 1.0, 1.0, 2.0,
4.0, 4.0, 5.0,
7.0, 7.0, 8.0,
10.0, 10.0, 11.0,
13.0, 13.0, 14.0,
16.0, 16.0, 17.0,
19.0, 19.0, 20.0,
22.0, 22.0, 23.0,
25.0, 25.0, 26.0,
2.0, 3.0, 3.0,
5.0, 6.0, 6.0,
8.0, 9.0, 9.0,
11.0, 12.0, 12.0,
14.0, 15.0, 15.0,
17.0, 18.0, 18.0,
20.0, 21.0, 21.0,
23.0, 24.0, 24.0,
26.0, 27.0, 27.0}};
EXPECT_EQ(refAr.size(), newAr.size());
for (size_t i = 0; i < refAr.size(); ++i)
EXPECT_FLOAT_EQ(refAr[i], newAr[i]);
}
TEST(TestASMu2D, Connect)
{
SIM2D sim(1);
sim.opt.discretization = ASM::LRSpline;
ASSERT_TRUE(sim.read("src/ASM/Test/refdata/DomainDecomposition_MPI_2D_4_orient0.xinp"));
ASSERT_TRUE(sim.createFEMmodel());
SIM2D sim2(1);
sim2.opt.discretization = ASM::LRSpline;
ASSERT_TRUE(sim2.read("src/ASM/Test/refdata/DomainDecomposition_MPI_2D_4_orient1.xinp"));
ASSERT_TRUE(sim2.createFEMmodel());
}
int main() {
USING_NAMESPACE_ACADO
//
// DEFINE THE VARIABLES:
//
DifferentialState xT; // the trolley position
DifferentialState vT; // the trolley velocity
IntermediateState aT; // the trolley acceleration
DifferentialState xL; // the cable length
DifferentialState vL; // the cable velocity
IntermediateState aL; // the cable acceleration
DifferentialState phi; // the excitation angle
DifferentialState omega; // the angular velocity
DifferentialState uT; // trolley velocity control
DifferentialState uL; // cable velocity control
Control duT;
Control duL;
//
// DEFINE THE PARAMETERS:
//
const double tau1 = 0.012790605943772;
const double a1 = 0.047418203070092;
const double tau2 = 0.024695192379264;
const double a2 = 0.034087337273386;
const double g = 9.81;
const double c = 0.0;
const double m = 1318.0;
//
// DEFINE THE MODEL EQUATIONS:
//
DifferentialEquation f;
aT = -1.0 / tau1 * vT + a1 / tau1 * uT;
aL = -1.0 / tau2 * vL + a2 / tau2 * uL;
f << 0 == dot( xT ) - vT;
f << 0 == dot( vT ) - aT;
f << 0 == dot( xL ) - vL;
f << 0 == dot( vL ) - aL;
f << 0 == dot( phi ) - omega;
f << 0 == dot( omega ) - 1.0/xL*(-g*sin(phi)-aT*cos(phi)
-2*vL*omega-c*omega/(m*xL));
f << 0 == dot( uT ) - duT;
f << 0 == dot( uL ) - duL;
//
// DEFINE THE OUTPUT MODEL:
//
OutputFcn h;
h << aT;
h << aL;
//
// SET UP THE SIMULATION EXPORT MODULE:
//
acadoPrintf( "-----------------------------------------\n Using an equidistant grid:\n-----------------------------------------\n" );
SIMexport sim( 1, 0.1 );
sim.setModel( f );
sim.addOutput( h, 5 );
sim.set( INTEGRATOR_TYPE, INT_IRK_RIIA5 );
sim.set( NUM_INTEGRATOR_STEPS, 5 );
sim.setTimingSteps( 10000 );
sim.exportAndRun( "crane_export", "init_crane.txt", "controls_crane.txt" );
acadoPrintf( "-----------------------------------------\n Using a provided grid:\n-----------------------------------------\n" );
Vector Meas(5);
Meas(0) = 0.0;
Meas(1) = 0.2;
Meas(2) = 0.4;
Meas(3) = 0.6;
Meas(4) = 0.8;
SIMexport sim2( 1, 0.1 );
sim2.setModel( f );
sim2.addOutput( h, Meas );
sim2.set( INTEGRATOR_TYPE, INT_IRK_RIIA5 );
sim2.set( NUM_INTEGRATOR_STEPS, 5 );
sim2.setTimingSteps( 10000 );
sim2.exportAndRun( "crane_export", "init_crane.txt", "controls_crane.txt" );
return 0;
}
USING_NAMESPACE_ACADO
int main( ){
// Define variables, functions and constants:
// ----------------------------------------------------------
DifferentialState dT1;
DifferentialState dT2;
DifferentialState dT3;
DifferentialState dT4;
DifferentialState T1;
DifferentialState T2;
DifferentialState T3;
DifferentialState T4;
DifferentialState W1;
DifferentialState W2;
DifferentialState W3;
DifferentialState W4;
DifferentialState q1;
DifferentialState q2;
DifferentialState q3;
DifferentialState q4;
DifferentialState Omega1;
DifferentialState Omega2;
DifferentialState Omega3;
DifferentialState V1;
DifferentialState V2;
DifferentialState V3;
DifferentialState P1; // x
DifferentialState P2; // y
DifferentialState P3; // z
DifferentialState IP1;
DifferentialState IP2;
DifferentialState IP3;
Control U1;
Control U2;
Control U3;
Control U4;
DifferentialEquation f1, f2;
const double rho = 1.23;
const double A = 0.1;
const double Cl = 0.25;
const double Cd = 0.3*Cl;
const double m = 10;
const double g = 9.81;
const double L = 0.5;
const double Jp = 1e-2;
const double xi = 1e-2;
const double J1 = 0.25;
const double J2 = 0.25;
const double J3 = 1;
const double gain = 1e-4;
const double alpha = 0.0;
// Define the quadcopter ODE model in fully nonlinear form:
// ----------------------------------------------------------
f1 << U1*gain;
f1 << U2*gain;
f1 << U3*gain;
f1 << U4*gain;
f1 << dT1;
f1 << dT2;
f1 << dT3;
f1 << dT4;
f1 << (T1 - W1*xi)/Jp;
f1 << (T2 - W2*xi)/Jp;
f1 << (T3 - W3*xi)/Jp;
f1 << (T4 - W4*xi)/Jp;
f1 << - (Omega1*q2)/2 - (Omega2*q3)/2 - (Omega3*q4)/2 - (alpha*q1*(q1*q1 + q2*q2 + q3*q3 + q4*q4 - 1))/(q1*q1 + q2*q2 + q3*q3 + q4*q4);
f1 << (Omega1*q1)/2 - (Omega3*q3)/2 + (Omega2*q4)/2 - (alpha*q2*(q1*q1 + q2*q2 + q3*q3 + q4*q4 - 1))/(q1*q1 + q2*q2 + q3*q3 + q4*q4);
f1 << (Omega2*q1)/2 + (Omega3*q2)/2 - (Omega1*q4)/2 - (alpha*q3*(q1*q1 + q2*q2 + q3*q3 + q4*q4 - 1))/(q1*q1 + q2*q2 + q3*q3 + q4*q4);
f1 << (Omega3*q1)/2 - (Omega2*q2)/2 + (Omega1*q3)/2 - (alpha*q4*(q1*q1 + q2*q2 + q3*q3 + q4*q4 - 1))/(q1*q1 + q2*q2 + q3*q3 + q4*q4);
f1 << (J3*Omega2*Omega3 - J2*Omega2*Omega3 + (A*Cl*L*rho*(W2*W2 - W4*W4))/2)/J1;
f1 << -(J3*Omega1*Omega3 - J1*Omega1*Omega3 + (A*Cl*L*rho*(W1*W1 - W3*W3))/2)/J2;
f1 << (J2*Omega1*Omega2 - J1*Omega1*Omega2 + (A*Cd*rho*(W1*W1 - W2*W2 + W3*W3 - W4*W4))/2)/J3;
f1 << (A*Cl*rho*(2*q1*q3 + 2*q2*q4)*(W1*W1 + W2*W2 + W3*W3 + W4*W4))/(2*m);
f1 << -(A*Cl*rho*(2*q1*q2 - 2*q3*q4)*(W1*W1 + W2*W2 + W3*W3 + W4*W4))/(2*m);
f1 << (A*Cl*rho*(W1*W1 + W2*W2 + W3*W3 + W4*W4)*(q1*q1 - q2*q2 - q3*q3 + q4*q4))/(2*m) - g;
f1 << V1;
f1 << V2;
f1 << V3;
f1 << P1;
f1 << P2;
f1 << P3;
// Define the quadcopter ODE model in 3-stage format:
// ----------------------------------------------------------
// LINEAR INPUT SYSTEM (STAGE 1):
Matrix M1, A1, B1;
M1 = eye(12);
A1 = zeros(12,12);
B1 = zeros(12,4);
A1(4,0) = 1.0;
A1(5,1) = 1.0;
A1(6,2) = 1.0;
A1(7,3) = 1.0;
A1(8,4) = 1.0/Jp; A1(8,8) = -xi/Jp;
A1(9,5) = 1.0/Jp; A1(9,9) = -xi/Jp;
A1(10,6) = 1.0/Jp; A1(10,10) = -xi/Jp;
A1(11,7) = 1.0/Jp; A1(11,11) = -xi/Jp;
B1(0,0) = gain;
B1(1,1) = gain;
B1(2,2) = gain;
B1(3,3) = gain;
// NONLINEAR SYSTEM (STAGE 2):
f2 << - (Omega1*q2)/2 - (Omega2*q3)/2 - (Omega3*q4)/2 - (alpha*q1*(q1*q1 + q2*q2 + q3*q3 + q4*q4 - 1))/(q1*q1 + q2*q2 + q3*q3 + q4*q4);
f2 << (Omega1*q1)/2 - (Omega3*q3)/2 + (Omega2*q4)/2 - (alpha*q2*(q1*q1 + q2*q2 + q3*q3 + q4*q4 - 1))/(q1*q1 + q2*q2 + q3*q3 + q4*q4);
f2 << (Omega2*q1)/2 + (Omega3*q2)/2 - (Omega1*q4)/2 - (alpha*q3*(q1*q1 + q2*q2 + q3*q3 + q4*q4 - 1))/(q1*q1 + q2*q2 + q3*q3 + q4*q4);
f2 << (Omega3*q1)/2 - (Omega2*q2)/2 + (Omega1*q3)/2 - (alpha*q4*(q1*q1 + q2*q2 + q3*q3 + q4*q4 - 1))/(q1*q1 + q2*q2 + q3*q3 + q4*q4);
f2 << (J3*Omega2*Omega3 - J2*Omega2*Omega3 + (A*Cl*L*rho*(W2*W2 - W4*W4))/2)/J1;
f2 << -(J3*Omega1*Omega3 - J1*Omega1*Omega3 + (A*Cl*L*rho*(W1*W1 - W3*W3))/2)/J2;
f2 << (J2*Omega1*Omega2 - J1*Omega1*Omega2 + (A*Cd*rho*(W1*W1 - W2*W2 + W3*W3 - W4*W4))/2)/J3;
f2 << (A*Cl*rho*(2*q1*q3 + 2*q2*q4)*(W1*W1 + W2*W2 + W3*W3 + W4*W4))/(2*m);
f2 << -(A*Cl*rho*(2*q1*q2 - 2*q3*q4)*(W1*W1 + W2*W2 + W3*W3 + W4*W4))/(2*m);
f2 << (A*Cl*rho*(W1*W1 + W2*W2 + W3*W3 + W4*W4)*(q1*q1 - q2*q2 - q3*q3 + q4*q4))/(2*m) - g;
// LINEAR OUTPUT SYSTEM (STAGE 3):
Matrix M3, A3;
M3 = eye(6);
A3 = zeros(6,6);
A3(3,0) = 1.0;
A3(4,1) = 1.0;
A3(5,2) = 1.0;
OutputFcn f3;
f3 << V1;
f3 << V2;
f3 << V3;
f3 << 0.0;
f3 << 0.0;
f3 << 0.0;
// ----------------------------------------------------------
// ----------------------------------------------------------
SIMexport sim1( 10, 1.0 );
sim1.setModel( f1 );
sim1.set( INTEGRATOR_TYPE, INT_IRK_GL4 );
sim1.set( NUM_INTEGRATOR_STEPS, 50 );
sim1.setTimingSteps( 10000 );
acadoPrintf( "-----------------------------------------------------------\n Using a QuadCopter ODE model in fully nonlinear form:\n-----------------------------------------------------------\n" );
sim1.exportAndRun( "quadcopter_export", "init_quadcopter.txt", "controls_quadcopter.txt" );
// ----------------------------------------------------------
// ----------------------------------------------------------
SIMexport sim2( 10, 1.0 );
sim2.setLinearInput( M1, A1, B1 );
sim2.setModel( f2 );
sim2.setLinearOutput( M3, A3, f3 );
sim2.set( INTEGRATOR_TYPE, INT_IRK_GL4 );
sim2.set( NUM_INTEGRATOR_STEPS, 50 );
sim2.setTimingSteps( 10000 );
acadoPrintf( "-----------------------------------------------------------\n Using a QuadCopter ODE model in 3-stage format:\n-----------------------------------------------------------\n" );
sim2.exportAndRun( "quadcopter_export", "init_quadcopter.txt", "controls_quadcopter.txt" );
return 0;
}
int main() {
USING_NAMESPACE_ACADO
// ----------------------------------------------------------
DifferentialState x;
DifferentialState y;
DifferentialState alpha;
DifferentialState dx;
DifferentialState dy;
DifferentialState dalpha;
AlgebraicState ddx;
AlgebraicState ddy;
AlgebraicState ddalpha;
AlgebraicState Fx;
AlgebraicState Fy;
Control u;
DifferentialEquation f;
OutputFcn h;
h << Fx;
h << Fy;
const double m = 2;
const double M = 3.5;
const double I = 0.1;
const double g = 9.81;
f << 0 == dot( x ) - dx;
f << 0 == dot( y ) - dy;
f << 0 == dot( alpha ) - dalpha;
f << 0 == dot( dx ) - ddx ;
f << 0 == dot( dy ) - ddy;
f << 0 == dot( dalpha ) - ddalpha;
f << 0 == m*ddx - (Fx+u);
f << 0 == m*ddy + m*g - (Fy+u);
f << 0 == I*ddalpha - M - (Fx+u)*y + (Fy+u)*x;
f << 0 == ddx + dy*dalpha + y*ddalpha;
f << 0 == ddy - dx*dalpha - x*ddalpha;
// ----------------------------------------------------------
Vector Meas(1);
Meas(0) = 5;
SIMexport sim( 1, 0.1 );
sim.set( INTEGRATOR_TYPE, INT_IRK_RIIA3 );
sim.set( NUM_INTEGRATOR_STEPS, 4 );
sim.set( MEASUREMENT_GRID, EQUIDISTANT_GRID );
sim.setModel( f );
sim.addOutput( h );
sim.setMeasurements( Meas );
sim.setTimingSteps( 10000 );
acadoPrintf( "-----------------------------------------\n Using a Pendulum DAE model in ACADO syntax:\n-----------------------------------------\n" );
sim.exportAndRun( "externModel_export", "init_externModel.txt", "controls_externModel.txt" );
SIMexport sim2( 1, 0.1 );
sim2.set( INTEGRATOR_TYPE, INT_IRK_RIIA3 );
sim2.set( NUM_INTEGRATOR_STEPS, 4 );
sim2.set( MEASUREMENT_GRID, EQUIDISTANT_GRID );
sim2.setModel( "model", "rhs", "rhs_jac" );
sim2.setDimensions( 6, 6, 5, 1 );
sim2.addOutput( "out", "out_jac", 2 );
sim2.setMeasurements( Meas );
sim2.setTimingSteps( 10000 );
acadoPrintf( "-----------------------------------------\n Using an externally defined Pendulum DAE model:\n-----------------------------------------\n" );
sim2.exportAndRun( "externModel_export", "init_externModel.txt", "controls_externModel.txt" );
return 0;
}
int
main(int argc, char **argv, char **envp)
{
char *s;
int i, exit_code;
simoutorder sim1(OCORE), sim2(RCORE);
#ifndef _MSC_VER
/* catch SIGUSR1 and dump intermediate stats */
signal(SIGUSR1, signal_sim_stats);
/* catch SIGUSR2 and dump final stats and exit */
signal(SIGUSR2, signal_exit_now);
#endif /* _MSC_VER */
/* register an error handler */
fatal_hook(sim_print_stats_prev);
/* set up a non-local exit point */
if ((exit_code = setjmp(sim_exit_buf)) != 0)
{
/* special handling as longjmp cannot pass 0 */
if(isSim1Run)
{
exit_now(exit_code-1, &sim1);
isSim1Run = false;
goto sim2ex;
}
else if(isSim2Run)
{
exit_now(exit_code-1, &sim2);
isSim2Run = false;
exit(0);
}
else
{
fprintf(stderr, "\n\n fatal error during exit\n");
}
}
banner(stderr, argc, argv);
sim_num_insn = 0;
#ifdef BFD_LOADER
/* initialize the bfd library */
bfd_init();
#endif /* BFD_LOADER */
/* initialize the instruction decoder */
md_init_decoder();
init( &sim1,
argc,
argv,
envp
);
init( &sim2,
argc,
argv,
envp
);
/* record start of execution time, used in rate stats */
sim_start_time = time((time_t *)NULL);
/* emit the command line for later reuse */
fprintf(stderr, "sim: command line: ");
for (i=0; i < argc; i++)
fprintf(stderr, "%s ", argv[i]);
fprintf(stderr, "\n");
/* output simulation conditions */
s = ctime(&sim_start_time);
if (s[strlen(s)-1] == '\n')
s[strlen(s)-1] = '\0';
fprintf(stderr, "\nsim: simulation started @ %s, options follow:\n", s);
opt_print_options(sim1.sim_odb, stderr, /* short */TRUE, /* notes */TRUE);
sim1.sim_aux_config(stderr);
fprintf(stderr, "\n");
/* omit option dump time from rate stats */
sim_start_time = time((time_t *)NULL);
if (init_quit)
exit_now(0, &sim1);
running = TRUE;
isSim1Run = true;
sim1.sim_main();
running = TRUE;
sim2ex:
isSim2Run = true;
sim2.sim_main();
/* simulation finished early */
exit_now(0, &sim1);
return 0;
}
