Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-30 03:27:33 +08:00
Code Issues Actions 27 Packages Projects Releases Wiki Activity

GVF Parametric (#2559)

Browse Source 
Co-authored-by: Hector Garcia de Marina <hgdemarina@gmail.com>
This commit is contained in:
Hector Garcia de Marina
2020-09-18 20:52:35 +02:00
committed by GitHub
parent 99c8d473aa
commit f38be8110c
17 changed files with 1870 additions and 72 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/flight_plans/demo_gvf.xml
+14
View File
@@ -29,10 +29,12 @@
<variables> <variables>
<variable var="angle_ps" init="0" min="-180" max="179" step="1"/> <variable var="angle_ps" init="0" min="-180" max="179" step="1"/>
<variable init="0" max="100" min="0" step="5" var="ell_delta"/>
</variables> </variables>
<modules> <modules>
<module name="gvf_module"/> <module name="gvf_module"/>
<module name="gvf_parametric"/>
</modules> </modules>
<blocks> <blocks>
@@ -78,6 +80,18 @@
<call_once fun="gvf_nav_survey_polygon_setup(WP_S1, 4, angle_ps, 30, 30, 40, flight_altitude)"/> <call_once fun="gvf_nav_survey_polygon_setup(WP_S1, 4, angle_ps, 30, 30, 40, flight_altitude)"/>
<call fun="gvf_nav_survey_polygon_run()"/> <call fun="gvf_nav_survey_polygon_run()"/>
</block> </block>
<block name="3D_ellipse">
<call fun="gvf_parametric_3D_ellipse_wp_delta(WP_ELLIPSE,gvf_parametric_3d_ellipse_par.r,flight_altitude-ground_alt,ell_delta,gvf_parametric_3d_ellipse_par.alpha)"/>
</block>
<block name="3D_lissajous">
<call fun="gvf_parametric_3D_lissajous_wp_center(WP_ELLIPSE,flight_altitude-ground_alt,gvf_parametric_3d_lissajous_par.cx, gvf_parametric_3d_lissajous_par.cy, gvf_parametric_3d_lissajous_par.cz, gvf_parametric_3d_lissajous_par.wx, gvf_parametric_3d_lissajous_par.wy, gvf_parametric_3d_lissajous_par.wz, gvf_parametric_3d_lissajous_par.dx, gvf_parametric_3d_lissajous_par.dy, gvf_parametric_3d_lissajous_par.dz, gvf_parametric_3d_lissajous_par.alpha)"/>
</block>
<block name="2D_trefoil">
<call_once fun="NavVerticalAutoThrottleMode(0.0)"/>
<call_once fun="NavVerticalAltitudeMode(flight_altitude, 0.0)"/>
<call fun="gvf_parametric_2D_trefoil_wp(WP_ELLIPSE,gvf_parametric_2d_trefoil_par.w1,gvf_parametric_2d_trefoil_par.w2,gvf_parametric_2d_trefoil_par.ratio,gvf_parametric_2d_trefoil_par.r, gvf_parametric_2d_trefoil_par.alpha)"/>
</block>
<block group="land" name="Land Right AF-TD" strip_button="Land right (wp AF-TD)" strip_icon="land-right.png"> <block group="land" name="Land Right AF-TD" strip_button="Land right (wp AF-TD)" strip_icon="land-right.png">
<set value="DEFAULT_CIRCLE_RADIUS" var="nav_radius"/> <set value="DEFAULT_CIRCLE_RADIUS" var="nav_radius"/>
<deroute block="land"/> <deroute block="land"/>
conf/modules/gvf_parametric.xml
+87
View File
@@ -0,0 +1,87 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="gvf_parametric" dir="guidance/gvf_parametric">
<doc>
<description>Guidance algorithm for tracking 2D and 3D smooth trajectories that are defined by a parametric equation.
For more details we refer to https://wiki.paparazziuav.org/wiki/Module/guidance_vector_field .
This algorithm needs the Eigen library, be sure you have it installed, e.g., git submodule update --init --recursive sw/ext/eigen
</description>
</doc>
<settings name="GVF_PARAMETRIC">
<dl_settings>
<dl_settings NAME="GVF_PARAMETRIC">
<dl_settings NAME="Control">
<dl_setting MAX="1" MIN="-1" STEP="2" VAR="gvf_parametric_control.s" shortname = "direction"/>
<dl_setting MAX="1.25" MIN="0.75" STEP="0.05" VAR="gvf_parametric_control.k_roll" shortname="k_roll" param="GVF_PARAMETRIC_CONTROL_KROLL"/>
<dl_setting MAX="1.25" MIN="0.75" STEP="0.05" VAR="gvf_parametric_control.k_climb" shortname="k_climb" param="GVF_PARAMETRIC_CONTROL_KCLIMB"/>
<dl_setting MAX="10" MIN="0.01" STEP="0.05" VAR="gvf_parametric_control.L" shortname="L" param="GVF_PARAMETRIC_CONTROL_L"/>
<dl_setting MAX="1" MIN="0.001" STEP="0.005" VAR="gvf_parametric_control.beta" shortname="beta" param="GVF_PARAMETRIC_CONTROL_BETA"/>
<dl_setting MAX="1.5" MIN="0.5" STEP="0.01" VAR="gvf_parametric_control.k_psi" shortname="k_psi" param="GVF_PARAMETRIC_CONTROL_KPSI"/>
</dl_settings>
<dl_settings NAME="2D_trefoil">
<dl_setting MAX="0.01" MIN="0.0" STEP="0.0005" VAR="gvf_parametric_2d_trefoil_par.kx" shortname="2d_tre_kx" param="GVF_PARAMETRIC_2D_TREFOIL_KX"/>
<dl_setting MAX="0.01" MIN="0.0" STEP="0.0005" VAR="gvf_parametric_2d_trefoil_par.ky" shortname="2d_tre_ky" param="GVF_PARAMETRIC_2D_TREFOIL_KY"/>
<dl_setting MAX="1" MIN="0.0" STEP="0.01" VAR="gvf_parametric_2d_trefoil_par.w1" shortname="2d_tre_w1" param="GVF_PARAMETRIC_2D_TREFOIL_W1"/>
<dl_setting MAX="1" MIN="0.0" STEP="0.01" VAR="gvf_parametric_2d_trefoil_par.w2" shortname="2d_tre_w2" param="GVF_PARAMETRIC_2D_TREFOIL_W2"/>
<dl_setting MAX="200" MIN="10" STEP="5" VAR="gvf_parametric_2d_trefoil_par.ratio" shortname="2d_tre_ratio" param="GVF_PARAMETRIC_2D_TREFOIL_RATIO"/>
<dl_setting MAX="200" MIN="10" STEP="5" VAR="gvf_parametric_2d_trefoil_par.r" shortname="2d_tre_r" param="GVF_PARAMETRIC_2D_TREFOIL_R"/>
<dl_setting MAX="180" MIN="-180" STEP="1" VAR="gvf_parametric_2d_trefoil_par.alpha" shortname="2d_tre_alpha" param="GVF_PARAMETRIC_2D_TREFOIL_ALPHA"/>
</dl_settings>
<dl_settings NAME="3D_ellipse">
<dl_setting MAX="0.01" MIN="0.0" STEP="0.0005" VAR="gvf_parametric_3d_ellipse_par.kx" shortname="3d_ell_kx" param="GVF_PARAMETRIC_3D_ELLIPSE_KX"/>
<dl_setting MAX="0.01" MIN="0.0" STEP="0.0005" VAR="gvf_parametric_3d_ellipse_par.ky" shortname="3d_ell_ky" param="GVF_PARAMETRIC_3D_ELLIPSE_KY"/>
<dl_setting MAX="0.1" MIN="0.0" STEP="0.0005" VAR="gvf_parametric_3d_ellipse_par.kz" shortname="3d_ell_kz" param="GVF_PARAMETRIC_3D_ELLIPSE_KZ"/>
<dl_setting MAX="150" MIN="0.0" STEP="5" VAR="gvf_parametric_3d_ellipse_par.r" shortname="3d_ell_r" param="GVF_PARAMETRIC_3D_ELLIPSE_R"/>
<dl_setting MAX="150" MIN="10" STEP="5" VAR="gvf_parametric_3d_ellipse_par.zl" shortname="3d_ell_zl" param="GVF_PARAMETRIC_3D_ELLIPSE_ZL"/>
<dl_setting MAX="150" MIN="10" STEP="5" VAR="gvf_parametric_3d_ellipse_par.zh" shortname="3d_ell_zh" param="GVF_PARAMETRIC_3D_ELLIPSE_ZH"/>
<dl_setting MAX="180" MIN="-180" STEP="1" VAR="gvf_parametric_3d_ellipse_par.alpha" shortname="3d_ell_alpha" param="GVF_PARAMETRIC_3D_ELLIPSE_ALPHA"/>
</dl_settings>
<dl_settings NAME="3D_lissajous">
<dl_setting MAX="0.01" MIN="0.0" STEP="0.0005" VAR="gvf_parametric_3d_lissajous_par.kx" shortname="3d_lis_kx" param="GVF_PARAMETRIC_3D_LISSAJOUS_KX"/>
<dl_setting MAX="0.01" MIN="0.0" STEP="0.0005" VAR="gvf_parametric_3d_lissajous_par.ky" shortname="3d_lis_ky" param="GVF_PARAMETRIC_3D_LISSAJOUS_KY"/>
<dl_setting MAX="0.1" MIN="0.0" STEP="0.0005" VAR="gvf_parametric_3d_lissajous_par.kz" shortname="3d_lis_kz" param="GVF_PARAMETRIC_3D_LISSAJOUS_KZ"/>
<dl_setting MAX="250" MIN="-250" STEP="5" VAR="gvf_parametric_3d_lissajous_par.cx" shortname="3d_lis_cx" param="GVF_PARAMETRIC_3D_LISSAJOUS_CX"/>
<dl_setting MAX="250" MIN="-250" STEP="5" VAR="gvf_parametric_3d_lissajous_par.cy" shortname="3d_lis_cy" param="GVF_PARAMETRIC_3D_LISSAJOUS_CY"/>
<dl_setting MAX="50" MIN="-50" STEP="5" VAR="gvf_parametric_3d_lissajous_par.cz" shortname="3d_lis_cz" param="GVF_PARAMETRIC_3D_LISSAJOUS_CZ"/>
<dl_setting MAX="10" MIN="-10.0" STEP="0.5" VAR="gvf_parametric_3d_lissajous_par.wx" shortname="3d_lis_wx" param="GVF_PARAMETRIC_3D_LISSAJOUS_WX"/>
<dl_setting MAX="10" MIN="-10" STEP="0.5" VAR="gvf_parametric_3d_lissajous_par.wy" shortname="3d_lis_wy" param="GVF_PARAMETRIC_3D_LISSAJOUS_WY"/>
<dl_setting MAX="10" MIN="-10" STEP="0.5" VAR="gvf_parametric_3d_lissajous_par.wz" shortname="3d_lis_wz" param="GVF_PARAMETRIC_3D_LISSAJOUS_WZ"/>
<dl_setting MAX="180" MIN="-180.0" STEP="1" VAR="gvf_parametric_3d_lissajous_par.dx" shortname="3d_lis_dx" param="GVF_PARAMETRIC_3D_LISSAJOUS_DX"/>
<dl_setting MAX="180" MIN="-180" STEP="1" VAR="gvf_parametric_3d_lissajous_par.dy" shortname="3d_lis_dy" param="GVF_PARAMETRIC_3D_LISSAJOUS_DY"/>
<dl_setting MAX="180" MIN="-180" STEP="1" VAR="gvf_parametric_3d_lissajous_par.dz" shortname="3d_lis_dz" param="GVF_PARAMETRIC_3D_LISSAJOUS_DZ"/>
<dl_setting MAX="180" MIN="-180" STEP="1" VAR="gvf_parametric_3d_lissajous_par.alpha" shortname="3d_lis_alpha" param="GVF_PARAMETRIC_3D_LISSAJOUS_ALPHA"/>
</dl_settings>
</dl_settings>
</dl_settings>
</settings>
<header>
<file name="gvf_parametric.h"/>
<file name="gvf_parametric_low_level_control.h"/>
<file name="trajectories/gvf_parametric_3d_ellipse.h"/>
<file name="trajectories/gvf_parametric_3d_lissajous.h"/>
<file name="trajectories/gvf_parametric_2d_trefoil.h"/>
</header>
<init fun = "gvf_parametric_init()"/>
<makefile target="ap|nps" firmware="fixedwing">
<configure name="CXXSTANDARD" value="-std=c++14"/>
<flag name="CXXFLAGS" value="Wno-int-in-bool-context -Wno-deprecated-copy"/>
<include name="$(PAPARAZZI_SRC)/sw/ext/eigen"/>
<define name="EIGEN_NO_MALLOC"/>
<define name="EIGEN_NO_AUTOMATIC_RESIZING"/>
<file name="gvf_parametric.cpp"/>
<file name="gvf_parametric_low_level_control.c"/>
<file name="trajectories/gvf_parametric_3d_ellipse.c"/>
<file name="trajectories/gvf_parametric_3d_lissajous.c"/>
<file name="trajectories/gvf_parametric_2d_trefoil.c"/>
<flag name="LDFLAGS" value="lstdc++" />
</makefile>
<makefile target="ap" firmware="fixedwing">
<define name="EIGEN_NO_DEBUG"/>
</makefile>
</module>
conf/telemetry/default_fixedwing_gvf.xml
+1
View File
@@ -32,6 +32,7 @@
<message name="AIR_DATA" period="1.3"/> <message name="AIR_DATA" period="1.3"/>
<message name="VECTORNAV_INFO" period="0.5"/> <message name="VECTORNAV_INFO" period="0.5"/>
<message name="GVF" period="1"/> <message name="GVF" period="1"/>
<message name="GVF_PARAMETRIC" period="1"/>
</mode> </mode>
<mode name="minimal"> <mode name="minimal">
<message name="ALIVE" period="5"/> <message name="ALIVE" period="5"/>
sw/airborne/modules/guidance/gvf/gvf.c
+30 -13
View File
@@ -40,6 +40,9 @@ gvf_con gvf_control;
gvf_tra gvf_trajectory; gvf_tra gvf_trajectory;
gvf_seg gvf_segment; gvf_seg gvf_segment;
// Time variables to check if GVF is active
uint32_t gvf_t0 = 0;
#if PERIODIC_TELEMETRY #if PERIODIC_TELEMETRY
#include "subsystems/datalink/telemetry.h" #include "subsystems/datalink/telemetry.h"
static void send_gvf(struct transport_tx *trans, struct link_device *dev) static void send_gvf(struct transport_tx *trans, struct link_device *dev)
@@ -65,27 +68,39 @@ static void send_gvf(struct transport_tx *trans, struct link_device *dev)
uint8_t traj_type = (uint8_t)gvf_trajectory.type; uint8_t traj_type = (uint8_t)gvf_trajectory.type;
pprz_msg_send_GVF(trans, dev, AC_ID, &gvf_control.error, &traj_type, uint32_t now = get_sys_time_msec();
&gvf_control.s, &gvf_control.ke, plen, gvf_trajectory.p); uint32_t delta_T = now - gvf_t0;
if (delta_T < 200)
pprz_msg_send_GVF(trans, dev, AC_ID, &gvf_control.error, &traj_type,
&gvf_control.s, &gvf_control.ke, plen, gvf_trajectory.p);
} }
static void send_circle(struct transport_tx *trans, struct link_device *dev) static void send_circle(struct transport_tx *trans, struct link_device *dev)
{ {
if (gvf_trajectory.type == ELLIPSE && uint32_t now = get_sys_time_msec();
(gvf_trajectory.p[2] == gvf_trajectory.p[3])) { uint32_t delta_T = now - gvf_t0;
pprz_msg_send_CIRCLE(trans, dev, AC_ID,
&gvf_trajectory.p[0], &gvf_trajectory.p[1], if (delta_T < 200)
&gvf_trajectory.p[2]); if (gvf_trajectory.type == ELLIPSE &&
} (gvf_trajectory.p[2] == gvf_trajectory.p[3])) {
pprz_msg_send_CIRCLE(trans, dev, AC_ID,
&gvf_trajectory.p[0], &gvf_trajectory.p[1],
&gvf_trajectory.p[2]);
}
} }
static void send_segment(struct transport_tx *trans, struct link_device *dev) static void send_segment(struct transport_tx *trans, struct link_device *dev)
{ {
if (gvf_trajectory.type == LINE && gvf_segment.seg == 1) { uint32_t now = get_sys_time_msec();
pprz_msg_send_SEGMENT(trans, dev, AC_ID, uint32_t delta_T = now - gvf_t0;
&gvf_segment.x1, &gvf_segment.y1,
&gvf_segment.x2, &gvf_segment.y2); if (delta_T < 200)
} if (gvf_trajectory.type == LINE && gvf_segment.seg == 1) {
pprz_msg_send_SEGMENT(trans, dev, AC_ID,
&gvf_segment.x1, &gvf_segment.y1,
&gvf_segment.x2, &gvf_segment.y2);
}
} }
#endif #endif
@@ -139,6 +154,8 @@ void gvf_init(void)
void gvf_control_2D(float ke, float kn, float e, void gvf_control_2D(float ke, float kn, float e,
struct gvf_grad *grad, struct gvf_Hess *hess) struct gvf_grad *grad, struct gvf_Hess *hess)
{ {
gvf_t0 = get_sys_time_msec();
struct FloatEulers *att = stateGetNedToBodyEulers_f(); struct FloatEulers *att = stateGetNedToBodyEulers_f();
float ground_speed = stateGetHorizontalSpeedNorm_f(); float ground_speed = stateGetHorizontalSpeedNorm_f();
float course = stateGetHorizontalSpeedDir_f(); float course = stateGetHorizontalSpeedDir_f();
sw/airborne/modules/guidance/gvf/gvf.h
+2 -2
View File
@@ -32,8 +32,8 @@
#include "std.h" #include "std.h"
/** @typedef gvf_conf /** @typedef gvf_con
* @brief Parameters for the GVF * @brief Control parameters for the GVF
* @param ke Gain defining how agressive is the vector field * @param ke Gain defining how agressive is the vector field
* @param kn Gain for making converge the vehile to the vector field * @param kn Gain for making converge the vehile to the vector field
* @param error Error signal. It does not have any specific units. It depends on how the trajectory has been implemented. Check the specific wiki entry for each trajectory. * @param error Error signal. It does not have any specific units. It depends on how the trajectory has been implemented. Check the specific wiki entry for each trajectory.
sw/airborne/modules/guidance/gvf_parametric/gvf_parametric.cpp
+440
View File
@@ -0,0 +1,440 @@
/*
* Copyright (C) 2020 Hector Garcia de Marina <hgarciad@ucm.es>
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file modules/guidance/gvf_parametric/gvf_parametric.cpp
*
* Guiding vector field algorithm for 2D and 3D complex trajectories.
*/
#include <iostream>
#include <Eigen/Dense>
#include "gvf_parametric.h"
#include "gvf_parametric_low_level_control.h"
#include "./trajectories/gvf_parametric_3d_ellipse.h"
#include "./trajectories/gvf_parametric_3d_lissajous.h"
#include "./trajectories/gvf_parametric_2d_trefoil.h"
#ifdef __cplusplus
extern "C" {
#endif
#include "autopilot.h"
// Control
uint32_t gvf_parametric_t0 = 0; // We need it for calculting the time lapse delta_T
gvf_parametric_con gvf_parametric_control;
// Trajectory
gvf_parametric_tra gvf_parametric_trajectory;
#if PERIODIC_TELEMETRY
#include "subsystems/datalink/telemetry.h"
static void send_gvf_parametric(struct transport_tx *trans, struct link_device *dev)
{
// Do not know whether is a good idea to do this check here or to include
// this plen in gvf_trajectory
int plen;
int elen;
switch (gvf_parametric_trajectory.type) {
case TREFOIL_2D:
plen = 7;
elen = 2;
break;
case ELLIPSE_3D:
plen = 6;
elen = 3;
break;
case LISSAJOUS_3D:
plen = 13;
elen = 3;
break;
default:
plen = 1;
elen = 3;
}
uint8_t traj_type = (uint8_t)gvf_parametric_trajectory.type;
uint32_t now = get_sys_time_msec();
uint32_t delta_T = now - gvf_parametric_t0;
float wb = gvf_parametric_control.w * gvf_parametric_control.beta;
if (delta_T < 200) {
pprz_msg_send_GVF_PARAMETRIC(trans, dev, AC_ID, &traj_type, &gvf_parametric_control.s, &wb, plen,
gvf_parametric_trajectory.p_parametric, elen, gvf_parametric_trajectory.phi_errors);
}
}
static void send_circle_parametric(struct transport_tx *trans, struct link_device *dev)
{
uint32_t now = get_sys_time_msec();
uint32_t delta_T = now - gvf_parametric_t0;
if (delta_T < 200)
if (gvf_parametric_trajectory.type == ELLIPSE_3D) {
pprz_msg_send_CIRCLE(trans, dev, AC_ID, &gvf_parametric_trajectory.p_parametric[0],
&gvf_parametric_trajectory.p_parametric[1], &gvf_parametric_trajectory.p_parametric[2]);
}
}
#endif // PERIODIC TELEMETRY
#ifdef __cplusplus
}
#endif
void gvf_parametric_init(void)
{
gvf_parametric_control.w = 0;
gvf_parametric_control.delta_T = 0;
gvf_parametric_control.s = 1;
gvf_parametric_control.k_roll = GVF_PARAMETRIC_CONTROL_KROLL;
gvf_parametric_control.k_climb = GVF_PARAMETRIC_CONTROL_KCLIMB;
gvf_parametric_control.k_psi = GVF_PARAMETRIC_CONTROL_KPSI;
gvf_parametric_control.L = GVF_PARAMETRIC_CONTROL_L;
gvf_parametric_control.beta = GVF_PARAMETRIC_CONTROL_BETA;
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_GVF_PARAMETRIC, send_gvf_parametric);
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_CIRCLE, send_circle_parametric);
#endif
}
void gvf_parametric_set_direction(int8_t s)
{
gvf_parametric_control.s = s;
}
void gvf_parametric_control_2D(float kx, float ky, float f1, float f2, float f1d, float f2d, float f1dd, float f2dd)
{
uint32_t now = get_sys_time_msec();
gvf_parametric_control.delta_T = now - gvf_parametric_t0;
gvf_parametric_t0 = now;
if (gvf_parametric_control.delta_T > 300) { // We need at least two iterations for Delta_T
gvf_parametric_control.w = 0; // Reset w since we assume the algorithm starts
return;
}
float L = gvf_parametric_control.L;
float beta = gvf_parametric_control.beta * gvf_parametric_control.s;
Eigen::Vector3f X;
Eigen::Matrix3f J;
// Error signals phi_x and phi_y
struct EnuCoor_f *pos_enu = stateGetPositionEnu_f();
float x = pos_enu->x;
float y = pos_enu->y;
float phi1 = L * (x - f1);
float phi2 = L * (y - f2);
gvf_parametric_trajectory.phi_errors[0] = phi1; // Error signals for the telemetry
gvf_parametric_trajectory.phi_errors[1] = phi2;
// Chi
X(0) = L * beta * f1d - kx * phi1;
X(1) = L * beta * f2d - ky * phi2;
X(2) = L + beta * (kx * phi1 * f1d + ky * phi2 * f2d);
X *= L;
// Jacobian
J.setZero();
J(0, 0) = -kx * L;
J(1, 1) = -ky * L;
J(2, 0) = (beta * L) * (beta * f1dd + kx * f1d);
J(2, 1) = (beta * L) * (beta * f2dd + ky * f2d);
J(2, 2) = beta * beta * (kx * (phi1 * f1dd - L * f1d * f1d) + ky * (phi2 * f2dd - L * f2d * f2d));
J *= L;
// Guidance algorithm
float ground_speed = stateGetHorizontalSpeedNorm_f();
float w_dot = (ground_speed * X(2)) / sqrtf(X(0) * X(0) + X(1) * X(1));
Eigen::Vector3f xi_dot;
struct EnuCoor_f *vel_enu = stateGetSpeedEnu_f();
float course = stateGetHorizontalSpeedDir_f();
xi_dot << vel_enu->x, vel_enu->y, w_dot;
Eigen::Matrix3f G;
Eigen::Matrix3f Gp;
Eigen::Matrix<float, 2, 3> Fp;
Eigen::Vector2f h;
Eigen::Matrix<float, 1, 2> ht;
G << 1, 0, 0,
0, 1, 0,
0, 0, 0;
Fp << 0, -1, 0,
1, 0, 0;
Gp << 0, -1, 0,
1, 0, 0,
0, 0, 0;
h << sinf(course), cosf(course);
ht = h.transpose();
Eigen::Matrix<float, 1, 3> Xt = X.transpose();
Eigen::Vector3f Xh = X / X.norm();
Eigen::Matrix<float, 1, 3> Xht = Xh.transpose();
Eigen::Matrix3f I;
I.setIdentity();
float aux = ht * Fp * X;
float heading_rate = -1 / (Xt * G * X) * Xt * Gp * (I - Xh * Xht) * J * xi_dot - (gvf_parametric_control.k_psi * aux /
sqrtf(Xt * G * X));
// Virtual coordinate update, even if the vehicle is not in autonomous mode, the parameter w will get "closer" to
// the vehicle. So it is not only okei but advisable to update it.
gvf_parametric_control.w += w_dot * gvf_parametric_control.delta_T * 1e-3;
gvf_parametric_low_level_control_2D(heading_rate);
}
void gvf_parametric_control_3D(float kx, float ky, float kz, float f1, float f2, float f3, float f1d, float f2d,
float f3d, float f1dd, float f2dd, float f3dd)
{
uint32_t now = get_sys_time_msec();
gvf_parametric_control.delta_T = now - gvf_parametric_t0;
gvf_parametric_t0 = now;
if (gvf_parametric_control.delta_T > 300) { // We need at least two iterations for Delta_T
gvf_parametric_control.w = 0; // Reset w since we assume the algorithm starts
return;
}
float L = gvf_parametric_control.L;
float beta = gvf_parametric_control.beta * gvf_parametric_control.s;
Eigen::Vector4f X;
Eigen::Matrix4f J;
// Error signals phi_x phi_y and phi_z
struct EnuCoor_f *pos_enu = stateGetPositionEnu_f();
float x = pos_enu->x;
float y = pos_enu->y;
float z = pos_enu->z;
float phi1 = L * (x - f1);
float phi2 = L * (y - f2);
float phi3 = L * (z - f3);
gvf_parametric_trajectory.phi_errors[0] = phi1 / L; // Error signals in meters for the telemetry
gvf_parametric_trajectory.phi_errors[1] = phi2 / L;
gvf_parametric_trajectory.phi_errors[2] = phi3 / L;
// Chi
X(0) = -f1d * L * L * beta - kx * phi1;
X(1) = -f2d * L * L * beta - ky * phi2;
X(2) = -f3d * L * L * beta - kz * phi3;
X(3) = -L * L + beta * (kx * phi1 * f1d + ky * phi2 * f2d + kz * phi3 * f3d);
X *= L;
// Jacobian
J.setZero();
J(0, 0) = -kx * L;
J(1, 1) = -ky * L;
J(2, 2) = -kz * L;
J(3, 0) = kx * f1d * beta * L;
J(3, 1) = ky * f2d * beta * L;
J(3, 2) = kz * f3d * beta * L;
J(0, 3) = -(beta * L) * (beta * L * f1dd - kx * f1d);
J(1, 3) = -(beta * L) * (beta * L * f2dd - ky * f2d);
J(2, 3) = -(beta * L) * (beta * L * f3dd - kz * f3d);
J(3, 3) = beta * beta * (kx * (phi1 * f1dd - L * f1d * f1d) + ky * (phi2 * f2dd - L * f2d * f2d)
+ kz * (phi3 * f3dd - L * f3d * f3d));
J *= L;
// Guidance algorithm
float ground_speed = stateGetHorizontalSpeedNorm_f();
float w_dot = (ground_speed * X(3)) / sqrtf(X(0) * X(0) + X(1) * X(1));
Eigen::Vector4f xi_dot;
struct EnuCoor_f *vel_enu = stateGetSpeedEnu_f();
float course = stateGetHorizontalSpeedDir_f();
xi_dot << vel_enu->x, vel_enu->y, vel_enu->z, w_dot;
Eigen::Matrix2f E;
Eigen::Matrix<float, 2, 4> F;
Eigen::Matrix<float, 2, 4> Fp;
Eigen::Matrix4f G;
Eigen::Matrix4f Gp;
Eigen::Matrix4f I;
Eigen::Vector2f h;
Eigen::Matrix<float, 1, 2> ht;
h << sinf(course), cosf(course);
ht = h.transpose();
I.setIdentity();
F << 1.0, 0.0, 0.0, 0.0,
0.0, 1.0, 0.0, 0.0;
E << 0.0, -1.0,
1.0, 0.0;
G = F.transpose() * F;
Fp = E * F;
Gp = F.transpose() * E * F;
Eigen::Matrix<float, 1, 4> Xt = X.transpose();
Eigen::Vector4f Xh = X / X.norm();
Eigen::Matrix<float, 1, 4> Xht = Xh.transpose();
float aux = ht * Fp * X;
float heading_rate = -1 / (Xt * G * X) * Xt * Gp * (I - Xh * Xht) * J * xi_dot - (gvf_parametric_control.k_psi * aux /
sqrtf(Xt * G * X));
float climbing_rate = (ground_speed * X(2)) / sqrtf(X(0) * X(0) + X(1) * X(1));
// Virtual coordinate update, even if the vehicle is not in autonomous mode, the parameter w will get "closer" to
// the vehicle. So it is not only okei but advisable to update it.
gvf_parametric_control.w += w_dot * gvf_parametric_control.delta_T * 1e-3;
gvf_parametric_low_level_control_3D(heading_rate, climbing_rate);
}
/** 2D TRAJECTORIES **/
// 2D TREFOIL KNOT
bool gvf_parametric_2D_trefoil_XY(float xo, float yo, float w1, float w2, float ratio, float r, float alpha)
{
gvf_parametric_trajectory.type = TREFOIL_2D;
gvf_parametric_trajectory.p_parametric[0] = xo;
gvf_parametric_trajectory.p_parametric[1] = yo;
gvf_parametric_trajectory.p_parametric[2] = w1;
gvf_parametric_trajectory.p_parametric[3] = w2;
gvf_parametric_trajectory.p_parametric[4] = ratio;
gvf_parametric_trajectory.p_parametric[5] = r;
gvf_parametric_trajectory.p_parametric[6] = alpha;
float f1, f2, f1d, f2d, f1dd, f2dd;
gvf_parametric_2d_trefoil_info(&f1, &f2, &f1d, &f2d, &f1dd, &f2dd);
gvf_parametric_control_2D(gvf_parametric_2d_trefoil_par.kx, gvf_parametric_2d_trefoil_par.ky, f1, f2, f1d, f2d, f1dd,
f2dd);
return true;
}
bool gvf_parametric_2D_trefoil_wp(uint8_t wp, float w1, float w2, float ratio, float r, float alpha)
{
gvf_parametric_2D_trefoil_XY(waypoints[wp].x, waypoints[wp].y, w1, w2, ratio, r, alpha);
return true;
}
/** 3D TRAJECTORIES **/
// 3D ELLIPSE
bool gvf_parametric_3D_ellipse_XYZ(float xo, float yo, float r, float zl, float zh, float alpha)
{
horizontal_mode = HORIZONTAL_MODE_CIRCLE; // Circle for the 2D GCS
// Safety first! If the asked altitude is low
if (zl > zh) {
zl = zh;
}
if (zl < 1 || zh < 1) {
zl = 10;
zh = 10;
}
if (r < 1) {
r = 60;
}
gvf_parametric_trajectory.type = ELLIPSE_3D;
gvf_parametric_trajectory.p_parametric[0] = xo;
gvf_parametric_trajectory.p_parametric[1] = yo;
gvf_parametric_trajectory.p_parametric[2] = r;
gvf_parametric_trajectory.p_parametric[3] = zl;
gvf_parametric_trajectory.p_parametric[4] = zh;
gvf_parametric_trajectory.p_parametric[5] = alpha;
float f1, f2, f3, f1d, f2d, f3d, f1dd, f2dd, f3dd;
gvf_parametric_3d_ellipse_info(&f1, &f2, &f3, &f1d, &f2d, &f3d, &f1dd, &f2dd, &f3dd);
gvf_parametric_control_3D(gvf_parametric_3d_ellipse_par.kx, gvf_parametric_3d_ellipse_par.ky,
gvf_parametric_3d_ellipse_par.kz, f1, f2, f3, f1d, f2d, f3d, f1dd, f2dd, f3dd);
return true;
}
bool gvf_parametric_3D_ellipse_wp(uint8_t wp, float r, float zl, float zh, float alpha)
{
gvf_parametric_3D_ellipse_XYZ(waypoints[wp].x, waypoints[wp].y, r, zl, zh, alpha);
return true;
}
bool gvf_parametric_3D_ellipse_wp_delta(uint8_t wp, float r, float alt_center, float delta, float alpha)
{
float zl = alt_center - delta;
float zh = alt_center + delta;
gvf_parametric_3D_ellipse_XYZ(waypoints[wp].x, waypoints[wp].y, r, zl, zh, alpha);
return true;
}
// 3D Lissajous
bool gvf_parametric_3D_lissajous_XYZ(float xo, float yo, float zo, float cx, float cy, float cz, float wx, float wy,
float wz, float dx, float dy, float dz, float alpha)
{
// Safety first! If the asked altitude is low
if ((zo - cz) < 1) {
zo = 10;
cz = 0;
}
gvf_parametric_trajectory.type = LISSAJOUS_3D;
gvf_parametric_trajectory.p_parametric[0] = xo;
gvf_parametric_trajectory.p_parametric[1] = yo;
gvf_parametric_trajectory.p_parametric[2] = zo;
gvf_parametric_trajectory.p_parametric[3] = cx;
gvf_parametric_trajectory.p_parametric[4] = cy;
gvf_parametric_trajectory.p_parametric[5] = cz;
gvf_parametric_trajectory.p_parametric[6] = wx;
gvf_parametric_trajectory.p_parametric[7] = wy;
gvf_parametric_trajectory.p_parametric[8] = wz;
gvf_parametric_trajectory.p_parametric[9] = dx;
gvf_parametric_trajectory.p_parametric[10] = dy;
gvf_parametric_trajectory.p_parametric[11] = dz;
gvf_parametric_trajectory.p_parametric[12] = alpha;
float f1, f2, f3, f1d, f2d, f3d, f1dd, f2dd, f3dd;
gvf_parametric_3d_lissajous_info(&f1, &f2, &f3, &f1d, &f2d, &f3d, &f1dd, &f2dd, &f3dd);
gvf_parametric_control_3D(gvf_parametric_3d_lissajous_par.kx, gvf_parametric_3d_lissajous_par.ky,
gvf_parametric_3d_lissajous_par.kz, f1, f2, f3, f1d, f2d, f3d, f1dd, f2dd, f3dd);
return true;
}
bool gvf_parametric_3D_lissajous_wp_center(uint8_t wp, float zo, float cx, float cy, float cz, float wx, float wy,
float wz, float dx, float dy, float dz, float alpha)
{
gvf_parametric_3D_lissajous_XYZ(waypoints[wp].x, waypoints[wp].y, zo, cx, cy, cz, wx, wy, wz, dx, dy, dz, alpha);
return true;
}
sw/airborne/modules/guidance/gvf_parametric/gvf_parametric.h
+132
View File
@@ -0,0 +1,132 @@
/*
* Copyright (C) 2020 Hector Garcia de Marina <hgarciad@ucm.es>
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file modules/guidance/gvf_parametric/gvf_parametric.h
*
* Guiding vector field algorithm for 2D and 3D parametric trajectories.
*/
#ifndef GVF_PARAMETRIC_H
#define GVF_PARAMETRIC_H
#define GVF_PARAMETRIC_GRAVITY 9.806
/*! Default gain kroll for tuning the "coordinated turn" */
#ifndef GVF_PARAMETRIC_CONTROL_KROLL
#define GVF_PARAMETRIC_CONTROL_KROLL 1
#endif
/*! Default gain kclimb for tuning the climbing setting point */
#ifndef GVF_PARAMETRIC_CONTROL_KCLIMB
#define GVF_PARAMETRIC_CONTROL_KCLIMB 1
#endif
/*! Default scale for the error signals */
#ifndef GVF_PARAMETRIC_CONTROL_L
#define GVF_PARAMETRIC_CONTROL_L 0.1
#endif
/*! Default scale for w */
#ifndef GVF_PARAMETRIC_CONTROL_BETA
#define GVF_PARAMETRIC_CONTROL_BETA 0.01
#endif
/*! Default gain kpsi for tuning the alignment of the vehicle with the vector field */
#ifndef GVF_PARAMETRIC_CONTROL_KPSI
#define GVF_PARAMETRIC_CONTROL_KPSI 1
#endif
#ifdef __cplusplus
extern "C" {
#endif
#include "modules/guidance/gvf_parametric/trajectories/gvf_parametric_3d_ellipse.h"
#include "modules/guidance/gvf_parametric/trajectories/gvf_parametric_3d_lissajous.h"
#include "modules/guidance/gvf_parametric/trajectories/gvf_parametric_2d_trefoil.h"
/** @typedef gvf_parametric_con
* @brief Control parameters for the GVF_PARAMETRIC
* @param w Virtual coordinate from the parametrization of the trajectory
* @param delta_T Time between iterations needed for integrating w
* @param s Defines the direction to be tracked. It takes the values -1 or 1.
* @param k_roll Gain for tuning the coordinated turn.
* @param k_climb Gain for tuning the climbing setting point.
*/
typedef struct {
float w;
float delta_T;
int8_t s;
float k_roll;
float k_climb;
float k_psi;
float L;
float beta;
} gvf_parametric_con;
extern gvf_parametric_con gvf_parametric_control;
// Parameters for the trajectories
enum trajectories_parametric {
TREFOIL_2D = 0,
ELLIPSE_3D = 1,
LISSAJOUS_3D = 2,
NONE_PARAMETRIC = 255,
};
typedef struct {
enum trajectories_parametric type;
float p_parametric[16];
float phi_errors[3];
} gvf_parametric_tra;
extern gvf_parametric_tra gvf_parametric_trajectory;
// Init function
extern void gvf_parametric_init(void);
// Control functions
extern void gvf_parametric_set_direction(int8_t s);
extern void gvf_parametric_control_2D(float, float, float, float, float, float, float, float);
extern void gvf_parametric_control_3D(float, float, float, float, float, float, float, float, float,
float, float, float);
// 2D Trefoil
extern bool gvf_parametric_2D_trefoil_XY(float, float, float, float, float, float, float);
extern bool gvf_parametric_2D_trefoil_wp(uint8_t, float, float, float, float, float);
// 3D Ellipse
extern bool gvf_parametric_3D_ellipse_XYZ(float, float, float, float, float, float);
extern bool gvf_parametric_3D_ellipse_wp(uint8_t, float, float, float, float);
extern bool gvf_parametric_3D_ellipse_wp_delta(uint8_t, float, float, float, float);
// 3D Lissajous
extern bool gvf_parametric_3D_lissajous_XYZ(float, float, float, float, float, float, float, float, float, float, float,
float, float);
extern bool gvf_parametric_3D_lissajous_wp_center(uint8_t, float, float, float, float, float, float, float, float,
float, float, float);
#ifdef __cplusplus
}
#endif
#endif // GVF_PARAMETRIC_H
sw/airborne/modules/guidance/gvf_parametric/gvf_parametric_low_level_control.c
+78
View File
@@ -0,0 +1,78 @@
/*
* Copyright (C) 2020 Hector Garcia de Marina <hgarciad@ucm.es>
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file modules/guidance/gvf_parametric/gvf_parametric_low_level_control.c
*
* Firmware dependent file for the guiding vector field algorithm for 2D and 3D parametric trajectories.
*/
#include "autopilot.h"
#include "gvf_parametric_low_level_control.h"
#include "gvf_parametric.h"
#if defined(FIXEDWING_FIRMWARE)
#include "firmwares/fixedwing/stabilization/stabilization_attitude.h"
#include "firmwares/fixedwing/guidance/guidance_v_n.h" // gvf_parametric is only compatible with the new pprz controller!
#endif
void gvf_parametric_low_level_control_2D(float heading_rate)
{
#if defined(FIXEDWING_FIRMWARE)
if (autopilot_get_mode() == AP_MODE_AUTO2) {
// Lateral XY coordinates
lateral_mode = LATERAL_MODE_ROLL;
struct FloatEulers *att = stateGetNedToBodyEulers_f();
float ground_speed = stateGetHorizontalSpeedNorm_f();
h_ctl_roll_setpoint =
-gvf_parametric_control.k_roll * atanf(heading_rate * ground_speed / GVF_PARAMETRIC_GRAVITY / cosf(att->theta));
BoundAbs(h_ctl_roll_setpoint, h_ctl_roll_max_setpoint); // Setting point for roll angle
}
#else
#error gvf_parametric does not support your firmware yet
#endif
}
void gvf_parametric_low_level_control_3D(float heading_rate, float climbing_rate)
{
#if defined(FIXEDWING_FIRMWARE)
if (autopilot_get_mode() == AP_MODE_AUTO2) {
// Vertical Z coordinate
v_ctl_mode = V_CTL_MODE_AUTO_CLIMB;
v_ctl_speed_mode = V_CTL_SPEED_THROTTLE;
v_ctl_climb_setpoint = gvf_parametric_control.k_climb * climbing_rate; // Setting point for vertical speed
// Lateral XY coordinates
lateral_mode = LATERAL_MODE_ROLL;
struct FloatEulers *att = stateGetNedToBodyEulers_f();
float ground_speed = stateGetHorizontalSpeedNorm_f();
h_ctl_roll_setpoint =
-gvf_parametric_control.k_roll * atanf(heading_rate * ground_speed / GVF_PARAMETRIC_GRAVITY / cosf(att->theta));
BoundAbs(h_ctl_roll_setpoint, h_ctl_roll_max_setpoint); // Setting point for roll angle
}
#else
#error gvf_parametric does not support your firmware yet
#endif
}
sw/airborne/modules/guidance/gvf_parametric/gvf_parametric_low_level_control.h
+44
View File
@@ -0,0 +1,44 @@
/*
* Copyright (C) 2020 Hector Garcia de Marina <hgarciad@ucm.es>
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file modules/guidance/gvf_parametric/gvf_parametric_low_level_control.h
*
* Firmware dependent file for the guiding vector field algorithm for 2D and 3D parametric trajectories.
*/
#ifndef GVF_PARAMETRIC_LOW_LEVEL_CONTROL_H
#define GVF_PARAMETRIC_LOW_LEVEL_CONTROL_H
#ifdef __cplusplus
extern "C" {
#endif
// Low level control functions
extern void gvf_parametric_low_level_control_2D(float);
extern void gvf_parametric_low_level_control_3D(float, float);
#ifdef __cplusplus
}
#endif
#endif // GVF_PARAMETRIC_LOW_LEVEL_CONTROL_H
sw/airborne/modules/guidance/gvf_parametric/trajectories/gvf_parametric_2d_trefoil.c
+102
View File
@@ -0,0 +1,102 @@
/*
* Copyright (C) 2020 Hector Garcia de Marina <hgarciad@ucm.es>
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file modules/guidance/gvf_parametric/trajectories/gvf_parametric_2d_trefoil.c
*
* Guiding vector field algorithm for 2D and 3D complex trajectories.
*
* 2D trefoil know
*/
#include "subsystems/navigation/common_nav.h"
#include "modules/guidance/gvf_parametric/gvf_parametric.h"
#include "gvf_parametric_2d_trefoil.h"
/*! Default gain kx for the 2d trefoil knot trajectory */
#ifndef GVF_PARAMETRIC_2D_TREFOIL_KX
#define GVF_PARAMETRIC_2D_TREFOIL_KX 0.001
#endif
/*! Default gain ky for the 2d trefoil knot trajectory */
#ifndef GVF_PARAMETRIC_2D_TREFOIL_KY
#define GVF_PARAMETRIC_2D_TREFOIL_KY 0.001
#endif
/*! Default 1st frequency for the 2d trefoil trajectory*/
#ifndef GVF_PARAMETRIC_2D_TREFOIL_W1
#define GVF_PARAMETRIC_2D_TREFOIL_W1 0.02
#endif
/*! Default 2nd frequency for the 2d trefoil trajectory*/
#ifndef GVF_PARAMETRIC_2D_TREFOIL_W2
#define GVF_PARAMETRIC_2D_TREFOIL_W2 0.03
#endif
/*! Default ratio for the 2d trefoil trajectory*/
#ifndef GVF_PARAMETRIC_2D_TREFOIL_RATIO
#define GVF_PARAMETRIC_2D_TREFOIL_RATIO 160
#endif
/*! Default radius of the circles for the 2d trefoil trajectory*/
#ifndef GVF_PARAMETRIC_2D_TREFOIL_R
#define GVF_PARAMETRIC_2D_TREFOIL_R 80
#endif
/*! Default orientation for the 2d trefoil trajectory*/
#ifndef GVF_PARAMETRIC_2D_TREFOIL_ALPHA
#define GVF_PARAMETRIC_2D_TREFOIL_ALPHA 0
#endif
gvf_par_2d_tre_par gvf_parametric_2d_trefoil_par = {GVF_PARAMETRIC_2D_TREFOIL_KX, GVF_PARAMETRIC_2D_TREFOIL_KY, GVF_PARAMETRIC_2D_TREFOIL_W1, GVF_PARAMETRIC_2D_TREFOIL_W2, GVF_PARAMETRIC_2D_TREFOIL_RATIO, GVF_PARAMETRIC_2D_TREFOIL_R, GVF_PARAMETRIC_2D_TREFOIL_ALPHA};
void gvf_parametric_2d_trefoil_info(float *f1, float *f2, float *f1d, float *f2d, float *f1dd, float *f2dd)
{
float xo = gvf_parametric_trajectory.p_parametric[0];
float yo = gvf_parametric_trajectory.p_parametric[1];
float w1 = gvf_parametric_trajectory.p_parametric[2];
float w2 = gvf_parametric_trajectory.p_parametric[3];
float ratio = gvf_parametric_trajectory.p_parametric[4];
float r = gvf_parametric_trajectory.p_parametric[5];
float alpha_rad = gvf_parametric_trajectory.p_parametric[6]*M_PI/180;
float w = gvf_parametric_control.w;
float wb = w * gvf_parametric_control.beta * gvf_parametric_control.s;
// Parametric equations of the trajectory and the partial derivatives w.r.t. 'w'
float nrf1 = cosf(wb*w1)*(r*cosf(wb*w2) + ratio);
float nrf2 = sinf(wb*w1)*(r*cosf(wb*w2) + ratio);
float nrf1d = -w1*sinf(wb*w1)*(r*cosf(wb*w2) + ratio) - cosf(wb*w1)*r*w2*sinf(wb*w2);
float nrf2d = w1*cosf(wb*w1)*(r*cosf(wb*w2) + ratio) - sinf(wb*w1)*r*w2*sinf(wb*w2);
float nrf1dd = -w1*w1*cosf(wb*w1)*(r*cosf(wb*w2) + ratio) + w1*sinf(wb*w1)*r*w2*sinf(wb*w2) + w1*sinf(wb*w1)*r*w2*sinf(wb*w2) - cosf(wb*w1)*r*w2*w2*cosf(wb*w2);
float nrf2dd = -w1*w1*sinf(wb*w1)*(r*cosf(wb*w2) + ratio) - w1*cosf(wb*w1)*r*w2*sinf(wb*w2) - w1*cosf(wb*w1)*r*w2*sinf(wb*w2) - sinf(wb*w1)*r*w2*w2*cosf(wb*w2);
*f1 = cosf(alpha_rad)*nrf1 - sinf(alpha_rad)*nrf2 + xo;
*f2 = sinf(alpha_rad)*nrf1 + cosf(alpha_rad)*nrf2 + yo;
*f1d = cosf(alpha_rad)*nrf1d - sinf(alpha_rad)*nrf2d;
*f2d = sinf(alpha_rad)*nrf1d + cosf(alpha_rad)*nrf2d;
*f1dd = cosf(alpha_rad)*nrf1dd - sinf(alpha_rad)*nrf2dd;
*f2dd = sinf(alpha_rad)*nrf1dd + cosf(alpha_rad)*nrf2dd;
}
sw/airborne/modules/guidance/gvf_parametric/trajectories/gvf_parametric_2d_trefoil.h
+64
View File
@@ -0,0 +1,64 @@
/*
* Copyright (C) 2020 Hector Garcia de Marina <hgarciad@ucm.es>
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file modules/guidance/gvf_parametric/trajectories/gvf_parametric_2d_trefoil.h
*
* Guiding vector field algorithm for 2D and 3D complex trajectories.
*
* 2D trefoil knot
*/
#ifndef GVF_PARAMETRIC_2D_TREFOIL_H
#define GVF_PARAMETRIC_2D_TREFOIL_H
#ifdef __cplusplus
extern "C" {
#endif
/** @typedef gvf_2d_tre_par
* @brief Parameters for the GVF parametric 2D trefoil knot
* @param kx Gain defining how agressive is the vector field in x coordinate
* @param ky Gain defining how agressive is the vector field in y coordinate
* @param w1 1st frequency
* @param w2 2nd frequency
* @param off Off-phase
* @param r Radius of the "circles"
* @param alpha Orientation/rotation of the trajectory in the XY plane
*/
typedef struct {
float kx;
float ky;
float w1;
float w2;
float ratio;
float r;
float alpha;
} gvf_par_2d_tre_par;
extern gvf_par_2d_tre_par gvf_parametric_2d_trefoil_par;
extern void gvf_parametric_2d_trefoil_info(float *f1, float *f2, float *f1d, float *f2d, float *f1dd, float *f2dd);
#ifdef __cplusplus
}
#endif
#endif // GVF_PARAMETRIC_2D_TREFOIL_H
sw/airborne/modules/guidance/gvf_parametric/trajectories/gvf_parametric_3d_ellipse.c
+98
View File
@@ -0,0 +1,98 @@
/*
* Copyright (C) 2020 Hector Garcia de Marina <hgarciad@ucm.es>
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file modules/guidance/gvf_parametric/trajectories/gvf_parametric_3d_ellipse.c
*
* Guiding vector field algorithm for 2D and 3D complex trajectories.
*
* 3D ellipse (intersection between a cylinder and a tilted plane)
*/
#include "subsystems/navigation/common_nav.h"
#include "modules/guidance/gvf_parametric/gvf_parametric.h"
#include "gvf_parametric_3d_ellipse.h"
/*! Default gain kx for the 3d ellipse trajectory */
#ifndef GVF_PARAMETRIC_3D_ELLIPSE_KX
#define GVF_PARAMETRIC_3D_ELLIPSE_KX 0.001
#endif
/*! Default gain ky for the 3d ellipse trajectory */
#ifndef GVF_PARAMETRIC_3D_ELLIPSE_KY
#define GVF_PARAMETRIC_3D_ELLIPSE_KY 0.001
#endif
/*! Default gain kz for the 3d ellipse trajectory */
#ifndef GVF_PARAMETRIC_3D_ELLIPSE_KZ
#define GVF_PARAMETRIC_3D_ELLIPSE_KZ 0.001
#endif
/*! Default radius of the cylinder */
#ifndef GVF_PARAMETRIC_3D_ELLIPSE_R
#define GVF_PARAMETRIC_3D_ELLIPSE_R 80
#endif
/*! Default highest point for the ellipse trajectory */
#ifndef GVF_PARAMETRIC_3D_ELLIPSE_ZL
#define GVF_PARAMETRIC_3D_ELLIPSE_ZL 40
#endif
/*! Default highest point for the ellipse trajectory */
#ifndef GVF_PARAMETRIC_3D_ELLIPSE_ZH
#define GVF_PARAMETRIC_3D_ELLIPSE_ZH 40
#endif
/*! Default orientation in degrees for the lowest point of the ellipse */
#ifndef GVF_PARAMETRIC_3D_ELLIPSE_ALPHA
#define GVF_PARAMETRIC_3D_ELLIPSE_ALPHA 0
#endif
gvf_par_3d_ell_par gvf_parametric_3d_ellipse_par = {GVF_PARAMETRIC_3D_ELLIPSE_KX,
GVF_PARAMETRIC_3D_ELLIPSE_KY, GVF_PARAMETRIC_3D_ELLIPSE_KZ, GVF_PARAMETRIC_3D_ELLIPSE_R, GVF_PARAMETRIC_3D_ELLIPSE_ZL, GVF_PARAMETRIC_3D_ELLIPSE_ZH, GVF_PARAMETRIC_3D_ELLIPSE_ALPHA
};
void gvf_parametric_3d_ellipse_info(float *f1, float *f2, float *f3, float *f1d, float *f2d, float *f3d,
float *f1dd, float *f2dd, float *f3dd)
{
float xo = gvf_parametric_trajectory.p_parametric[0];
float yo = gvf_parametric_trajectory.p_parametric[1];
float r = gvf_parametric_trajectory.p_parametric[2];
float zl = gvf_parametric_trajectory.p_parametric[3];
float zh = gvf_parametric_trajectory.p_parametric[4];
float alpha_rad = gvf_parametric_trajectory.p_parametric[5]*M_PI/180;
float w = gvf_parametric_control.w;
float wb = w * gvf_parametric_control.beta * gvf_parametric_control.s;
// Parametric equations of the trajectory and the partial derivatives w.r.t. 'w'
*f1 = r * cosf(wb) + xo;
*f2 = r * sinf(wb) + yo;
*f3 = 0.5 * (zh + zl + (zl - zh) * sinf(alpha_rad - wb));
*f1d = -r * sinf(wb);
*f2d = r * cosf(wb);
*f3d = -0.5 * (zl - zh) * cosf(alpha_rad - wb);
*f1dd = -r * cosf(wb);
*f2dd = -r * sinf(wb);
*f3dd = -0.5 * (zl - zh) * sinf(alpha_rad - wb);
}
sw/airborne/modules/guidance/gvf_parametric/trajectories/gvf_parametric_3d_ellipse.h
+65
View File
@@ -0,0 +1,65 @@
/*
* Copyright (C) 2020 Hector Garcia de Marina <hgarciad@ucm.es>
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file modules/guidance/gvf_parametric/trajectories/gvf_parametric_3d_ellipse.h
*
* Guiding vector field algorithm for 2D and 3D complex trajectories.
*
* 3D ellipse (intersection between a cylinder and a tilted plane)
*/
#ifndef GVF_PARAMETRIC_3D_ELLIPSE_H
#define GVF_PARAMETRIC_3D_ELLIPSE_H
#ifdef __cplusplus
extern "C" {
#endif
/** @typedef gvf_3d_ell_par
* @brief Parameters for the GVF parametric 3D ellipse
* @param kx Gain defining how agressive is the vector field in x coordinate
* @param ky Gain defining how agressive is the vector field in y coordinate
* @param kz Gain defining how agressive is the vector field in z coordinate
* @param r Radius of the cylinder in meters
* @param zl Altitude of the lowest point of the ellipse
* @param zh Altitude of the highest point of the ellipse
* @param alpha Heading of the lowest point zl in rads
*/
typedef struct {
float kx;
float ky;
float kz;
float r;
float zl;
float zh;
float alpha;
} gvf_par_3d_ell_par;
extern gvf_par_3d_ell_par gvf_parametric_3d_ellipse_par;
extern void gvf_parametric_3d_ellipse_info(float *f1, float *f2, float *f3, float *f1d, float *f2d, float *f3d,
float *f1dd, float *f2dd, float *f3dd);
#ifdef __cplusplus
}
#endif
#endif // GVF_PARAMETRIC_3D_ELLIPSE_H
sw/airborne/modules/guidance/gvf_parametric/trajectories/gvf_parametric_3d_lissajous.c
+143
View File
@@ -0,0 +1,143 @@
/*
* Copyright (C) 2020 Hector Garcia de Marina <hgarciad@ucm.es>
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file modules/guidance/gvf_parametric/trajectories/gvf_parametric_3d_lissajous.c
*
* Guiding vector field algorithm for 2D and 3D complex trajectories.
*
* 3D lissajous
*/
#include "subsystems/navigation/common_nav.h"
#include "modules/guidance/gvf_parametric/gvf_parametric.h"
#include "gvf_parametric_3d_lissajous.h"
/*! Default gain kx for the 3d lissajous trajectory */
#ifndef GVF_PARAMETRIC_3D_LISSAJOUS_KX
#define GVF_PARAMETRIC_3D_LISSAJOUS_KX 0.001
#endif
/*! Default gain ky for the 3d lissajous trajectory */
#ifndef GVF_PARAMETRIC_3D_LISSAJOUS_KY
#define GVF_PARAMETRIC_3D_LISSAJOUS_KY 0.001
#endif
/*! Default gain kz for the 3d lissajous trajectory */
#ifndef GVF_PARAMETRIC_3D_LISSAJOUS_KZ
#define GVF_PARAMETRIC_3D_LISSAJOUS_KZ 0.001
#endif
/*! Default amplitude of the trajectory in the X plane */
#ifndef GVF_PARAMETRIC_3D_LISSAJOUS_CX
#define GVF_PARAMETRIC_3D_LISSAJOUS_CX 80
#endif
/*! Default amplitude of the trajectory in the Y plane */
#ifndef GVF_PARAMETRIC_3D_LISSAJOUS_CY
#define GVF_PARAMETRIC_3D_LISSAJOUS_CY 80
#endif
/*! Default amplitude of the trajectory in the Z plane */
#ifndef GVF_PARAMETRIC_3D_LISSAJOUS_CZ
#define GVF_PARAMETRIC_3D_LISSAJOUS_CZ 10
#endif
/*! Default frequency of the trajectory in the X plane */
#ifndef GVF_PARAMETRIC_3D_LISSAJOUS_WX
#define GVF_PARAMETRIC_3D_LISSAJOUS_WX 1
#endif
/*! Default frequency of the trajectory in the Y plane */
#ifndef GVF_PARAMETRIC_3D_LISSAJOUS_WY
#define GVF_PARAMETRIC_3D_LISSAJOUS_WY 1
#endif
/*! Default frequency of the trajectory in the Z plane */
#ifndef GVF_PARAMETRIC_3D_LISSAJOUS_WZ
#define GVF_PARAMETRIC_3D_LISSAJOUS_WZ 1
#endif
/*! Default offphase of the trajectory in the X plane */
#ifndef GVF_PARAMETRIC_3D_LISSAJOUS_DX
#define GVF_PARAMETRIC_3D_LISSAJOUS_DX 0
#endif
/*! Default offphase of the trajectory in the Y plane */
#ifndef GVF_PARAMETRIC_3D_LISSAJOUS_DY
#define GVF_PARAMETRIC_3D_LISSAJOUS_DY 0
#endif
/*! Default offphase of the trajectory in the Z plane */
#ifndef GVF_PARAMETRIC_3D_LISSAJOUS_DZ
#define GVF_PARAMETRIC_3D_LISSAJOUS_DZ 0
#endif
/*! Default orientation in degrees for the trajectory in the XY plane */
#ifndef GVF_PARAMETRIC_3D_LISSAJOUS_ALPHA
#define GVF_PARAMETRIC_3D_LISSAJOUS_ALPHA 0
#endif
gvf_par_3d_lis_par gvf_parametric_3d_lissajous_par = {GVF_PARAMETRIC_3D_LISSAJOUS_KX, GVF_PARAMETRIC_3D_LISSAJOUS_KY, GVF_PARAMETRIC_3D_LISSAJOUS_KZ, GVF_PARAMETRIC_3D_LISSAJOUS_CX, GVF_PARAMETRIC_3D_LISSAJOUS_CY, GVF_PARAMETRIC_3D_LISSAJOUS_CZ, GVF_PARAMETRIC_3D_LISSAJOUS_WX, GVF_PARAMETRIC_3D_LISSAJOUS_WY, GVF_PARAMETRIC_3D_LISSAJOUS_WZ, GVF_PARAMETRIC_3D_LISSAJOUS_DX, GVF_PARAMETRIC_3D_LISSAJOUS_DY, GVF_PARAMETRIC_3D_LISSAJOUS_DZ, GVF_PARAMETRIC_3D_LISSAJOUS_ALPHA};
void gvf_parametric_3d_lissajous_info(float *f1, float *f2, float *f3, float *f1d, float *f2d, float *f3d,
float *f1dd, float *f2dd, float *f3dd)
{
float xo = gvf_parametric_trajectory.p_parametric[0];
float yo = gvf_parametric_trajectory.p_parametric[1];
float zo = gvf_parametric_trajectory.p_parametric[2];
float cx = gvf_parametric_trajectory.p_parametric[3];
float cy = gvf_parametric_trajectory.p_parametric[4];
float cz = gvf_parametric_trajectory.p_parametric[5];
float wx = gvf_parametric_trajectory.p_parametric[6];
float wy = gvf_parametric_trajectory.p_parametric[7];
float wz = gvf_parametric_trajectory.p_parametric[8];
float deltax_rad = gvf_parametric_trajectory.p_parametric[9]*M_PI/180;
float deltay_rad = gvf_parametric_trajectory.p_parametric[10]*M_PI/180;
float deltaz_rad = gvf_parametric_trajectory.p_parametric[11]*M_PI/180;
float alpha_rad = gvf_parametric_trajectory.p_parametric[12]*M_PI/180;
float w = gvf_parametric_control.w;
float wb = w * gvf_parametric_control.beta * gvf_parametric_control.s;
// Parametric equations of the trajectory and the partial derivatives w.r.t. 'w'
float nrf1 = cx*cosf(wx*wb + deltax_rad);
float nrf2 = cy*cosf(wy*wb + deltay_rad);
*f1 = cosf(alpha_rad)*nrf1 - sinf(alpha_rad)*nrf2 + xo;
*f2 = sinf(alpha_rad)*nrf1 + cosf(alpha_rad)*nrf2 + yo;
*f3 = cz*cosf(wz*wb + deltaz_rad) + zo;
float nrf1d = -wx*cx*sinf(wx*wb + deltax_rad);
float nrf2d = -wy*cy*sinf(wy*wb + deltay_rad);
*f1d = cosf(alpha_rad)*nrf1d - sinf(alpha_rad)*nrf2d;
*f2d = sinf(alpha_rad)*nrf1d + cosf(alpha_rad)*nrf2d;
*f3d = -wz*cz*sinf(wz*wb + deltaz_rad);
float nrf1dd = -wx*wx*cx*cosf(wx*wb + deltax_rad);
float nrf2dd = -wy*wy*cy*cosf(wy*wb + deltay_rad);
*f1dd = cosf(alpha_rad)*nrf1dd - sinf(alpha_rad)*nrf2dd;
*f2dd = sinf(alpha_rad)*nrf1dd + cosf(alpha_rad)*nrf2dd;
*f3dd = -wz*wz*cz*cosf(wz*wb + deltaz_rad);
}
sw/airborne/modules/guidance/gvf_parametric/trajectories/gvf_parametric_3d_lissajous.h
+77
View File
@@ -0,0 +1,77 @@
/*
* Copyright (C) 2020 Hector Garcia de Marina <hgarciad@ucm.es>
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, see
* <http://www.gnu.org/licenses/>.
*/
/**
* @file modules/guidance/gvf_parametric/trajectories/gvf_parametric_3d_lissajous.h
*
* Guiding vector field algorithm for 2D and 3D complex trajectories.
*
* 3D lissajous figures
*/
#ifndef GVF_PARAMETRIC_3D_LISSAJOUS_H
#define GVF_PARAMETRIC_3D_LISSAJOUS_H
#ifdef __cplusplus
extern "C" {
#endif
/** @typedef gvf_3d_lis_par
* @brief Parameters for the GVF parametric 3D lissajous
* @param kx Gain defining how agressive is the vector field in x coordinate
* @param ky Gain defining how agressive is the vector field in y coordinate
* @param kz Gain defining how agressive is the vector field in z coordinate
* @param cx Amplitude of the trajectory in the X plane
* @param cy Amplitude of the trajectory in the Y plane
* @param cz Amplitude of the trajectory in the Z plane
* @param wx Frequency of the trajectory in the X plane
* @param wy Frequency of the trajectory in the Y plane
* @param wz Frequency of the trajectory in the Z plane
* @param dx Offphase of the trajectory in the X plane
* @param dy Offphase of the trajectory in the Y plane
* @param dz Offphase of the trajectory in the Z plane
* @param alpha Orientation/rotation of the trajectory in the XY plane
*/
typedef struct {
float kx;
float ky;
float kz;
float cx;
float cy;
float cz;
float wx;
float wy;
float wz;
float dx;
float dy;
float dz;
float alpha;
} gvf_par_3d_lis_par;
extern gvf_par_3d_lis_par gvf_parametric_3d_lissajous_par;
extern void gvf_parametric_3d_lissajous_info(float *f1, float *f2, float *f3, float *f1d, float *f2d, float *f3d,
float *f1dd, float *f2dd, float *f3dd);
#ifdef __cplusplus
}
#endif
#endif // GVF_PARAMETRIC_3D_LISSAJOUS_H
sw/ground_segment/python/gvf/gvf_parametric_3d_lissa_sim.py
+189
View File
@@ -0,0 +1,189 @@
# Simulator for the gvf_parametric algorithm in Paparazzi (fixed-wing).
# Here you can check the demanded climbing and heading rates for your aircraft
# and the expected trajectories as well.
import numpy as np
from scipy import linalg as la
import matplotlib.pyplot as plt
import matplotlib
from mpl_toolkits.mplot3d import Axes3D
# Happy pdf for a happy submission without complains in paperplaza, arxiv, etc
font = {'size' : 20}
matplotlib.rc('font', **font)
matplotlib.rcParams['ps.useafm'] = True
matplotlib.rcParams['pdf.use14corefonts'] = True
matplotlib.rcParams['text.usetex'] = True
# Simulation parameters
tf = 500
dt = 5e-2
time = np.linspace(0, tf, tf/dt)
it = 1
# Data log
X_h = np.zeros((time.size, 4))
theta_h = np.zeros((time.size, 1))
e1_h = np.zeros((time.size, 1))
e2_h = np.zeros((time.size, 1))
e3_h = np.zeros((time.size, 1))
u_theta_h = np.zeros((time.size, 1))
u_z_h = np.zeros((time.size, 1))
u_w_h = np.zeros((time.size, 1))
# Initial conditions
X = np.array([[100], [-53], [40], [0]])
X_dot = np.array([[0], [0], [0], [0]])
theta = np.pi/4
X_h[0, :] = X.transpose()
theta_h[0] = theta
# Desired trajectory
xo = 0
yo = 0
zo = 100
cx = 150
cy = 150
cz = -10
deltax = 0
deltay = np.pi/2
deltaz = 0
wx = 1
wy = 1
wz = 1
alpha = np.pi/4
# Controller
L = 1e-1
beta = 1e-2
k1 = 1e-3
k2 = 1e-3
k3 = 1e-3
ktheta = 0.5
# Vehicle
s = 15
for t in time[:-1]:
x = X[0][0]
y = X[1][0]
z = X[2][0]
w = X[3][0]
wb = w*beta
#f
nrf1 = cx*np.cos(wx*wb + deltax)
nrf2 = cy*np.cos(wy*wb + deltay)
f3 = cz*np.cos(wz*wb + deltaz) + zo
nrf1d = -wx*cx*np.sin(wx*wb + deltax)
nrf2d = -wy*cy*np.sin(wy*wb + deltay)
f3d = -wz*cz*np.sin(wz*wb + deltaz)
nrf1dd = -wx*wx*cx*np.cos(wx*wb + deltax)
nrf2dd = -wy*wy*cy*np.cos(wy*wb + deltay)
f3dd = -wz*wz*cz*np.cos(wz*wb + deltaz)
f1 = np.cos(alpha)*nrf1 - np.sin(alpha)*nrf2 + xo
f2 = np.sin(alpha)*nrf1 + np.cos(alpha)*nrf2 + yo
f1d = np.cos(alpha)*nrf1d - np.sin(alpha)*nrf2d
f2d = np.sin(alpha)*nrf1d + np.cos(alpha)*nrf2d
f1dd = np.cos(alpha)*nrf1dd - np.sin(alpha)*nrf2dd
f2dd = np.sin(alpha)*nrf1dd + np.cos(alpha)*nrf2dd
#phi
phi1 = L*(x - f1)
phi2 = L*(y - f2)
phi3 = L*(z - f3)
#Chi, J
Chi = L*np.array([[-f1d*L*L*beta -k1*phi1],
[-f2d*L*L*beta -k2*phi2],
[-f3d*L*L*beta -k3*phi3],
[-L*L + beta*(k1*phi1*f1d + k2*phi2*f2d + k3*phi3*f3d)]])
j44 = beta*beta*(k1*(phi1*f1dd-L*f1d*f1d) + k2*(phi2*f2dd-L*f2d*f2d) + k3*(phi3*f3dd-L*f3d*f3d))
J = L*np.array([[-k1*L, 0, 0, -(beta*L)*(beta*L*f1dd-k1*f1d)],
[ 0, -k2*L, 0, -(beta*L)*(beta*L*f2dd-k2*f2d)],
[ 0, 0, -k3*L, -(beta*L)*(beta*L*f3dd-k3*f3d)],
[beta*L*k1*f1d, beta*L*k2*f2d, beta*L*k3*f3d, j44]])
#G, Fp, Gp
G = np.array([[1,0,0,0],
[0,1,0,0],
[0,0,0,0],
[0,0,0,0]])
Fp = np.array([[0, -1, 0, 0],
[1, 0, 0, 0]])
Gp = np.array([[0, -1, 0, 0],
[1, 0, 0, 0],
[0, 0, 0, 0],
[0, 0, 0, 0]])
h = np.array([[np.cos(theta)],[np.sin(theta)]])
ht = h.transpose()
Chit = Chi.transpose()
Chinorm = np.sqrt(Chi.transpose().dot(Chi))[0][0]
Chih = Chi / Chinorm
u_theta = (-(1/(Chit.dot(G).dot(Chi))*Chit.dot(Gp).dot(np.eye(4) - Chih.dot(Chih.transpose())).dot(J).dot(X_dot)) - ktheta*ht.dot(Fp).dot(Chi) / np.sqrt(Chit.dot(G).dot(Chi)))[0][0]
u_zeta = Chi[2][0]*s / np.sqrt(Chi[0][0]*Chi[0][0] + Chi[1][0]*Chi[1][0])
u_w = Chi[3][0]*s / np.sqrt(Chi[0][0]*Chi[0][0] + Chi[1][0]*Chi[1][0])
# Euler integration
theta = theta + u_theta*dt
X_dot = np.array([[s*np.cos(theta)],[s*np.sin(theta)], [u_zeta], [u_w]])
X = X + X_dot*dt
# Log
X_h[it, :] = X.transpose()
theta_h[it] = theta
e1_h[it] = phi1
e2_h[it] = phi2
e3_h[it] = phi3
u_theta_h[it] = u_theta
u_z_h[it] = u_zeta
u_w_h[it] = u_w
it = it + 1
# Plots
fig3d = plt.figure()
ax3d = fig3d.gca(projection='3d')
ax3d.plot(X_h[:,0], X_h[:,1], X_h[:,2])
f, (axe1,axe2,axe3) = plt.subplots(3,1)
axe1.plot(time[1:], e1_h[1:])
axe2.plot(time[1:], e2_h[1:])
axe3.plot(time[1:], e3_h[1:])
axe3.set_xlabel("Time [s]")
axe1.set_xlabel("error X")
axe2.set_xlabel("error Y")
axe3.set_xlabel("error Z")
f, (axut,axuz,axuw) = plt.subplots(3,1)
axut.plot(time[1:], u_theta_h[1:])
axuz.plot(time[1:], u_z_h[1:])
axuw.plot(time[1:], u_w_h[1:])
axuw.set_xlabel("Time [s]")
axut.set_ylabel("Heading rate [rad/s]")
axuz.set_ylabel("Climbing rate [m/s]")
axuw.set_ylabel("Virual coord rate")
plt.show()
sw/ground_segment/python/gvf/gvfframe.py
+304 -57
View File
@@ -4,6 +4,7 @@ import time
from scipy import linalg as la from scipy import linalg as la
from matplotlib.path import Path from matplotlib.path import Path
from matplotlib.backends.backend_wxagg import FigureCanvasWxAgg as FigureCanvas from matplotlib.backends.backend_wxagg import FigureCanvasWxAgg as FigureCanvas
from mpl_toolkits.mplot3d import Axes3D
import matplotlib.pyplot as pl import matplotlib.pyplot as pl
import matplotlib.patches as patches import matplotlib.patches as patches
import numpy as np import numpy as np
@@ -33,12 +34,13 @@ class GVFFrame(wx.Frame):
self.course = 0 self.course = 0
self.yaw = 0 self.yaw = 0
self.XY = np.array([0, 0]) self.XY = np.array([0, 0])
self.altitude = 0
self.ground_altitude = -1
# Desired trajectory # Desired trajectory
self.timer_traj = 0 # We do not update the traj every time we receive a msg
self.timer_traj_lim = 7 # (7+1) * 0.25secs
self.s = 0 self.s = 0
self.ke = 0 self.ke = 0
self.gvf_error = 0
self.map_gvf = map2d(np.array([0, 0]), 150000) self.map_gvf = map2d(np.array([0, 0]), 150000)
self.traj = None self.traj = None
@@ -59,16 +61,18 @@ class GVFFrame(wx.Frame):
if int(ac_id) == self.ac_id: if int(ac_id) == self.ac_id:
if msg.name == 'GPS': if msg.name == 'GPS':
self.course = int(msg.get_field(3))*np.pi/1800 self.course = int(msg.get_field(3))*np.pi/1800
self.altitude = float(msg.get_field(4))/1000
if msg.name == 'NAVIGATION': if msg.name == 'NAVIGATION':
self.XY[0] = float(msg.get_field(2)) self.XY[0] = float(msg.get_field(2))
self.XY[1] = float(msg.get_field(3)) self.XY[1] = float(msg.get_field(3))
if msg.name == 'NAVIGATION_REF':
self.ground_altitude = float(msg.get_field(3))
if msg.name == 'ATTITUDE': if msg.name == 'ATTITUDE':
self.yaw = float(msg.get_field(1)) self.yaw = float(msg.get_field(1))
if msg.name == 'GVF': if msg.name == 'GVF':
self.gvf_error = float(msg.get_field(0)) self.gvf_error = float(msg.get_field(0))
# Straight line # Straight line
if int(msg.get_field(1)) == 0 \ if int(msg.get_field(1)) == 0:
and self.timer_traj == self.timer_traj_lim:
self.s = int(msg.get_field(2)) self.s = int(msg.get_field(2))
self.ke = float(msg.get_field(3)) self.ke = float(msg.get_field(3))
param = [float(x) for x in msg.get_field(4)] param = [float(x) for x in msg.get_field(4)]
@@ -81,8 +85,7 @@ class GVFFrame(wx.Frame):
self.s, self.ke) self.s, self.ke)
# Ellipse # Ellipse
if int(msg.get_field(1)) == 1 \ elif int(msg.get_field(1)) == 1:
and self.timer_traj == self.timer_traj_lim:
self.s = int(msg.get_field(2)) self.s = int(msg.get_field(2))
self.ke = float(msg.get_field(3)) self.ke = float(msg.get_field(3))
param = [float(x) for x in msg.get_field(4)] param = [float(x) for x in msg.get_field(4)]
@@ -96,8 +99,7 @@ class GVFFrame(wx.Frame):
self.map_gvf.area, self.s, self.ke) self.map_gvf.area, self.s, self.ke)
# Sin # Sin
if int(msg.get_field(1)) == 2 \ elif int(msg.get_field(1)) == 2:
and self.timer_traj == self.timer_traj_lim:
self.s = int(msg.get_field(2)) self.s = int(msg.get_field(2))
self.ke = float(msg.get_field(3)) self.ke = float(msg.get_field(3))
param = [float(x) for x in msg.get_field(4)] param = [float(x) for x in msg.get_field(4)]
@@ -112,34 +114,83 @@ class GVFFrame(wx.Frame):
self.traj.vector_field(self.traj.XYoff, \ self.traj.vector_field(self.traj.XYoff, \
self.map_gvf.area, self.s, self.ke) self.map_gvf.area, self.s, self.ke)
self.timer_traj = self.timer_traj + 1 if msg.name == 'GVF_PARAMETRIC':
if self.timer_traj > self.timer_traj_lim: # Trefoil 2D
self.timer_traj = 0 if int(msg.get_field(0)) == 0:
self.s = int(msg.get_field(1))
self.wb = float(msg.get_field(2))
param = [float(x) for x in msg.get_field(3)]
xo = param[0]
yo = param[1]
w1 = param[2]
w2 = param[3]
ratio = param[4]
r = param[5]
alpha = param[6]
phi = [float(x) for x in msg.get_field(4)]
phi_x = phi[0]
phi_y = phi[1]
self.traj = traj_param_trefoil_2D(np.array([xo, yo]), w1, w2, ratio, r, alpha, self.wb)
def draw_gvf(self, XY, yaw, course): # Ellipse 3D
elif int(msg.get_field(0)) == 1:
self.s = int(msg.get_field(1))
self.wb = float(msg.get_field(2))
param = [float(x) for x in msg.get_field(3)]
xo = param[0]
yo = param[1]
r = param[2]
zl = param[3]
zh = param[4]
alpha = param[5]
phi = [float(x) for x in msg.get_field(4)]
phi_x = phi[0]
phi_y = phi[1]
phi_z = phi[2]
self.traj = traj_param_ellipse_3D(np.array([xo,yo]), r, zl, zh, alpha, self.wb)
# Lissajous 3D
elif int(msg.get_field(0)) == 2:
self.s = int(msg.get_field(1))
self.wb = float(msg.get_field(2))
param = [float(x) for x in msg.get_field(3)]
xo = param[0]
yo = param[1]
zo = param[2]
cx = param[3]
cy = param[4]
cz = param[5]
wx = param[6]
wy = param[7]
wz = param[8]
dx = param[9]
dy = param[10]
dz = param[11]
alpha = param[12]
phi = [float(x) for x in msg.get_field(4)]
phi_x = phi[0]
phi_y = phi[1]
phi_z = phi[2]
self.traj = traj_param_lissajous_3D(np.array([xo,yo]), zo, cx, cy, cz, \
wx, wy, wz, dx, dy, dz, alpha, self.wb)
def draw_gvf(self, XY, yaw, course, altitude):
if self.traj is not None: if self.traj is not None:
self.map_gvf.draw(XY, yaw, course, self.traj) self.map_gvf.draw(XY, yaw, course, altitude, self.traj)
def OnClose(self, event): def OnClose(self, event):
self.interface.shutdown() self.interface.shutdown()
self.Destroy() self.Destroy()
def OnRedrawTimer(self, event): def OnRedrawTimer(self, event):
self.draw_gvf(self.XY, self.yaw, self.course) self.draw_gvf(self.XY, self.yaw, self.course, self.altitude-self.ground_altitude)
self.canvas.draw() self.canvas.draw()
class map2d: class map2d:
def __init__(self, XYoff, area): def __init__(self, XYoff, area):
self.XYoff = XYoff self.XYoff = XYoff
self.area = area self.area = area
self.fig, self.ax = pl.subplots() self.fig = pl.figure()
self.ax.set_xlabel('South [m]')
self.ax.set_ylabel('West [m]')
self.ax.set_title('2D Map')
self.ax.annotate('HOME', xy = (0, 0))
self.ax.set_xlim(XYoff[0]-0.5*np.sqrt(area), XYoff[0]+0.5*np.sqrt(area))
self.ax.set_ylim(XYoff[1]-0.5*np.sqrt(area), XYoff[1]+0.5*np.sqrt(area))
self.ax.axis('equal')
def vehicle_patch(self, XY, yaw): def vehicle_patch(self, XY, yaw):
Rot = np.array([[np.cos(yaw), np.sin(yaw)],[-np.sin(yaw), np.cos(yaw)]]) Rot = np.array([[np.cos(yaw), np.sin(yaw)],[-np.sin(yaw), np.cos(yaw)]])
@@ -167,42 +218,113 @@ class map2d:
return patches.PathPatch(path, facecolor='red', lw=2) return patches.PathPatch(path, facecolor='red', lw=2)
def draw(self, XY, yaw, course, traj): def draw(self, XY, yaw, course, altitude, traj):
self.ax.clear() self.fig.clf()
self.ax.plot(traj.traj_points[0, :], traj.traj_points[1, :]) if traj.dim == 2:
self.ax.quiver(traj.mapgrad_X, traj.mapgrad_Y, \ ax = self.fig.add_subplot(111)
traj.mapgrad_U, traj.mapgrad_V, color='Teal', \ ax.plot(traj.traj_points[0, :], traj.traj_points[1, :])
pivot='mid', width=0.002) ax.quiver(traj.mapgrad_X, traj.mapgrad_Y, \
self.ax.add_patch(self.vehicle_patch(XY, yaw)) # In radians traj.mapgrad_U, traj.mapgrad_V, color='Teal', \
apex = 45*np.pi/180 # 30 degrees apex angle pivot='mid', width=0.002)
b = np.sqrt(2*(self.area/2000) / np.sin(apex)) ax.add_patch(self.vehicle_patch(XY, yaw)) # In radians
h = b*np.cos(apex/2) apex = 45*np.pi/180 # 30 degrees apex angle
self.ax.arrow(XY[0], XY[1], \ b = np.sqrt(2*(self.area/2000) / np.sin(apex))
h*np.sin(course), h*np.cos(course),\ h = b*np.cos(apex/2)
head_width=5, head_length=10, fc='k', ec='k') ax.arrow(XY[0], XY[1], \
self.ax.annotate('HOME', xy = (0, 0)) h*np.sin(course), h*np.cos(course),\
if isinstance(traj, traj_ellipse): head_width=5, head_length=10, fc='k', ec='k')
self.ax.annotate('ELLIPSE', xy = (traj.XYoff[0], traj.XYoff[1])) ax.annotate('HOME', xy = (0, 0))
self.ax.plot(0, 0, 'kx', ms=10, mew=2) if isinstance(traj, traj_ellipse):
self.ax.plot(traj.XYoff[0], traj.XYoff[1], 'kx', ms=10, mew=2) ax.annotate('ELLIPSE', xy = (traj.XYoff[0], traj.XYoff[1]))
elif isinstance(traj, traj_sin): ax.plot(0, 0, 'kx', ms=10, mew=2)
self.ax.annotate('SIN', xy = (traj.XYoff[0], traj.XYoff[1])) ax.plot(traj.XYoff[0], traj.XYoff[1], 'kx', ms=10, mew=2)
self.ax.plot(0, 0, 'kx', ms=10, mew=2) elif isinstance(traj, traj_sin):
self.ax.plot(traj.XYoff[0], traj.XYoff[1], 'kx', ms=10, mew=2) ax.annotate('SIN', xy = (traj.XYoff[0], traj.XYoff[1]))
elif isinstance(traj, traj_line): ax.plot(0, 0, 'kx', ms=10, mew=2)
self.ax.annotate('LINE', xy = (traj.XYoff[0], traj.XYoff[1])) ax.plot(traj.XYoff[0], traj.XYoff[1], 'kx', ms=10, mew=2)
self.ax.plot(0, 0, 'kx', ms=10, mew=2) elif isinstance(traj, traj_line):
self.ax.plot(traj.XYoff[0], traj.XYoff[1], 'kx', ms=10, mew=2) ax.annotate('LINE', xy = (traj.XYoff[0], traj.XYoff[1]))
ax.plot(0, 0, 'kx', ms=10, mew=2)
ax.plot(traj.XYoff[0], traj.XYoff[1], 'kx', ms=10, mew=2)
elif isinstance(traj, traj_param_trefoil_2D):
ax.annotate('TREFOIL', xy = (traj.XYoff[0], traj.XYoff[1]))
ax.plot(0, 0, 'kx', ms=10, mew=2)
ax.plot(traj.XYoff[0], traj.XYoff[1], 'kx', ms=10, mew=2)
ax.plot(traj.wpoint[0], traj.wpoint[1], 'rx', ms=10, mew=2)
self.ax.set_xlabel('South [m]') ax.set_xlabel('South [m]')
self.ax.set_ylabel('West [m]') ax.set_ylabel('West [m]')
self.ax.set_title('2D Map') ax.set_title('2D Map')
self.ax.set_xlim(self.XYoff[0]-0.5*np.sqrt(self.area), \ ax.set_xlim(self.XYoff[0]-0.5*np.sqrt(self.area), \
self.XYoff[0]+0.5*np.sqrt(self.area)) self.XYoff[0]+0.5*np.sqrt(self.area))
self.ax.set_ylim(self.XYoff[1]-0.5*np.sqrt(self.area), \ ax.set_ylim(self.XYoff[1]-0.5*np.sqrt(self.area), \
self.XYoff[1]+0.5*np.sqrt(self.area)) self.XYoff[1]+0.5*np.sqrt(self.area))
self.ax.axis('equal') ax.axis('equal')
self.ax.grid() ax.grid()
if traj.dim == 3:
a3d = self.fig.add_subplot(2,2,1, projection='3d')
axy = self.fig.add_subplot(2,2,2)
axz = self.fig.add_subplot(2,2,3)
ayz = self.fig.add_subplot(2,2,4)
a3d.set_title('3D Map')
axy.set_title('XY Map')
axz.set_title('XZ Map')
ayz.set_title('YZ Map')
# 3D
a3d.plot(traj.traj_points[0, :], traj.traj_points[1, :], traj.traj_points[2, :])
if altitude != -1:
a3d.plot([XY[0]], [XY[1]], [altitude], marker='o', markerfacecolor='r', markeredgecolor='r')
a3d.plot([traj.wpoint[0]], [traj.wpoint[1]], [traj.wpoint[2]], marker='x', markerfacecolor='r', markeredgecolor='r')
a3d.axis('equal')
if traj.deltaz < 0:
a3d.set_zlim(traj.zo+1.5*traj.deltaz, traj.zo-1.5*traj.deltaz)
else:
a3d.set_zlim(traj.zo-1.5*traj.deltaz, traj.zo+1.5*traj.deltaz)
# XY
axy.plot(traj.traj_points[0, :], traj.traj_points[1, :])
axy.add_patch(self.vehicle_patch(XY, yaw)) # In radians
apex = 45*np.pi/180 # 30 degrees apex angle
b = np.sqrt(2*(self.area/2000) / np.sin(apex))
h = b*np.cos(apex/2)
axy.arrow(XY[0], XY[1], \
h*np.sin(course), h*np.cos(course),\
head_width=5, head_length=10, fc='k', ec='k')
axy.annotate('HOME', xy = (0, 0))
axy.plot(traj.wpoint[0], traj.wpoint[1], 'rx', ms=10, mew=2)
if isinstance(traj, traj_param_ellipse_3D):
axy.annotate('ELLIPSE_3D', xy = (traj.XYoff[0], traj.XYoff[1]))
axy.plot(0, 0, 'kx', ms=10, mew=2)
axy.plot(traj.XYoff[0], traj.XYoff[1], 'kx', ms=10, mew=2)
elif isinstance(traj, traj_param_lissajous_3D):
axy.annotate('LISSA_3D', xy = (traj.XYoff[0], traj.XYoff[1]))
axy.plot(0, 0, 'kx', ms=10, mew=2)
axy.plot(traj.XYoff[0], traj.XYoff[1], 'kx', ms=10, mew=2)
axy.axis('equal')
# XZ
axz.plot(traj.traj_points[0, :], traj.traj_points[2, :])
if altitude != -1:
axz.plot([XY[0]], [altitude], 'ro')
axz.plot(traj.wpoint[0], traj.wpoint[2], 'rx', ms=10, mew=2)
if traj.deltaz < 0:
axz.set_ylim(traj.zo+1.5*traj.deltaz, traj.zo-1.5*traj.deltaz)
else:
axz.set_ylim(traj.zo-1.5*traj.deltaz, traj.zo+1.5*traj.deltaz)
# YZ
ayz.plot(traj.traj_points[1, :], traj.traj_points[2, :])
if altitude != -1:
ayz.plot([XY[1]], [altitude], 'ro')
ayz.plot(traj.wpoint[1], traj.wpoint[2], 'rx', ms=10, mew=2)
if traj.deltaz < 0:
axz.set_ylim(traj.zo+1.5*traj.deltaz, traj.zo-1.5*traj.deltaz)
else:
ayz.set_ylim(traj.zo-1.5*traj.deltaz, traj.zo+1.5*traj.deltaz)
class traj_line: class traj_line:
def float_range(self, start, end, step): def float_range(self, start, end, step):
@@ -211,6 +333,7 @@ class traj_line:
start += step start += step
def __init__(self, Xminmax, a, b, alpha): def __init__(self, Xminmax, a, b, alpha):
self.dim = 2
self.XYoff = np.array([a, b]) self.XYoff = np.array([a, b])
self.Xminmax = Xminmax self.Xminmax = Xminmax
self.a, self.b, self.alpha = a, b, alpha self.a, self.b, self.alpha = a, b, alpha
@@ -265,6 +388,7 @@ class traj_ellipse:
start += step start += step
def __init__(self, XYoff, rot, a, b): def __init__(self, XYoff, rot, a, b):
self.dim = 2
self.XYoff = XYoff self.XYoff = XYoff
self.a, self.b = a, b self.a, self.b = a, b
self.rot = rot self.rot = rot
@@ -324,6 +448,7 @@ class traj_sin:
start += step start += step
def __init__(self, Xminmax, a, b, alpha, w, off, A): def __init__(self, Xminmax, a, b, alpha, w, off, A):
self.dim = 2
self.XYoff = np.array([a, b]) self.XYoff = np.array([a, b])
self.Xminmax = Xminmax self.Xminmax = Xminmax
self.a, self.b, self.alpha, self.w, self.off, self.A = \ self.a, self.b, self.alpha, self.w, self.off, self.A = \
@@ -382,3 +507,125 @@ class traj_sin:
self.mapgrad_U = self.mapgrad_U/norm self.mapgrad_U = self.mapgrad_U/norm
self.mapgrad_V = self.mapgrad_V/norm self.mapgrad_V = self.mapgrad_V/norm
class traj_param_trefoil_2D:
def float_range(self, start, end, step):
while start <= end:
yield start
start += step
def __init__(self, XYoff, w1, w2, ratio, r, alpha, wb):
self.dim = 2
self.XYoff, self.w1, self.w2, self.ratio, self.r, self.alpha, self.wb = XYoff, w1, w2, ratio, r, alpha, wb
self.mapgrad_X = []
self.mapgrad_Y = []
self.mapgrad_U = []
self.mapgrad_V = []
self.alpha = alpha*np.pi/180
self.wpoint = self.param_point(self.wb)
num_points = 1000
self.traj_points = np.zeros((2, num_points))
i = 0
range_points = 1000.0
for t in self.float_range(0, range_points, range_points/num_points):
self.traj_points[:, i] = self.param_point(t)
i = i + 1
if i >= num_points:
break
def param_point(self, t):
xnr = np.cos(t*self.w1)*(self.r*np.cos(t*self.w2) + self.ratio)
ynr = np.sin(t*self.w1)*(self.r*np.cos(t*self.w2) + self.ratio)
x = np.cos(self.alpha)*xnr - np.sin(self.alpha)*ynr
y = np.sin(self.alpha)*xnr + np.cos(self.alpha)*ynr
return self.XYoff + np.array([x,y])
class traj_param_ellipse_3D:
def float_range(self, start, end, step):
while start <= end:
yield start
start += step
def __init__(self, XYoff, r, zl, zh, alpha, wb):
self.dim = 3
self.XYoff, self.r, self.zl, self.zh, self.alpha, self.wb = XYoff, r, zl, zh, alpha, wb
self.mapgrad_X = []
self.mapgrad_Y = []
self.mapgrad_U = []
self.mapgrad_V = []
self.deltaz = self.zh - self.zl # For the 3D plot
self.zo = self.zl + self.deltaz # For the 3D plot
self.alpha = self.alpha*np.pi/180
self.wpoint = self.param_point(self.wb)
num_points = 100
self.traj_points = np.zeros((3, num_points))
i = 0
range_points = 2*np.pi + 0.1
for t in self.float_range(0, range_points, range_points/num_points):
self.traj_points[:, i] = self.param_point(t)
i = i + 1
if i >= num_points:
break
def param_point(self, t):
x = self.r * np.cos(t) + self.XYoff[0]
y = self.r * np.sin(t) + self.XYoff[1]
z = 0.5 * (self.zh + self.zl + (self.zl - self.zh) * np.sin(self.alpha - t))
return np.array([x,y,z])
class traj_param_lissajous_3D:
def float_range(self, start, end, step):
while start <= end:
yield start
start += step
def __init__(self, XYoff, zo, cx, cy, cz, wx, wy, wz, dx, dy, dz, alpha, wb):
self.dim = 3
self.XYoff, self.zo, self.cx, self.cy, self.cz, self.wx, self.wy, self.wz, self.dx, self.dy, self.dz, \
self.alpha, self.wb = XYoff, zo, cx, cy, cz, wx, wy, wz, dx, dy, dz, alpha, wb
self.mapgrad_X = []
self.mapgrad_Y = []
self.mapgrad_U = []
self.mapgrad_V = []
self.deltaz = self.cz # For the 3D plot
self.alpha = self.alpha*np.pi/180
self.dx = self.dx*np.pi/180
self.dy = self.dy*np.pi/180
self.dz = self.dz*np.pi/180
self.wpoint = self.param_point(self.wb)
smallest_w = min([self.wx, self.wy, self.wz])
num_points = 100
self.traj_points = np.zeros((3, num_points))
i = 0
range_points = 3*np.pi / smallest_w
for t in self.float_range(0, range_points, range_points/num_points):
self.traj_points[:, i] = self.param_point(t)
i = i + 1
if i >= num_points:
break
def param_point(self, t):
xnr = self.cx*np.cos(self.wx*t + self.dx)
ynr = self.cy*np.cos(self.wy*t + self.dy)
z = self.cz*np.cos(self.wz*t + self.dz) + self.zo
x = np.cos(self.alpha)*xnr - np.sin(self.alpha)*ynr + self.XYoff[0]
y = np.sin(self.alpha)*xnr + np.cos(self.alpha)*ynr + self.XYoff[1]
return np.array([x,y,z])