From 35a2c91c866e30dd699fb6bb62fcb80cc762b05e Mon Sep 17 00:00:00 2001 From: "Penn, John M 047828115" Date: Wed, 30 Jun 2021 11:44:58 -0500 Subject: [PATCH] Fix SIM_ball_L3 by integrating an ensemble state. #725 --- .../SIM_ball_L3/DP_Product/DP_ball_force.xml | 8 +-- .../SIM_ball_L3/DP_Product/DP_ball_states.xml | 24 +++---- .../SIM_ball_L3/Modified_data/auto_test.dr | 12 ++-- .../SIM_ball_L3/Modified_data/data_record.dr | 22 +++--- trick_sims/Ball/SIM_ball_L3/RUN_test/input.py | 5 +- trick_sims/Ball/SIM_ball_L3/S_define | 45 ++++++------ .../models/ball/L3/include/ball_ensemble.h | 27 +++++++ .../ball/L3/src/ball_ensemble_collision.c | 34 +++++++++ .../models/ball/L3/src/ball_ensemble_integ.c | 72 +++++++++++++++++++ 9 files changed, 192 insertions(+), 57 deletions(-) create mode 100644 trick_sims/Ball/models/ball/L3/include/ball_ensemble.h create mode 100644 trick_sims/Ball/models/ball/L3/src/ball_ensemble_collision.c create mode 100644 trick_sims/Ball/models/ball/L3/src/ball_ensemble_integ.c diff --git a/trick_sims/Ball/SIM_ball_L3/DP_Product/DP_ball_force.xml b/trick_sims/Ball/SIM_ball_L3/DP_Product/DP_ball_force.xml index 8c325fc8..8c165df9 100644 --- a/trick_sims/Ball/SIM_ball_L3/DP_Product/DP_ball_force.xml +++ b/trick_sims/Ball/SIM_ball_L3/DP_Product/DP_ball_force.xml @@ -11,11 +11,11 @@ Ball X,Y External Forces sys.exec.out.time - ball.state.output.external_force[0] + ball1.state.output.external_force[0] sys.exec.out.time - ball.state.output.external_force[1] + ball1.state.output.external_force[1] sys.exec.out.time @@ -35,8 +35,8 @@ - ball.state.output.external_force[0] - ball.state.output.external_force[1] + ball1.state.output.external_force[0] + ball1.state.output.external_force[1] ball2.state.output.external_force[0] diff --git a/trick_sims/Ball/SIM_ball_L3/DP_Product/DP_ball_states.xml b/trick_sims/Ball/SIM_ball_L3/DP_Product/DP_ball_states.xml index 6c9f84da..c960daf9 100644 --- a/trick_sims/Ball/SIM_ball_L3/DP_Product/DP_ball_states.xml +++ b/trick_sims/Ball/SIM_ball_L3/DP_Product/DP_ball_states.xml @@ -17,11 +17,11 @@ sys.exec.out.time - ball.state.output.position[0] + ball1.state.output.position[0] sys.exec.out.time - ball.state.output.position[1] + ball1.state.output.position[1] sys.exec.out.time @@ -42,11 +42,11 @@ sys.exec.out.time - ball.state.output.velocity[0] + ball1.state.output.velocity[0] sys.exec.out.time - ball.state.output.velocity[1] + ball1.state.output.velocity[1] sys.exec.out.time @@ -67,11 +67,11 @@ sys.exec.out.time - ball.state.output.acceleration[0] + ball1.state.output.acceleration[0] sys.exec.out.time - ball.state.output.acceleration[1] + ball1.state.output.acceleration[1] sys.exec.out.time @@ -94,8 +94,8 @@ - ball.state.output.position[0] - ball.state.output.position[1] + ball1.state.output.position[0] + ball1.state.output.position[1] @@ -107,8 +107,8 @@ - ball.state.output.velocity[0] - ball.state.output.velocity[1] + ball1.state.output.velocity[0] + ball1.state.output.velocity[1] @@ -120,8 +120,8 @@ - ball.state.output.acceleration[0] - ball.state.output.acceleration[1] + ball1.state.output.acceleration[0] + ball1.state.output.acceleration[1] diff --git a/trick_sims/Ball/SIM_ball_L3/Modified_data/auto_test.dr b/trick_sims/Ball/SIM_ball_L3/Modified_data/auto_test.dr index 214cb88e..49c6fb7d 100644 --- a/trick_sims/Ball/SIM_ball_L3/Modified_data/auto_test.dr +++ b/trick_sims/Ball/SIM_ball_L3/Modified_data/auto_test.dr @@ -23,12 +23,12 @@ def add_dr_group(): drg[DR_GROUP_ID].enable() drg[DR_GROUP_ID].set_cycle(10.0) - drg[DR_GROUP_ID].add_variable("ball.state.output.position[0]") - drg[DR_GROUP_ID].add_variable("ball.state.output.position[1]") - drg[DR_GROUP_ID].add_variable("ball.state.output.velocity[0]") - drg[DR_GROUP_ID].add_variable("ball.state.output.velocity[1]") - drg[DR_GROUP_ID].add_variable("ball.state.output.acceleration[0]") - drg[DR_GROUP_ID].add_variable("ball.state.output.acceleration[1]") + drg[DR_GROUP_ID].add_variable("ball1.state.output.position[0]") + drg[DR_GROUP_ID].add_variable("ball1.state.output.position[1]") + drg[DR_GROUP_ID].add_variable("ball1.state.output.velocity[0]") + drg[DR_GROUP_ID].add_variable("ball1.state.output.velocity[1]") + drg[DR_GROUP_ID].add_variable("ball1.state.output.acceleration[0]") + drg[DR_GROUP_ID].add_variable("ball1.state.output.acceleration[1]") drg[DR_GROUP_ID].add_variable("ball2.state.output.position[0]") drg[DR_GROUP_ID].add_variable("ball2.state.output.position[1]") drg[DR_GROUP_ID].add_variable("ball2.state.output.velocity[0]") diff --git a/trick_sims/Ball/SIM_ball_L3/Modified_data/data_record.dr b/trick_sims/Ball/SIM_ball_L3/Modified_data/data_record.dr index aabe9195..28345f92 100644 --- a/trick_sims/Ball/SIM_ball_L3/Modified_data/data_record.dr +++ b/trick_sims/Ball/SIM_ball_L3/Modified_data/data_record.dr @@ -18,19 +18,19 @@ def add_dr_group(): global DR_GROUP_ID global drg group_name = "Ball1" - drg.append( trick.DRBinary(group_name) ) + drg.append( trick.DRAscii(group_name) ) drg[DR_GROUP_ID].set_freq(trick.sim_services.DR_Always) drg[DR_GROUP_ID].enable() drg[DR_GROUP_ID].set_cycle(0.1) - drg[DR_GROUP_ID].add_variable("ball.state.output.position[0]") - drg[DR_GROUP_ID].add_variable("ball.state.output.position[1]") - drg[DR_GROUP_ID].add_variable("ball.state.output.velocity[0]") - drg[DR_GROUP_ID].add_variable("ball.state.output.velocity[1]") - drg[DR_GROUP_ID].add_variable("ball.state.output.acceleration[0]") - drg[DR_GROUP_ID].add_variable("ball.state.output.acceleration[1]") - drg[DR_GROUP_ID].add_variable("ball.state.output.external_force[0]") - drg[DR_GROUP_ID].add_variable("ball.state.output.external_force[1]") + drg[DR_GROUP_ID].add_variable("ball1.state.output.position[0]") + drg[DR_GROUP_ID].add_variable("ball1.state.output.position[1]") + drg[DR_GROUP_ID].add_variable("ball1.state.output.velocity[0]") + drg[DR_GROUP_ID].add_variable("ball1.state.output.velocity[1]") + drg[DR_GROUP_ID].add_variable("ball1.state.output.acceleration[0]") + drg[DR_GROUP_ID].add_variable("ball1.state.output.acceleration[1]") + drg[DR_GROUP_ID].add_variable("ball1.state.output.external_force[0]") + drg[DR_GROUP_ID].add_variable("ball1.state.output.external_force[1]") drg[DR_GROUP_ID].add_variable("ball2.state.output.position[0]") drg[DR_GROUP_ID].add_variable("ball2.state.output.position[1]") drg[DR_GROUP_ID].add_variable("ball2.state.output.velocity[0]") @@ -61,12 +61,12 @@ def add_dr_group(): global DR_GROUP_ID global drg group_name = "BallControl" - drg.append( trick.sim_services.DRBinary(group_name) ) + drg.append( trick.sim_services.DRAscii(group_name) ) drg[DR_GROUP_ID].set_freq(trick.sim_services.DR_Always) drg[DR_GROUP_ID].enable() drg[DR_GROUP_ID].set_cycle(0.02) - drg[DR_GROUP_ID].add_variable("ball.altimeter.output.altitude") + drg[DR_GROUP_ID].add_variable("ball1.altimeter.output.altitude") drg[DR_GROUP_ID].add_variable("ball2.altimeter.output.altitude") trick_data_record.drd.add_group( drg[DR_GROUP_ID], trick.DR_Buffer ) diff --git a/trick_sims/Ball/SIM_ball_L3/RUN_test/input.py b/trick_sims/Ball/SIM_ball_L3/RUN_test/input.py index 9b17bfa2..835525f9 100644 --- a/trick_sims/Ball/SIM_ball_L3/RUN_test/input.py +++ b/trick_sims/Ball/SIM_ball_L3/RUN_test/input.py @@ -1,10 +1,7 @@ - exec(open("Modified_data/data_record.dr").read()) exec(open("Modified_data/auto_test.dr").read()) -ball.altimeter.input.add_noise = False +ball1.altimeter.input.add_noise = False ball2.altimeter.input.add_noise = False -my_integ_loop.getIntegrator(trick.Runge_Kutta_Fehlberg_78, 4) - trick.stop(300.0) diff --git a/trick_sims/Ball/SIM_ball_L3/S_define b/trick_sims/Ball/SIM_ball_L3/S_define index 5d147c60..05806fe3 100644 --- a/trick_sims/Ball/SIM_ball_L3/S_define +++ b/trick_sims/Ball/SIM_ball_L3/S_define @@ -19,6 +19,8 @@ LIBRARY DEPENDENCIES: (ball/L2/src/ball_state_deriv.c) (ball/L2/src/ball_state_init.c) (ball/L2/src/ball_state_integ.c) + (ball/L3/src/ball_ensemble_integ.c) + (ball/L3/src/ball_ensemble_collision.c) ) *************************************************************/ @@ -29,29 +31,23 @@ LIBRARY DEPENDENCIES: ##include "ball/L2/include/ball_control.h" ##include "ball/L2/include/ball_jet.h" ##include "ball/L2/include/ball_state.h" +##include "ball/L3/include/ball_ensemble.h" %{ // Allow C++ access to the these C functions extern "C" { - int ball_altimeter(BALT*,double*) ; int ball_altimeter_default_data(BALT*) ; - double ball_ceiling(BSTATE*) ; int ball_control(BCONTROL*,double) ; - double ball_floor(BSTATE*) ; int ball_force_default_data(BFORCE*) ; int ball_force_field(BFORCE*,double*) ; int ball_jet(BJET*,Flag*) ; - double ball_left_wall(BSTATE*) ; - double ball_right_wall(BSTATE*) ; int ball_state_default_data(BSTATE*) ; int ball_state_deriv(BSTATE*) ; int ball_state_init(BSTATE*) ; - int ball_state_integ(BSTATE*) ; int ball_jet_default_data(BJET*) ; int ball_control_default_data(BCONTROL*) ; - } %} @@ -83,38 +79,47 @@ class ballSimObject : public Trick::SimObject { // EOM DERIVATIVE AND STATE INTEGRATION JOBS ("derivative") ball_force_field( &force, state.output.position ) ; ("derivative") ball_state_deriv( &state ) ; - ("integration") ball_state_integ( &state ) ; - - // DYNAMIC EVENT JOBS - ("dynamic_event") ball_floor( &state ) ; - ("dynamic_event") ball_right_wall( &state ) ; - ("dynamic_event") ball_ceiling( &state ) ; - ("dynamic_event") ball_left_wall( &state ) ; // ALTITUDE SENSING, CONTROL, AND EFFECTING (0.01, "sensor") ball_altimeter( &altimeter, &state.output.position[1] ) ; (0.01, "scheduled") ball_control( &control, altimeter.output.altitude ) ; (0.01, "effector") ball_jet( &jet, &control.output.jet_command[0] ); } - } ; // Instantiations -ballSimObject ball ; +ballSimObject ball1 ; ballSimObject ball2 ; -collect ball.state.work.external_force = { ball.force.output.force[0] , - ball.jet.output.force[0] } ; +class ensembleSimObject : public Trick::SimObject { + + public: + BSTATE* states[2]; + ensembleSimObject () { + ("integration") ball_ensemble_integ( states ) ; + ("dynamic_event") ball_ensemble_collision( states ) ; + } +} ; + +// Instantiation +ensembleSimObject ensemble; + +collect ball1.state.work.external_force = { ball1.force.output.force[0] , + ball1.jet.output.force[0] } ; collect ball2.state.work.external_force = { ball2.force.output.force[0] , ball2.jet.output.force[0] } ; -IntegLoop my_integ_loop (0.01) ball, ball2; +IntegLoop my_integ_loop (0.01) ball1, ball2, ensemble ; // Connect objects void create_connections() { exec_set_freeze_frame(0.10) ; - my_integ_loop.getIntegrator(Runge_Kutta_2, 4); + /* We are integrating the ensemble; that is, ball1 and ball2 at the same time. */ + my_integ_loop.getIntegrator(Runge_Kutta_4, 8); + + ensemble.states[0] = &(ball1.state); + ensemble.states[1] = &(ball2.state); } diff --git a/trick_sims/Ball/models/ball/L3/include/ball_ensemble.h b/trick_sims/Ball/models/ball/L3/include/ball_ensemble.h new file mode 100644 index 00000000..6de04283 --- /dev/null +++ b/trick_sims/Ball/models/ball/L3/include/ball_ensemble.h @@ -0,0 +1,27 @@ +/********************************* TRICK HEADER ******************************* +PURPOSE: + () +CLASS: + (integration) +LIBRARY DEPENDENCY: + ((ball_ensemble_integ.o) + (ball_ensemble_collision.o)) +PROGRAMMERS: + (((Your Name) (Company Name) (Date) (Trick tutorial))) +*******************************************************************************/ +#ifndef _BALL_ENSEMBLE_H_ +#define _BALL_ENSEMBLE_H_ +#include "../../L2/include/ball_state.h" + +#ifdef __cplusplus +extern "C" { +#endif + +int ball_ensemble_integ( BSTATE *S[] ); +double ball_ensemble_collision( BSTATE * states[2] ); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/trick_sims/Ball/models/ball/L3/src/ball_ensemble_collision.c b/trick_sims/Ball/models/ball/L3/src/ball_ensemble_collision.c new file mode 100644 index 00000000..e61fc0a4 --- /dev/null +++ b/trick_sims/Ball/models/ball/L3/src/ball_ensemble_collision.c @@ -0,0 +1,34 @@ +/********************************* TRICK HEADER ******************************* +PURPOSE: () +CLASS: + (dynamic-event) +LIBRARY DEPENDENCY: + ((ball_ensemble_collision.o)) +PROGRAMMERS: + (((Your Name) (Company Name) (Date) (Trick tutorial))) +*o******************************************************************************/ +#include "../include/ball_ensemble.h" + +double ball_ceiling(BSTATE*) ; +double ball_floor(BSTATE*) ; +double ball_left_wall(BSTATE*) ; +double ball_right_wall(BSTATE*) ; + +double ball_ensemble_collision( BSTATE * states[2] ) { + double tgo ; + double event_tgo; + event_tgo = BIG_TGO; + + for (int i=0 ; i < 2 ; i++) { + tgo = ball_ceiling( states[i]); + if (tgo < event_tgo) event_tgo = tgo; + tgo = ball_floor( states[i] ) ; + if (tgo < event_tgo) event_tgo = tgo; + tgo = ball_left_wall( states[i]) ; + if (tgo < event_tgo) event_tgo = tgo; + tgo = ball_right_wall( states[i]) ; + if (tgo < event_tgo) event_tgo = tgo; + } + + return (event_tgo) ; +} diff --git a/trick_sims/Ball/models/ball/L3/src/ball_ensemble_integ.c b/trick_sims/Ball/models/ball/L3/src/ball_ensemble_integ.c new file mode 100644 index 00000000..dbf9c59f --- /dev/null +++ b/trick_sims/Ball/models/ball/L3/src/ball_ensemble_integ.c @@ -0,0 +1,72 @@ +/********************************* TRICK HEADER ******************************* +PURPOSE: + () +CLASS: + (integration) +LIBRARY DEPENDENCY: + ((ball_ensemble_integ.o)) +PROGRAMMERS: + (((Your Name) (Company Name) (Date) (Trick tutorial))) +*******************************************************************************/ + +#include + +/* GLOBAL DATA STRUCTURE DECLARATIONS */ +#include "sim_services/Integrator/include/integrator_c_intf.h" +#include "../include/ball_ensemble.h" + +/* ENTRY POINT */ +int ball_ensemble_integ( BSTATE *S[] ) { + + int ipass; + + /* GET SHORTHAND NOTATION FOR DATA STRUCTURES */ + BSTATE_OUT *SO0 = &(S[0]->output) ; + BSTATE_OUT *SO1 = &(S[1]->output) ; + + /* LOAD THE POSITION AND VELOCITY STATES */ + load_state( + &(SO0->position[0]) , + &(SO0->position[1]) , + &(SO0->velocity[0]) , + &(SO0->velocity[1]) , + &(SO1->position[0]) , + &(SO1->position[1]) , + &(SO1->velocity[0]) , + &(SO1->velocity[1]) , + NULL + ); + + /* LOAD THE POSITION AND VELOCITY STATE DERIVATIVES */ + load_deriv( + &(SO0->velocity[0]) , + &(SO0->velocity[1]) , + &(SO0->acceleration[0]) , + &(SO0->acceleration[1]) , + &(SO1->velocity[0]) , + &(SO1->velocity[1]) , + &(SO1->acceleration[0]) , + &(SO1->acceleration[1]) , + NULL + ); + + /* CALL THE TRICK INTEGRATION SERVICE */ + ipass = integrate(); + + /* UNLOAD THE NEW POSITION AND VELOCITY STATES */ + unload_state( + &(SO0->position[0]) , + &(SO0->position[1]) , + &(SO0->velocity[0]) , + &(SO0->velocity[1]) , + &(SO1->position[0]) , + &(SO1->position[1]) , + &(SO1->velocity[0]) , + &(SO1->velocity[1]) , + NULL + ); + + /* RETURN */ + return( ipass ); +} +