ExternalVendorCode/trick
1
0
Fork 0
mirror of https://github.com/nasa/trick.git synced 2024-12-18 20:57:55 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

Fix SIM_ball_L3 by integrating an ensemble state. #725

Browse Source
This commit is contained in:
Penn, John M 047828115 2021-06-30 11:44:58 -05:00
parent c88faa04df
commit 35a2c91c86
9 changed files with 192 additions and 57 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
trick_sims/Ball/SIM_ball_L3/DP_Product/DP_ball_force.xml
View File

@ -11,11 +11,11 @@
<title>Ball X,Y External Forces</title>
<curve>
<var label="time" units="s">sys.exec.out.time</var>
<var gnuplot_line_style="lines" label="external_force" line_color="#000000" line_style="Plain" symbol_size="Tiny" symbol_style="None" units="N">ball.state.output.external_force[0]</var>
<var gnuplot_line_style="lines" label="external_force" line_color="#000000" line_style="Plain" symbol_size="Tiny" symbol_style="None" units="N">ball1.state.output.external_force[0]</var>
</curve>
<curve>
<var label="time" units="s">sys.exec.out.time</var>
<var gnuplot_line_style="lines" label="Y Force" line_color="#000000" line_style="Med_Fine_Dash" symbol_size="Tiny" symbol_style="None" units="N">ball.state.output.external_force[1]</var>
<var gnuplot_line_style="lines" label="Y Force" line_color="#000000" line_style="Med_Fine_Dash" symbol_size="Tiny" symbol_style="None" units="N">ball1.state.output.external_force[1]</var>
</curve>
<curve>
<var label="time" units="s">sys.exec.out.time</var>
@ -35,8 +35,8 @@
<label>Y Force</label>
</yaxis>
<curve>
<var label="X Force" units="N">ball.state.output.external_force[0]</var>
<var gnuplot_line_style="lines" label="Y Force" line_color="#000000" line_style="Plain" symbol_size="Tiny" symbol_style="None" units="N">ball.state.output.external_force[1]</var>
<var label="X Force" units="N">ball1.state.output.external_force[0]</var>
<var gnuplot_line_style="lines" label="Y Force" line_color="#000000" line_style="Plain" symbol_size="Tiny" symbol_style="None" units="N">ball1.state.output.external_force[1]</var>
</curve>
<curve>
<var label="X Force" units="N">ball2.state.output.external_force[0]</var>

24
trick_sims/Ball/SIM_ball_L3/DP_Product/DP_ball_states.xml
View File

@ -17,11 +17,11 @@
</yaxis>
<curve>
<var label="Time" units="s">sys.exec.out.time</var>
<var gnuplot_line_style="lines" label="X Pos" line_color="#000000" line_style="Plain" symbol_size="Tiny" symbol_style="None" units="m">ball.state.output.position[0]</var>
<var gnuplot_line_style="lines" label="X Pos" line_color="#000000" line_style="Plain" symbol_size="Tiny" symbol_style="None" units="m">ball1.state.output.position[0]</var>
</curve>
<curve>
<var label="Time" units="s">sys.exec.out.time</var>
<var gnuplot_line_style="lines" label="Y Pos" line_color="#000000" line_style="Med_Fine_Dash" symbol_size="Tiny" symbol_style="None" units="m">ball.state.output.position[1]</var>
<var gnuplot_line_style="lines" label="Y Pos" line_color="#000000" line_style="Med_Fine_Dash" symbol_size="Tiny" symbol_style="None" units="m">ball1.state.output.position[1]</var>
</curve>
<curve>
<var units="s">sys.exec.out.time</var>
@ -42,11 +42,11 @@
</yaxis>
<curve>
<var label="Time" units="s">sys.exec.out.time</var>
<var gnuplot_line_style="lines" label="X Vel" line_color="#000000" line_style="Plain" symbol_size="Tiny" symbol_style="None" units="m/s">ball.state.output.velocity[0]</var>
<var gnuplot_line_style="lines" label="X Vel" line_color="#000000" line_style="Plain" symbol_size="Tiny" symbol_style="None" units="m/s">ball1.state.output.velocity[0]</var>
</curve>
<curve>
<var label="Time" units="s">sys.exec.out.time</var>
<var gnuplot_line_style="lines" label="Y Vel" line_color="#000000" line_style="Med_Fine_Dash" symbol_size="Tiny" symbol_style="None" units="m/s">ball.state.output.velocity[1]</var>
<var gnuplot_line_style="lines" label="Y Vel" line_color="#000000" line_style="Med_Fine_Dash" symbol_size="Tiny" symbol_style="None" units="m/s">ball1.state.output.velocity[1]</var>
</curve>
<curve>
<var label="Time" units="s">sys.exec.out.time</var>
@ -67,11 +67,11 @@
</yaxis>
<curve>
<var label="Time" units="s">sys.exec.out.time</var>
<var gnuplot_line_style="lines" label="X Acc" line_color="#000000" line_style="Plain" symbol_size="Tiny" symbol_style="None" units="m/s2">ball.state.output.acceleration[0]</var>
<var gnuplot_line_style="lines" label="X Acc" line_color="#000000" line_style="Plain" symbol_size="Tiny" symbol_style="None" units="m/s2">ball1.state.output.acceleration[0]</var>
</curve>
<curve>
<var label="Time" units="s">sys.exec.out.time</var>
<var gnuplot_line_style="lines" label="Y Acc" line_color="#000000" line_style="Med_Fine_Dash" symbol_size="Tiny" symbol_style="None" units="m/s2">ball.state.output.acceleration[1]</var>
<var gnuplot_line_style="lines" label="Y Acc" line_color="#000000" line_style="Med_Fine_Dash" symbol_size="Tiny" symbol_style="None" units="m/s2">ball1.state.output.acceleration[1]</var>
</curve>
<curve>
<var label="Time" units="s">sys.exec.out.time</var>
@ -94,8 +94,8 @@
<label>Y Pos</label>
</yaxis>
<curve>
<var label="X Pos" units="m">ball.state.output.position[0]</var>
<var gnuplot_line_style="lines" label="Y Pos" line_color="#000000" line_style="Plain" symbol_size="Tiny" symbol_style="None" units="m">ball.state.output.position[1]</var>
<var label="X Pos" units="m">ball1.state.output.position[0]</var>
<var gnuplot_line_style="lines" label="Y Pos" line_color="#000000" line_style="Plain" symbol_size="Tiny" symbol_style="None" units="m">ball1.state.output.position[1]</var>
</curve>
</plot>
<plot background_color="#ede9e3" foreground_color="#000000" grid="Yes" grid_color="#ffffff">
@ -107,8 +107,8 @@
<label>Y Vel</label>
</yaxis>
<curve>
<var label="X Vel" units="m/s">ball.state.output.velocity[0]</var>
<var gnuplot_line_style="lines" label="Y Vel" line_color="#000000" line_style="Plain" symbol_size="Tiny" symbol_style="None" units="m/s">ball.state.output.velocity[1]</var>
<var label="X Vel" units="m/s">ball1.state.output.velocity[0]</var>
<var gnuplot_line_style="lines" label="Y Vel" line_color="#000000" line_style="Plain" symbol_size="Tiny" symbol_style="None" units="m/s">ball1.state.output.velocity[1]</var>
</curve>
</plot>
<plot background_color="#ede9e3" foreground_color="#000000" grid="Yes" grid_color="#ffffff">
@ -120,8 +120,8 @@
<label>Y Accel</label>
</yaxis>
<curve>
<var units="m/s2">ball.state.output.acceleration[0]</var>
<var gnuplot_line_style="lines" label="Y Acc" line_color="#000000" line_style="Plain" symbol_size="Tiny" symbol_style="None" units="m/s2">ball.state.output.acceleration[1]</var>
<var units="m/s2">ball1.state.output.acceleration[0]</var>
<var gnuplot_line_style="lines" label="Y Acc" line_color="#000000" line_style="Plain" symbol_size="Tiny" symbol_style="None" units="m/s2">ball1.state.output.acceleration[1]</var>
</curve>
</plot>
<plot background_color="#ede9e3" foreground_color="#000000" grid="Yes" grid_color="#ffffff">

12
trick_sims/Ball/SIM_ball_L3/Modified_data/auto_test.dr
View File

@ -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]")

22
trick_sims/Ball/SIM_ball_L3/Modified_data/data_record.dr
View File

@ -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 )

5
trick_sims/Ball/SIM_ball_L3/RUN_test/input.py
View File

@ -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)

45
trick_sims/Ball/SIM_ball_L3/S_define
View File

@ -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);
}

27
trick_sims/Ball/models/ball/L3/include/ball_ensemble.h Normal file
View File

@ -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

34
trick_sims/Ball/models/ball/L3/src/ball_ensemble_collision.c Normal file
View File

@ -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) ;
}

72
trick_sims/Ball/models/ball/L3/src/ball_ensemble_integ.c Normal file
View File

@ -0,0 +1,72 @@
/********************************* TRICK HEADER *******************************
PURPOSE:
()
CLASS:
(integration)
LIBRARY DEPENDENCY:
((ball_ensemble_integ.o))
PROGRAMMERS:
(((Your Name) (Company Name) (Date) (Trick tutorial)))
*******************************************************************************/
#include <stdio.h>
/* 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 );
}