@@ -4650,10 +4650,10 @@ def MRACwithBASELINE_SafetyMechanism(t, y):
46504650 DATA_vector [68 :71 ] = Moment_adaptive
46514651 DATA_vector [71 :74 ] = Moment_baseline_PI
46524652
4653- DATA_vector [74 :77 ] = omega_ref_dot
4654- DATA_vector [77 :80 ] = omega_cmd_dot
4655- DATA_vector [80 :83 ] = omega_cmd
4656- DATA_vector [83 :86 ] = angular_position_dot
4653+ # DATA_vector[74:77] = omega_ref_dot
4654+ # DATA_vector[77:80] = omega_cmd_dot
4655+ # DATA_vector[80:83] = omega_cmd
4656+ # DATA_vector[83:86] = angular_position_dot
46574657
46584658 DATA = np .append (DATA ,np .resize (DATA_vector ,(size_DATA ,1 )), axis = 1 )
46594659 ###################################################################
@@ -4741,123 +4741,126 @@ def MRACwithBASELINE_SafetyMechanism(t, y):
47414741
47424742 # Print data to Console -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
47434743 print ('\n Simulation time: ' , time_now )
4744-
4745- print ('omega_ref_dot: ' , '%.4f' % omega_ref_dot [0 ], '%.4f' % omega_ref_dot [1 ], '%.4f' % omega_ref_dot [2 ])
4746- print ('omega_ref: ' , '%.4f' % omega_ref [0 ], '%.4f' % omega_ref [1 ], '%.4f' % omega_ref [2 ])
4747- print ('omega_cmd_dot: ' , '%.8f' % omega_cmd_dot [0 ],
4748- '%.8f' % omega_cmd_dot [1 ],
4749- '%.8f' % omega_cmd_dot [2 ])
4750- print ('omega_cmd: ' , '%.4f' % omega_cmd [0 ], '%.4f' % omega_cmd [1 ], '%.4f' % omega_cmd [2 ])
4751- # print('Jacobian_matrix_dot: ', Jacobian_matrix_dot)
4752- # print('Jacobian_matrix: ', Jacobian_matrix)
4753- print ('integral_angular_error: ' , '%.4f' % integral_angular_error [0 ], '%.4f' % integral_angular_error [1 ], '%.4f' % integral_angular_error [2 ])
4754- print ('angular_error: ' , '%.4f' % angular_error [0 ], '%.4f' % angular_error [1 ], '%.4f' % angular_error [2 ])
4755- print ('angular_error_dot: ' , '%.4f' % angular_error_dot [0 ], '%.4f' % angular_error_dot [1 ], '%.4f' % angular_error_dot [2 ])
4756-
4757-
4758- # print('z - z_ref: ', pos_pixhawk_LOC_to_GLOB_NED.z - z_ref)
4759- print ('Z_onboard: ' , '%.4f' % pos_pixhawk_LOC_to_GLOB_NED .z )
4760- # print('(G_acc - controller_z_output)*mass_total: ', (G_acc + controller_z_output)*mass_total)
4761- print ('Thrust T: ' , '%.4f' % T [0 ], '%.4f' % T [1 ], '%.4f' % T [2 ], '%.4f' % T [3 ], '%.4f' % T [4 ], '%.4f' % T [5 ], '%.4f' % T [6 ], '%.4f' % T [7 ])
4762- print ('Total thrust: ' , '%4f' % np .sum (T ))
4763- print ('Total torque around Z_onboard: ' , '%.4f' % torque_total )
4764- # print('Pixhawk Euler 321 angles [rad]: ', chvector_to_list(pixhawk_euler))
4765-
4766- print ('Pixhawk Euler 321 angles [deg]: ' , pixhawk_euler_deg_trunc )
4767- # print('Pixhawk LOCAL angular velocity [rad/s]: ', Wvel_pixhawk_LOC)
4768- # print('Pixhawk GLOBAL angular velocity [rad/s]: ', Wvel_pixhawk_GLOB)
4769-
4770-
4771- # print('Pixhawk Euler 321 angles OPPOSITE [deg]: ', pixhawk_euler_opposite_deg_trunc)
4772- # print('Controllers U1 U2 U3 U4:', ' '.join([f'{u:.4f}' for u in U]))
4773- print ('Controllers U1 U2 U3 U4: ' , '%.4f' % U [0 ], '%.4f' % U [1 ], '%.4f' % U [2 ], '%.4f' % U [3 ])
4774-
4775- print ('mu: ' , '%.4f' % mu_x , '%.4f' % mu_y , '%.4f' % mu_z )
4776-
4777- # print ('Roll reference [rad]: ', roll_ref)
4778- # print ('Pitch reference [rad]: ', pitch_ref)
4779- # print ('Yaw reference [rad]: ', yaw_ref)
4780-
4781- print ('Roll reference [deg]: ' , '%.4f' % np .rad2deg (roll_ref ))
4782- print ('Pitch reference [deg]: ' , '%.4f' % np .rad2deg (pitch_ref ))
4783- print ('Yaw reference [deg]: ' , '%.4f' % np .rad2deg (yaw_ref ))
4784-
4785- print ('mu_PD_baseline_tran [norm x y z]: ' , '%.4f' % LA .norm (mu_PD_baseline_tran ), '%.4f' % mu_PD_baseline_tran [0 ].item (), '%.4f' % mu_PD_baseline_tran [1 ].item (), '%.4f' % mu_PD_baseline_tran [2 ].item ())
4786- print ('mu_baseline_tran [norm x y z]: ' , '%.4f' % LA .norm (mu_baseline_tran ), '%.4f' % mu_baseline_tran [0 ].item (), '%.4f' % mu_baseline_tran [1 ].item (), '%.4f' % mu_baseline_tran [2 ].item ())
4787- print ('mu_adaptive_tran [norm x y z]: ' , '%.4f' % LA .norm (mu_adaptive_tran ), '%.4f' % mu_adaptive_tran [0 ].item (), '%.4f' % mu_adaptive_tran [1 ].item (), '%.4f' % mu_adaptive_tran [2 ].item ())
4788- print ('Moment_baseline_PI [norm x y z]: ' , '%.4f' % LA .norm (Moment_baseline_PI ), '%.4f' % Moment_baseline_PI [0 ].item (), '%.4f' % Moment_baseline_PI [1 ].item (), '%.4f' % Moment_baseline_PI [2 ].item ())
4789- print ('Moment_baseline [norm x y z]: ' , '%.4f' % LA .norm (Moment_baseline ), '%.4f' % Moment_baseline [0 ].item (), '%.4f' % Moment_baseline [1 ].item (), '%.4f' % Moment_baseline [2 ].item ())
4790- print ('Moment_adaptive [norm x y z]: ' , '%.4f' % LA .norm (Moment_adaptive ), '%.4f' % Moment_adaptive [0 ].item (), '%.4f' % Moment_adaptive [1 ].item (), '%.4f' % Moment_adaptive [2 ].item ())
4791-
4792- print ('Time the simulation is taking: ' , '%.4f' % simulation_time )
4793-
4794- # print('Accumulated force: ', my_frame.Get_accumulated_force())
4795- # print('Aerodynamic force: ', '%.4f'%aerodynamic_force[0].item(), '%.4f'%aerodynamic_force[1].item(), '%.4f'%aerodynamic_force[2].item())
4796-
4797- #$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$
4798-
4799- # print('Ball 3 Euler 321 angles [deg]: ', ball3_euler_deg_trunc)
4800- # print('Ball 3 Rotmat: ', chmatrix33_to_list(ball3_rotmat))
4801-
4802- # print('omega: ', omega)
4803-
4804- # print('Drone Frame Position: ', my_frame_pos)
4805- # print('Pixhawk Coordinate: ', chcoordsys_to_list(coord_pixhawk_GLOB)[0],'\n') # '\n' prints in a new line
4806-
4807-
4808- # print('Pixhawk LOCAL Position: ', chvector_to_list(pos_pixhawk_LOC))
4809- print ('Pixhawk GLOBAL Position: ' , '%.4f' % chcoordsys_to_list (coord_pixhawk_GLOB )[0 ][0 ],
4810- '%.4f' % chcoordsys_to_list (coord_pixhawk_GLOB )[0 ][1 ],
4811- '%.4f' % chcoordsys_to_list (coord_pixhawk_GLOB )[0 ][2 ])
4812- # print('Pixhawk GLOBAL Velocity: ', chcoordsys_to_list(coord_dt_pixhawk_GLOB)[0])
4813- # print('Pixhawk LOCAL Velocity_T: ', chvector_to_list(vel_pixhawk_LOC_T))
4814- # print('Pixhawk LOCAL Velocity: ', chvector_to_list(vel_pixhawk_LOC))
4815- print ('Pixhawk GLOBAL Velocity NORM: ' , '%.4f' % LA .norm (np .asarray (chcoordsys_to_list (coord_dt_pixhawk_GLOB )[0 ])))
4816- # print('Pixhawk LOCAL Velocity_T NORM: ', LA.norm(np.asarray(chvector_to_list(vel_pixhawk_LOC_T))))
4817- # print('Pixhawk LOCAL Velocity NORM: ', LA.norm(np.asarray(chvector_to_list(vel_pixhawk_LOC))),'\n')
4818-
4819- # print('Pixhawk Acceleration: ', chcoordsys_to_list(coord_dtdt_pixhawk_GLOB)[0])
4820- # print('Pixhawk Angular Velocity: ', chvector_to_list(Wvel_pixhawk_GLOB))
4821- # print('Pixhawk Angular Acceleration: ', chvector_to_list(Wacc_pixhawk_GLOB))
4822-
4823- # print('Drone Frame Position: ', chvector_to_list(my_frame_pos))
4824-
4825- # print('Ball_1 Position: ', my_ball1_pos)
4826- # print('Ball_1 Position seen from the Box Ref. Sys.: ', my_ball1_pos_box)
4827-
4828- # print('Ball_3 GLOBAL Velocity: ', chcoordsys_to_list(my_ball3.GetCoord_dt())[0])
4829- # print('Ball_3 LOCAL Velocity: ', chvector_to_list(coord_dt_ball3_LOC),'\n')
4830- # print('Ball_3 Quaternion: ', chcoordsys_to_list(my_ball3.GetCoord())[1],'\n')
4831- # print('Ball_3 Euler 321 angles: ', chvector_to_list(euler321_fromQ_asChVector(my_ball3.GetCoord().rot)),'\n')
4832- # print('Ball_3 Euler 123 angles: ', chvector_to_list(my_ball3.GetCoord().rot.Q_to_Euler123()),'\n')
4833-
4834- # print('COG Position: ', chvector_to_list(COG_total))
4835-
4836-
4837- # my_ball1_cfr = my_ball1.GetContactForce() # contact force applied to ball 1
4838- # print('Contact Force Ball_1: ', my_ball1_cfr)
4839-
4840- # my_ball2_cfr = my_ball2.GetContactForce()
4841- # my_ball_cfr_list.append([time_now, chvector_to_list(my_ball1_cfr)[0], chvector_to_list(my_ball1_cfr)[1], chvector_to_list(my_ball1_cfr)[2],chvector_to_list(my_ball2_cfr)[0], chvector_to_list(my_ball2_cfr)[1], chvector_to_list(my_ball2_cfr)[2]])
4842- # print('Contact Force Ball_2: ', my_ball2_cfr)
4843-
4844- # my_box_cfr = my_box.GetContactForce()
4845- # print('Contact Force Box: ', my_box_cfr)
4846-
4847- # my_frame_cfr = my_frame.GetContactForce()
4848- # print('Contact Force Drone Frame: ', my_box_cfr, '\n')
4849-
4850- # my_ball1_ctr = my_ball1.GetContactTorque() # contact torque applied to ball 1
4851- # print('Contact Torque Ball_1: ', my_ball1_ctr)
4852-
4853- # my_ball2_ctr = my_ball2.GetContactTorque()
4854- # print('Contact Torque Ball_2: ', my_ball2_ctr)
48554744
4856- # my_box_ctr = my_box.GetContactTorque()
4857- # print('Contact Torque Box: ', my_box_ctr)
4858-
4859- # my_frame_ctr = my_frame.GetContactTorque()
4860- # print('Contact Torque Drone Frame: ', my_box_ctr, '\n')
4745+ print_cosole_flag = False
4746+ if (print_cosole_flag ):
4747+
4748+ # print('omega_ref_dot: ', '%.4f'%omega_ref_dot[0], '%.4f'%omega_ref_dot[1], '%.4f'%omega_ref_dot[2])
4749+ # print('omega_ref: ', '%.4f'%omega_ref[0], '%.4f'%omega_ref[1], '%.4f'%omega_ref[2])
4750+ # print('omega_cmd_dot: ', '%.8f'%omega_cmd_dot[0],
4751+ # '%.8f'%omega_cmd_dot[1],
4752+ # '%.8f'%omega_cmd_dot[2])
4753+ # print('omega_cmd: ', '%.4f'%omega_cmd[0], '%.4f'%omega_cmd[1], '%.4f'%omega_cmd[2])
4754+ # print('Jacobian_matrix_dot: ', Jacobian_matrix_dot)
4755+ # print('Jacobian_matrix: ', Jacobian_matrix)
4756+ # print('integral_angular_error: ', '%.4f'%integral_angular_error[0], '%.4f'%integral_angular_error[1], '%.4f'%integral_angular_error[2])
4757+ print ('angular_error: ' , '%.4f' % angular_error [0 ], '%.4f' % angular_error [1 ], '%.4f' % angular_error [2 ])
4758+ print ('angular_error_dot: ' , '%.4f' % angular_error_dot [0 ], '%.4f' % angular_error_dot [1 ], '%.4f' % angular_error_dot [2 ])
4759+
4760+
4761+ # print('z - z_ref: ', pos_pixhawk_LOC_to_GLOB_NED.z - z_ref)
4762+ print ('Z_onboard: ' , '%.4f' % pos_pixhawk_LOC_to_GLOB_NED .z )
4763+ # print('(G_acc - controller_z_output)*mass_total: ', (G_acc + controller_z_output)*mass_total)
4764+ print ('Thrust T: ' , '%.4f' % T [0 ], '%.4f' % T [1 ], '%.4f' % T [2 ], '%.4f' % T [3 ], '%.4f' % T [4 ], '%.4f' % T [5 ], '%.4f' % T [6 ], '%.4f' % T [7 ])
4765+ print ('Total thrust: ' , '%4f' % np .sum (T ))
4766+ print ('Total torque around Z_onboard: ' , '%.4f' % torque_total )
4767+ # print('Pixhawk Euler 321 angles [rad]: ', chvector_to_list(pixhawk_euler))
4768+
4769+ print ('Pixhawk Euler 321 angles [deg]: ' , pixhawk_euler_deg_trunc )
4770+ # print('Pixhawk LOCAL angular velocity [rad/s]: ', Wvel_pixhawk_LOC)
4771+ # print('Pixhawk GLOBAL angular velocity [rad/s]: ', Wvel_pixhawk_GLOB)
4772+
4773+
4774+ # print('Pixhawk Euler 321 angles OPPOSITE [deg]: ', pixhawk_euler_opposite_deg_trunc)
4775+ # print('Controllers U1 U2 U3 U4:', ' '.join([f'{u:.4f}' for u in U]))
4776+ print ('Controllers U1 U2 U3 U4: ' , '%.4f' % U [0 ], '%.4f' % U [1 ], '%.4f' % U [2 ], '%.4f' % U [3 ])
4777+
4778+ print ('mu: ' , '%.4f' % mu_x , '%.4f' % mu_y , '%.4f' % mu_z )
4779+
4780+ # print ('Roll reference [rad]: ', roll_ref)
4781+ # print ('Pitch reference [rad]: ', pitch_ref)
4782+ # print ('Yaw reference [rad]: ', yaw_ref)
4783+
4784+ print ('Roll reference [deg]: ' , '%.4f' % np .rad2deg (roll_ref ))
4785+ print ('Pitch reference [deg]: ' , '%.4f' % np .rad2deg (pitch_ref ))
4786+ print ('Yaw reference [deg]: ' , '%.4f' % np .rad2deg (yaw_ref ))
4787+
4788+ print ('mu_PD_baseline_tran [norm x y z]: ' , '%.4f' % LA .norm (mu_PD_baseline_tran ), '%.4f' % mu_PD_baseline_tran [0 ].item (), '%.4f' % mu_PD_baseline_tran [1 ].item (), '%.4f' % mu_PD_baseline_tran [2 ].item ())
4789+ print ('mu_baseline_tran [norm x y z]: ' , '%.4f' % LA .norm (mu_baseline_tran ), '%.4f' % mu_baseline_tran [0 ].item (), '%.4f' % mu_baseline_tran [1 ].item (), '%.4f' % mu_baseline_tran [2 ].item ())
4790+ print ('mu_adaptive_tran [norm x y z]: ' , '%.4f' % LA .norm (mu_adaptive_tran ), '%.4f' % mu_adaptive_tran [0 ].item (), '%.4f' % mu_adaptive_tran [1 ].item (), '%.4f' % mu_adaptive_tran [2 ].item ())
4791+ print ('Moment_baseline_PI [norm x y z]: ' , '%.4f' % LA .norm (Moment_baseline_PI ), '%.4f' % Moment_baseline_PI [0 ].item (), '%.4f' % Moment_baseline_PI [1 ].item (), '%.4f' % Moment_baseline_PI [2 ].item ())
4792+ print ('Moment_baseline [norm x y z]: ' , '%.4f' % LA .norm (Moment_baseline ), '%.4f' % Moment_baseline [0 ].item (), '%.4f' % Moment_baseline [1 ].item (), '%.4f' % Moment_baseline [2 ].item ())
4793+ print ('Moment_adaptive [norm x y z]: ' , '%.4f' % LA .norm (Moment_adaptive ), '%.4f' % Moment_adaptive [0 ].item (), '%.4f' % Moment_adaptive [1 ].item (), '%.4f' % Moment_adaptive [2 ].item ())
4794+
4795+ print ('Time the simulation is taking: ' , '%.4f' % simulation_time )
4796+
4797+ # print('Accumulated force: ', my_frame.Get_accumulated_force())
4798+ # print('Aerodynamic force: ', '%.4f'%aerodynamic_force[0].item(), '%.4f'%aerodynamic_force[1].item(), '%.4f'%aerodynamic_force[2].item())
4799+
4800+ #$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$$
4801+
4802+ # print('Ball 3 Euler 321 angles [deg]: ', ball3_euler_deg_trunc)
4803+ # print('Ball 3 Rotmat: ', chmatrix33_to_list(ball3_rotmat))
4804+
4805+ # print('omega: ', omega)
4806+
4807+ # print('Drone Frame Position: ', my_frame_pos)
4808+ # print('Pixhawk Coordinate: ', chcoordsys_to_list(coord_pixhawk_GLOB)[0],'\n') # '\n' prints in a new line
4809+
4810+
4811+ # print('Pixhawk LOCAL Position: ', chvector_to_list(pos_pixhawk_LOC))
4812+ print ('Pixhawk GLOBAL Position: ' , '%.4f' % chcoordsys_to_list (coord_pixhawk_GLOB )[0 ][0 ],
4813+ '%.4f' % chcoordsys_to_list (coord_pixhawk_GLOB )[0 ][1 ],
4814+ '%.4f' % chcoordsys_to_list (coord_pixhawk_GLOB )[0 ][2 ])
4815+ # print('Pixhawk GLOBAL Velocity: ', chcoordsys_to_list(coord_dt_pixhawk_GLOB)[0])
4816+ # print('Pixhawk LOCAL Velocity_T: ', chvector_to_list(vel_pixhawk_LOC_T))
4817+ # print('Pixhawk LOCAL Velocity: ', chvector_to_list(vel_pixhawk_LOC))
4818+ print ('Pixhawk GLOBAL Velocity NORM: ' , '%.4f' % LA .norm (np .asarray (chcoordsys_to_list (coord_dt_pixhawk_GLOB )[0 ])))
4819+ # print('Pixhawk LOCAL Velocity_T NORM: ', LA.norm(np.asarray(chvector_to_list(vel_pixhawk_LOC_T))))
4820+ # print('Pixhawk LOCAL Velocity NORM: ', LA.norm(np.asarray(chvector_to_list(vel_pixhawk_LOC))),'\n')
4821+
4822+ # print('Pixhawk Acceleration: ', chcoordsys_to_list(coord_dtdt_pixhawk_GLOB)[0])
4823+ # print('Pixhawk Angular Velocity: ', chvector_to_list(Wvel_pixhawk_GLOB))
4824+ # print('Pixhawk Angular Acceleration: ', chvector_to_list(Wacc_pixhawk_GLOB))
4825+
4826+ # print('Drone Frame Position: ', chvector_to_list(my_frame_pos))
4827+
4828+ # print('Ball_1 Position: ', my_ball1_pos)
4829+ # print('Ball_1 Position seen from the Box Ref. Sys.: ', my_ball1_pos_box)
4830+
4831+ # print('Ball_3 GLOBAL Velocity: ', chcoordsys_to_list(my_ball3.GetCoord_dt())[0])
4832+ # print('Ball_3 LOCAL Velocity: ', chvector_to_list(coord_dt_ball3_LOC),'\n')
4833+ # print('Ball_3 Quaternion: ', chcoordsys_to_list(my_ball3.GetCoord())[1],'\n')
4834+ # print('Ball_3 Euler 321 angles: ', chvector_to_list(euler321_fromQ_asChVector(my_ball3.GetCoord().rot)),'\n')
4835+ # print('Ball_3 Euler 123 angles: ', chvector_to_list(my_ball3.GetCoord().rot.Q_to_Euler123()),'\n')
4836+
4837+ # print('COG Position: ', chvector_to_list(COG_total))
4838+
4839+
4840+ # my_ball1_cfr = my_ball1.GetContactForce() # contact force applied to ball 1
4841+ # print('Contact Force Ball_1: ', my_ball1_cfr)
4842+
4843+ # my_ball2_cfr = my_ball2.GetContactForce()
4844+ # my_ball_cfr_list.append([time_now, chvector_to_list(my_ball1_cfr)[0], chvector_to_list(my_ball1_cfr)[1], chvector_to_list(my_ball1_cfr)[2],chvector_to_list(my_ball2_cfr)[0], chvector_to_list(my_ball2_cfr)[1], chvector_to_list(my_ball2_cfr)[2]])
4845+ # print('Contact Force Ball_2: ', my_ball2_cfr)
4846+
4847+ # my_box_cfr = my_box.GetContactForce()
4848+ # print('Contact Force Box: ', my_box_cfr)
4849+
4850+ # my_frame_cfr = my_frame.GetContactForce()
4851+ # print('Contact Force Drone Frame: ', my_box_cfr, '\n')
4852+
4853+ # my_ball1_ctr = my_ball1.GetContactTorque() # contact torque applied to ball 1
4854+ # print('Contact Torque Ball_1: ', my_ball1_ctr)
4855+
4856+ # my_ball2_ctr = my_ball2.GetContactTorque()
4857+ # print('Contact Torque Ball_2: ', my_ball2_ctr)
4858+
4859+ # my_box_ctr = my_box.GetContactTorque()
4860+ # print('Contact Torque Box: ', my_box_ctr)
4861+
4862+ # my_frame_ctr = my_frame.GetContactTorque()
4863+ # print('Contact Torque Drone Frame: ', my_box_ctr, '\n')
48614864 if Wrapper_execution == True :
48624865 DATA = DATA .T
48634866 with open (csv_file_path , mode = 'a' , newline = '' ) as csv_file_wrapper :
0 commit comments