Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
nps_fdm_pybullet.c
Go to the documentation of this file.
1/*
2 * Copyright (C) 2021 Fabien-B <fabien-b@github.com>
3 *
4 * This file is part of paparazzi. See LICENCE file.
5 */
6
7#include "nps_fdm.h"
8
9#include <stdlib.h>
10#include <stdio.h>
11#include <Python.h>
12
13#include "math/pprz_geodetic.h"
16#include "math/pprz_algebra.h"
18#include "math/pprz_isa.h"
19
20#include "generated/airframe.h"
21#include "generated/flight_plan.h"
22
23#include "state.h"
24
25#define _WIDEN(x) L ## x
26#define WIDEN(x) _WIDEN(x)
27
28#ifndef NPS_PYTHON_EXEC
29#define NPS_PYTHON_EXEC "python3"
30#else
31MESSAGE("PyBullet using" VALUE(NPS_PYTHON_EXEC))
32#endif
33
34#ifndef NPS_PYBULLET_GUI
35#define NPS_PYBULLET_GUI TRUE
36#endif
37
38#ifdef NPS_PYBULLET_ACTUATORS_ORDER
39 int actuators_order[ACTUATORS_NB] = NPS_PYBULLET_ACTUATORS_ORDER;
40#else
41 #error "[PyBullet] missing NPS_PYBULLET_ACTUATORS_ORDER define!"
42#endif
43
44#ifndef NPS_PYBULLET_MODULE
45#define NPS_PYBULLET_MODULE "simple_quad_sim"
46MESSAGE("NPS_PYBULLET_MODULE not defined, take 'simple_quad_sim' as default value.")
47#endif
48
49
50#ifndef NPS_PYBULLET_URDF
51#define NPS_PYBULLET_URDF "robobee.urdf"
52MESSAGE("NPS_PYBULLET_URDF not defined, take 'robobee.urdf' as default value.")
53#endif
54
55
56// NpsFdm structure
57struct NpsFdm fdm;
58
59// Reference point
60static struct LtpDef_d ltpdef;
61
62// rotation from pybullet to pprz
63// due to ENU body_x and NED body_x having a rotatio of 90° around z
65 1/sqrt(2),
66 0,
67 0,
68 1/sqrt(2),
69};
70
71// python object declarations
72PyObject *fdm_module = NULL;
73PyObject *bullet_fdm = NULL;
74
75// static python related function declaration
76static void py_check_status(PyConfig* config, PyStatus* status);
77static void py_check(bool exit_on_error, int line_nb);
78static void python_init(double dt);
79
80// Static functions declaration
81static void init_ltp(void);
82
83static void get_pos(PyObject* ppos);
84static void get_vel(PyObject* pvel);
85static void get_acc(PyObject* pacc);
86
87static void get_orient(PyObject* porient);
88static void get_ang_vel(PyObject* pang_vel);
89static void get_ang_acc(PyObject* pang_acc);
90
91void nps_fdm_init(double dt)
92{
93 fdm.init_dt = dt; // (1 / simulation freq)
94 fdm.curr_dt = dt;
95 fdm.time = dt;
96
97 fdm.on_ground = TRUE;
98
99 fdm.nan_count = 0;
100 fdm.pressure = -1;
101 fdm.pressure_sl = PPRZ_ISA_SEA_LEVEL_PRESSURE;
102 fdm.total_pressure = -1;
103 fdm.dynamic_pressure = -1;
104 fdm.temperature = -1;
105
106 fdm.ltpprz_to_body_eulers.psi = 0.0;
107 init_ltp();
108}
109
111{
112 python_init(fdm.init_dt);
113
114 // run a first step to initialize all fdm fields
115 double dummy_commands[] = {0., 0., 0., 0.};
116 nps_fdm_run_step(false, dummy_commands, 4);
117}
118
119void nps_fdm_run_step(bool launch __attribute__((unused)), double *commands, int commands_nb)
120{
121 // TODO create a np.array instead ?
122 PyObject* pcmd = PyList_New(commands_nb);
123
124 for (int i=0; i<commands_nb; i++) {
125 int j = actuators_order[i];
126 PyList_SetItem(pcmd, i, PyFloat_FromDouble(commands[j]));
127 }
128
129 if (commands[0] < 0) {
130 return;
131 }
132
133 PyObject* ret = PyObject_CallMethod(bullet_fdm, "step", "(O)", pcmd);
134 py_check(true, __LINE__);
135
136 // borrowed references
137 PyObject* ppos = PyDict_GetItemString(ret, "pos");
138 PyObject* pvel = PyDict_GetItemString(ret, "vel");
139 PyObject* pacc = PyDict_GetItemString(ret, "accel");
140
141 PyObject* pquat = PyDict_GetItemString(ret, "quat");
142 PyObject* pang_v = PyDict_GetItemString(ret, "ang_v");
143 PyObject* pang_acc = PyDict_GetItemString(ret, "ang_accel");
144 py_check(true, __LINE__);
145
146
147 get_pos(ppos);
148 get_orient(pquat);
149 get_vel(pvel);
150 get_acc(pacc);
151 get_ang_vel(pang_v);
152 get_ang_acc(pang_acc);
153
154 Py_XDECREF(pcmd);
155 Py_XDECREF(ret);
156}
157
158static void get_pos(PyObject* ppos) {
159 // get position from pybullet, in ENU, in the local frame
160 struct EnuCoor_d enu_pos;
161 enu_pos.x = PyFloat_AsDouble(PyTuple_GetItem(ppos, 0));
162 enu_pos.y = PyFloat_AsDouble(PyTuple_GetItem(ppos, 1));
163 enu_pos.z = PyFloat_AsDouble(PyTuple_GetItem(ppos, 2));
164 py_check(true, __LINE__);
165
166 /*********** positions *************/
167 VECT3_NED_OF_ENU(fdm.ltpprz_pos, enu_pos)
168 ecef_of_enu_point_d(&fdm.ecef_pos, &ltpdef, &enu_pos);
169 lla_of_ecef_d(&fdm.lla_pos, &fdm.ecef_pos);
170 fdm.hmsl = -fdm.ltpprz_pos.z; //FIXME
171 fdm.agl = fdm.hmsl; //FIXME
172}
173
174static void get_vel(PyObject* pvel) {
175 // get velocity from pybullet, in ENU wrt local frame (fixed/world)
176 struct EnuCoor_d enu_vel;
177 enu_vel.x = PyFloat_AsDouble(PyTuple_GetItem(pvel, 0));
178 enu_vel.y = PyFloat_AsDouble(PyTuple_GetItem(pvel, 1));
179 enu_vel.z = PyFloat_AsDouble(PyTuple_GetItem(pvel, 2));
180 py_check(true, __LINE__);
181
182 /*********** velocities *************/
184 VECT3_NED_OF_ENU(fdm.ltp_ecef_vel, enu_vel);
186 ecef_of_enu_vect_d(&fdm.ecef_ecef_vel, &ltpdef, &enu_vel);
187 // /** velocity in body frame, wrt ECEF frame */
188 // struct DoubleVect3 body_ecef_vel; /* aka UVW */
189 // /** velocity in ltppprz frame, wrt ECEF frame */
190 fdm.ltpprz_ecef_vel = fdm.ltp_ecef_vel;
191}
192
193static void get_acc(PyObject* pacc) {
194
195 struct EnuCoor_d enu_accel = {
196 PyFloat_AsDouble(PyTuple_GetItem(pacc, 0)),
197 PyFloat_AsDouble(PyTuple_GetItem(pacc, 1)),
198 PyFloat_AsDouble(PyTuple_GetItem(pacc, 2)),
199 };
200
202 VECT3_NED_OF_ENU(fdm.ltpprz_ecef_accel, enu_accel);
204 VECT3_COPY(fdm.ltp_ecef_accel, fdm.ltpprz_ecef_accel);
205
207 ecef_of_enu_vect_d(&fdm.ecef_ecef_accel, &ltpdef, &enu_accel);
208
210 struct DoubleVect3 tmp = {
211 fdm.ltp_ecef_accel.x,
212 fdm.ltp_ecef_accel.y,
213 fdm.ltp_ecef_accel.z - 9.81,
214 };
215 double_quat_vmult(&fdm.body_accel, &fdm.ltp_to_body_quat, &tmp);
216
217 // /** acceleration in body frame, wrt ECEF frame */
218 // fdm.body_ecef_accel;
219 // /** acceleration in body frame, wrt ECI inertial frame */
220 // fdm.body_inertial_accel;
221
222}
223
224static void get_orient(PyObject* pquat) {
225
226 // convert ENU to NED world frames for quaternion
227 struct DoubleQuat enu_quat = {
228 PyFloat_AsDouble(PyTuple_GetItem(pquat, 3)),
229 PyFloat_AsDouble(PyTuple_GetItem(pquat, 1)),
230 PyFloat_AsDouble(PyTuple_GetItem(pquat, 0)),
231 -PyFloat_AsDouble(PyTuple_GetItem(pquat, 2)),
232 };
233
234 // extra rotation to 90° yaw rotation (due to ENU/NED body frames x axis)
235 double_quat_comp(&fdm.ltpprz_to_body_quat, &enu_quat, &quat_to_pprz);
236 double_eulers_of_quat(&fdm.ltpprz_to_body_eulers, &fdm.ltpprz_to_body_quat);
237 QUAT_COPY(fdm.ltp_to_body_quat, fdm.ltpprz_to_body_quat);
238 EULERS_COPY(fdm.ltp_to_body_eulers, fdm.ltpprz_to_body_eulers);
239}
240
241static void get_ang_vel(PyObject* pang_vel) {
242
243 // convert ENU to NED body frames (with x front)
244 struct DoubleVect3 body_rates = {
245 .x = PyFloat_AsDouble(PyTuple_GetItem(pang_vel, 0)),
246 .y = -PyFloat_AsDouble(PyTuple_GetItem(pang_vel, 1)),
247 .z = -PyFloat_AsDouble(PyTuple_GetItem(pang_vel, 2)),
248 };
249
250 fdm.body_inertial_rotvel.p = body_rates.x;
251 fdm.body_inertial_rotvel.q = body_rates.y;
252 fdm.body_inertial_rotvel.r = body_rates.z;
253 fdm.body_ecef_rotvel = fdm.body_inertial_rotvel;
254}
255
256static void get_ang_acc(PyObject* pang_acc) {
257
258 // convert ENU to NED body frames (with x front)
259 struct DoubleVect3 body_rotaccel = {
260 .x = PyFloat_AsDouble(PyTuple_GetItem(pang_acc, 0)),
261 .y = -PyFloat_AsDouble(PyTuple_GetItem(pang_acc, 1)),
262 .z = -PyFloat_AsDouble(PyTuple_GetItem(pang_acc, 2)),
263 };
264
265 fdm.body_inertial_rotaccel.p = body_rotaccel.x;
266 fdm.body_inertial_rotaccel.q = body_rotaccel.y;
267 fdm.body_inertial_rotaccel.r = body_rotaccel.z;
268 fdm.body_ecef_rotaccel = fdm.body_inertial_rotaccel;
269}
270
271
272
273/**************************
274 ** Generating LTP plane **
275 **************************/
276
277static void init_ltp(void)
278{
279
280 struct LlaCoor_d llh_nav0; /* Height above the ellipsoid */
281 llh_nav0.lat = RadOfDeg((double)NAV_LAT0 / 1e7);
282 llh_nav0.lon = RadOfDeg((double)NAV_LON0 / 1e7);
283 llh_nav0.alt = NAV_ALT0 / 1000.0;
284
285 struct EcefCoor_d ecef_nav0;
286
287 ecef_of_lla_d(&ecef_nav0, &llh_nav0);
288
289 ltp_def_from_ecef_d(&ltpdef, &ecef_nav0);
290 fdm.ecef_pos = ecef_nav0;
291
292// TODO vérifier tout ça
293 fdm.ltp_g.x = 0.;
294 fdm.ltp_g.y = 0.;
295 fdm.ltp_g.z = 0.; // accel data are already with the correct format
296
297#ifdef AHRS_H_X
298 fdm.ltp_h.x = AHRS_H_X;
299 fdm.ltp_h.y = AHRS_H_Y;
300 fdm.ltp_h.z = AHRS_H_Z;
301 PRINT_CONFIG_MSG("Using magnetic field as defined in airframe file.")
302#else
303 fdm.ltp_h.x = 1.;
304 fdm.ltp_h.y = 0.;
305 fdm.ltp_h.z = 0.;
306#endif
307
308}
309
310
311/*****************************************************/
312// Atmosphere function (we don't need that features) //
313void nps_fdm_set_wind(double speed __attribute__((unused)),
314 double dir __attribute__((unused)))
315{
316}
317
318void nps_fdm_set_wind_ned(double wind_north __attribute__((unused)),
319 double wind_east __attribute__((unused)),
320 double wind_down __attribute__((unused)))
321{
322}
323
324void nps_fdm_set_turbulence(double wind_speed __attribute__((unused)),
325 int turbulence_severity __attribute__((unused)))
326{
327}
328
329void nps_fdm_set_temperature(double temp __attribute__((unused)),
330 double h __attribute__((unused)))
331{
332}
333
334static void python_init(double dt) {
335 PyStatus status;
336 PyConfig config;
337 PyConfig_InitPythonConfig(&config);
338
339 // needed for the prints
340 config.configure_c_stdio = 1;
341 config.buffered_stdio = 0;
342
343 status = PyConfig_SetString(&config, &config.program_name, WIDEN(NPS_PYTHON_EXEC));
344 py_check_status(&config, &status);
345 status = Py_InitializeFromConfig(&config);
346 py_check_status(&config, &status);
347
348 // add path to sys.path
349 PyObject *sys_path = PySys_GetObject("path");
350 PyObject *module_path = PyUnicode_FromString( PAPARAZZI_SRC "/sw/simulator/nps/pybullet");
351 PyList_Append(sys_path, module_path);
352 Py_DECREF(module_path);
353
354 fdm_module = PyImport_ImportModule(NPS_PYBULLET_MODULE);
355 py_check(true, __LINE__);
356
357 PyObject* bullet_fdm_class = PyObject_GetAttrString(fdm_module, "BulletFDM");
358 py_check(true, __LINE__);
359
360 PyObject* fdm_ctor_args = Py_BuildValue("(d)", dt);
361 PyObject* fdm_ctor_kwargs = Py_BuildValue("{siss}", "GUI", NPS_PYBULLET_GUI, "urdf", NPS_PYBULLET_URDF);
362 bullet_fdm = PyObject_Call(bullet_fdm_class, fdm_ctor_args, fdm_ctor_kwargs);
363
364 py_check(true, __LINE__);
365 Py_DECREF(fdm_ctor_args);
366 Py_DECREF(bullet_fdm_class);
367}
368
369static void py_check_status(PyConfig* config, PyStatus* status) {
370 if (PyStatus_Exception(*status)) {
371 printf("error\n");
372 PyConfig_Clear(config);
373 Py_ExitStatusException(*status);
374 exit(1);
375 }
376}
377
378
379static void py_check(bool exit_on_error, int line_nb) {
380 if(PyErr_Occurred()) {
381 printf("Error at line %d\n", line_nb);
382 PyErr_Print();
383 if(exit_on_error) {
384 exit(1);
385 }
386 }
387}
static void h(const real32_T x[7], const real32_T q[4], real32_T y[6])
static uint8_t status
void double_eulers_of_quat(struct DoubleEulers *e, struct DoubleQuat *q)
void double_quat_vmult(struct DoubleVect3 *v_out, struct DoubleQuat *q, struct DoubleVect3 *v_in)
void double_quat_comp(struct DoubleQuat *a2c, struct DoubleQuat *a2b, struct DoubleQuat *b2c)
Composition (multiplication) of two quaternions.
Roation quaternion.
#define EULERS_COPY(_a, _b)
#define QUAT_COPY(_qo, _qi)
#define VECT3_COPY(_a, _b)
double z
in meters
double y
in meters
double x
in meters
double lat
in radians
double alt
in meters above WGS84 reference ellipsoid
double lon
in radians
void ltp_def_from_ecef_d(struct LtpDef_d *def, struct EcefCoor_d *ecef)
void ecef_of_enu_vect_d(struct EcefCoor_d *ecef, struct LtpDef_d *def, struct EnuCoor_d *enu)
void lla_of_ecef_d(struct LlaCoor_d *lla, struct EcefCoor_d *ecef)
void ecef_of_lla_d(struct EcefCoor_d *ecef, struct LlaCoor_d *lla)
void ecef_of_enu_point_d(struct EcefCoor_d *ecef, struct LtpDef_d *def, struct EnuCoor_d *enu)
vector in EarthCenteredEarthFixed coordinates
vector in East North Up coordinates Units: meters
vector in Latitude, Longitude and Altitude
definition of the local (flat earth) coordinate system
#define VECT3_NED_OF_ENU(_o, _i)
#define PPRZ_ISA_SEA_LEVEL_PRESSURE
ISA sea level standard atmospheric pressure in Pascal.
Definition pprz_isa.h:50
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
struct NpsFdm fdm
Holds all necessary NPS FDM state information.
static struct LtpDef_d ltpdef
void nps_fdm_set_wind_ned(double wind_north, double wind_east, double wind_down)
static void python_init(double dt)
void nps_fdm_init(double dt)
NPS FDM rover init.
static void init_ltp(void)
struct DoubleQuat quat_to_pprz
void nps_fdm_run_step(bool launch, double *commands, int commands_nb)
Minimum complexity flight dynamic model In legacy Paparazzi simulator, was implemented in OCaml and c...
PyObject * bullet_fdm
#define NPS_PYBULLET_GUI
PyObject * fdm_module
#define NPS_PYBULLET_URDF
void nps_fdm_set_temperature(double temp, double h)
Set temperature in degrees Celcius at given height h above MSL.
#define WIDEN(x)
static void get_vel(PyObject *pvel)
static void get_ang_acc(PyObject *pang_acc)
#define NPS_PYTHON_EXEC
static void get_orient(PyObject *porient)
static void get_ang_vel(PyObject *pang_vel)
void nps_fdm_set_turbulence(double wind_speed, int turbulence_severity)
static void py_check_status(PyConfig *config, PyStatus *status)
#define NPS_PYBULLET_MODULE
static void get_pos(PyObject *ppos)
void nps_fdm_set_wind(double speed, double dir)
void nps_fdm_run_init_step(void)
static void get_acc(PyObject *pacc)
static void py_check(bool exit_on_error, int line_nb)
Paparazzi generic algebra macros.
Paparazzi floating point algebra.
Paparazzi generic macros for geodetic calculations.
Paparazzi double-precision floating point math for geodetic calculations.
Paparazzi floating point math for geodetic calculations.
Paparazzi atmospheric pressure conversion utilities.
static const ShellCommand commands[]
Definition shell_arch.c:77
static const float dir[]
API to get/set the generic vehicle states.
#define TRUE
Definition std.h:4