From 68005486a17e4ac935b5df87729a6d5fe5b309ce Mon Sep 17 00:00:00 2001 From: Azarakhsh Keipour Date: Mon, 13 Jul 2020 19:06:10 -0400 Subject: [PATCH] Omni Pos-Ctrl: Thrust projected on current/commanded axes based on input --- .../PositionControl/ControlMath.cpp | 92 ++++++++++--------- .../PositionControl/ControlMath.hpp | 21 +++-- .../PositionControl/PositionControl.cpp | 2 +- 3 files changed, 66 insertions(+), 49 deletions(-) diff --git a/src/modules/mc_pos_control/PositionControl/ControlMath.cpp b/src/modules/mc_pos_control/PositionControl/ControlMath.cpp index 40367b6d59..b9f0fbacef 100644 --- a/src/modules/mc_pos_control/PositionControl/ControlMath.cpp +++ b/src/modules/mc_pos_control/PositionControl/ControlMath.cpp @@ -46,8 +46,9 @@ namespace ControlMath { void thrustToAttitude(const Vector3f &thr_sp, const float yaw_sp, const matrix::Quatf &att, const int omni_att_mode, const float omni_dfc_max_thrust, float &omni_att_tilt_angle, float &omni_att_tilt_dir, - float &omni_att_roll, float &omni_att_pitch, vehicle_attitude_setpoint_s &att_sp) + float &omni_att_roll, float &omni_att_pitch, int omni_proj_axes, vehicle_attitude_setpoint_s &att_sp) { + // Print an error if the omni_att_mode parameter is out of range if (omni_att_mode > 6 || omni_att_mode < 0) { PX4_ERR("OMNI_ATT_MODE parameter set to unknown value!"); @@ -55,24 +56,24 @@ void thrustToAttitude(const Vector3f &thr_sp, const float yaw_sp, const matrix:: switch (omni_att_mode) { case 1: // Attitude is set to the minimum roll and pitch (used for omnidirectional vehicles) - thrustToMinTiltAttitude(thr_sp, yaw_sp, omni_dfc_max_thrust, att_sp); + thrustToMinTiltAttitude(thr_sp, yaw_sp, omni_dfc_max_thrust, att, omni_proj_axes, att_sp); break; case 2: // Attitude is set to the fixed zero roll and pitch (used for omnidirectional vehicles) - thrustToZeroTiltAttitude(thr_sp, yaw_sp, att_sp); + thrustToZeroTiltAttitude(thr_sp, yaw_sp, att, omni_proj_axes, att_sp); break; case 3: { // Attitude is set to a fixed tilt at a fixed global direction (used for omnidirectional vehicles) float tilt_angle = math::radians(omni_att_tilt_angle); float tilt_dir = math::radians(omni_att_tilt_dir); - thrustToFixedTiltAttitude(thr_sp, yaw_sp, att, tilt_angle, tilt_dir, att_sp); + thrustToFixedTiltAttitude(thr_sp, yaw_sp, att, tilt_angle, tilt_dir, omni_proj_axes, att_sp); break; } case 4: { // Attitude is set to a fixed roll and pitch (used for omnidirectional vehicles) float roll_angle = math::radians(omni_att_roll); float pitch_angle = math::radians(omni_att_pitch); - thrustToFixedRollPitch(thr_sp, yaw_sp, att, roll_angle, pitch_angle, att_sp); + thrustToFixedRollPitch(thr_sp, yaw_sp, att, roll_angle, pitch_angle, omni_proj_axes, att_sp); break; } @@ -135,7 +136,8 @@ void bodyzToAttitude(Vector3f body_z, const float yaw_sp, vehicle_attitude_setpo att_sp.yaw_body = euler(2); } -void thrustToZeroTiltAttitude(const Vector3f &thr_sp, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp) +void thrustToZeroTiltAttitude(const Vector3f &thr_sp, const float yaw_sp, const matrix::Quatf &att, int omni_proj_axes, + vehicle_attitude_setpoint_s &att_sp) { // set Z axis to upward direction Vector3f body_z = Vector3f(0.f, 0.f, 1.f); @@ -163,13 +165,24 @@ void thrustToZeroTiltAttitude(const Vector3f &thr_sp, const float yaw_sp, vehicl att_sp.pitch_body = 0; att_sp.yaw_body = yaw_sp; + + if (omni_proj_axes == 1) { // if thrust is projected on the current attitude + matrix::Dcmf R_body = att; + + for (int i = 0; i < 3; i++) { + body_x(i) = R_body(i, 0); + body_y(i) = R_body(i, 1); + body_z(i) = R_body(i, 2); + } + } + att_sp.thrust_body[0] = thr_sp.dot(body_x); att_sp.thrust_body[1] = thr_sp.dot(body_y); - att_sp.thrust_body[2] = thr_sp(2); + att_sp.thrust_body[2] = thr_sp.dot(body_z); } void thrustToMinTiltAttitude(const Vector3f &thr_sp, const float yaw_sp, const float omni_dfc_max_thrust, - vehicle_attitude_setpoint_s &att_sp) + const matrix::Quatf &att, int omni_proj_axes, vehicle_attitude_setpoint_s &att_sp) { Vector3f body_z; float lambda = 0.f; // the minimum tilt angle @@ -183,7 +196,7 @@ void thrustToMinTiltAttitude(const Vector3f &thr_sp, const float yaw_sp, const f Vector2f thr_sp_h(thr_sp(0), thr_sp(1)); if (thr_sp_h.norm() <= omni_dfc_max_thrust) { - thrustToZeroTiltAttitude(thr_sp, yaw_sp, att_sp); + thrustToZeroTiltAttitude(thr_sp, yaw_sp, att, omni_proj_axes, att_sp); return; } @@ -267,7 +280,7 @@ void thrustToMinTiltAttitude(const Vector3f &thr_sp, const float yaw_sp, const f void thrustToFixedTiltAttitude(const Vector3f &thr_sp, const float yaw_sp, const matrix::Quatf &att, const float tilt_angle, const float tilt_dir, - vehicle_attitude_setpoint_s &att_sp) + int omni_proj_axes, vehicle_attitude_setpoint_s &att_sp) { Vector3f body_z; @@ -330,31 +343,43 @@ void thrustToFixedTiltAttitude(const Vector3f &thr_sp, const float yaw_sp, const att_sp.pitch_body = euler(1); att_sp.yaw_body = euler(2); - matrix::Dcmf R_curr = att; + if (omni_proj_axes == 1) { // if thrust is projected on the current attitude + matrix::Dcmf R_body = att; - Vector3f curr_x, curr_y, curr_z; - - for (int i = 0; i < 3; i++) { - curr_x(i) = R_curr(i, 0); - curr_y(i) = R_curr(i, 1); - curr_z(i) = R_curr(i, 2); + for (int i = 0; i < 3; i++) { + body_x(i) = R_body(i, 0); + body_y(i) = R_body(i, 1); + body_z(i) = R_body(i, 2); + } } - att_sp.thrust_body[0] = thr_sp.dot(curr_x); - att_sp.thrust_body[1] = thr_sp.dot(curr_y); - att_sp.thrust_body[2] = thr_sp.dot(curr_z); + att_sp.thrust_body[0] = thr_sp.dot(body_x); + att_sp.thrust_body[1] = thr_sp.dot(body_y); + att_sp.thrust_body[2] = thr_sp.dot(body_z); } void thrustToFixedRollPitch(const matrix::Vector3f &thr_sp, const float yaw_sp, const matrix::Quatf &att, - const float roll_angle, - const float pitch_angle, vehicle_attitude_setpoint_s &att_sp) + const float roll_angle, const float pitch_angle, int omni_proj_axes, vehicle_attitude_setpoint_s &att_sp) { Eulerf euler_cmd(roll_angle, pitch_angle, yaw_sp); Quatf q_sp = euler_cmd; q_sp.copyTo(att_sp.q_d); - matrix::Dcmf R_body = q_sp; + // calculate euler angles, for logging only, must not be used for control + att_sp.roll_body = roll_angle; + att_sp.pitch_body = pitch_angle; + att_sp.yaw_body = yaw_sp; + + matrix::Dcmf R_body; + + if (omni_proj_axes == 0) { // if thrust is projected onm the commanded attitude + R_body = q_sp; + + } else if (omni_proj_axes == 1) { // if thrust is projected on the current attitude + R_body = att; + } + Vector3f body_x, body_y, body_z; for (int i = 0; i < 3; i++) { @@ -363,24 +388,9 @@ void thrustToFixedRollPitch(const matrix::Vector3f &thr_sp, const float yaw_sp, body_z(i) = R_body(i, 2); } - // calculate euler angles, for logging only, must not be used for control - att_sp.roll_body = roll_angle; - att_sp.pitch_body = pitch_angle; - att_sp.yaw_body = yaw_sp; - - matrix::Dcmf R_curr = att; - - Vector3f curr_x, curr_y, curr_z; - - for (int i = 0; i < 3; i++) { - curr_x(i) = R_curr(i, 0); - curr_y(i) = R_curr(i, 1); - curr_z(i) = R_curr(i, 2); - } - - att_sp.thrust_body[0] = thr_sp.dot(curr_x); - att_sp.thrust_body[1] = thr_sp.dot(curr_y); - att_sp.thrust_body[2] = thr_sp.dot(curr_z); + att_sp.thrust_body[0] = thr_sp.dot(body_x); + att_sp.thrust_body[1] = thr_sp.dot(body_y); + att_sp.thrust_body[2] = thr_sp.dot(body_z); } Vector2f constrainXY(const Vector2f &v0, const Vector2f &v1, const float &max) diff --git a/src/modules/mc_pos_control/PositionControl/ControlMath.hpp b/src/modules/mc_pos_control/PositionControl/ControlMath.hpp index 6d1fc9eb24..8f186bf673 100644 --- a/src/modules/mc_pos_control/PositionControl/ControlMath.hpp +++ b/src/modules/mc_pos_control/PositionControl/ControlMath.hpp @@ -60,7 +60,7 @@ namespace ControlMath */ void thrustToAttitude(const matrix::Vector3f &thr_sp, const float yaw_sp, const matrix::Quatf &att, const int omni_att_mode, const float omni_dfc_max_thrust, float &omni_att_tilt_angle, float &omni_att_tilt_dir, - float &omni_att_roll, float &omni_att_pitch, vehicle_attitude_setpoint_s &att_sp); + float &omni_att_roll, float &omni_att_pitch, int omni_proj_axes, vehicle_attitude_setpoint_s &att_sp); /** * Converts a body z vector and yaw set-point to a desired attitude. * @param body_z a world frame 3D vector in direction of the desired body z axis @@ -73,42 +73,49 @@ void bodyzToAttitude(matrix::Vector3f body_z, const float yaw_sp, vehicle_attitu * Converts thrust vector and yaw set-point to a zero-tilt attitude for an omni-directional multirotor. * @param thr_sp a 3D vector * @param yaw_sp the desired yaw + * @param att current attitude of the robot + * @param omni_proj_axes the axes used for thrust projection (0=calculated, 1=current) * @param att_sp attitude setpoint to fill */ -void thrustToZeroTiltAttitude(const matrix::Vector3f &thr_sp, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp); +void thrustToZeroTiltAttitude(const matrix::Vector3f &thr_sp, const float yaw_sp, const matrix::Quatf &att, + int omni_proj_axes, vehicle_attitude_setpoint_s &att_sp); /** * Converts thrust vector and yaw set-point to a minimum-tilt attitude for an omni-directional multirotor. * @param thr_sp a 3D vector * @param yaw_sp the desired yaw * @param omni_dfc_max_thrust maximum direct-force (horizontal) scaled thrust for omnidirectional vehicles + * @param att current attitude of the robot + * @param omni_proj_axes the axes used for thrust projection (0=calculated, 1=current) * @param att_sp attitude setpoint to fill */ void thrustToMinTiltAttitude(const matrix::Vector3f &thr_sp, const float yaw_sp, const float omni_dfc_max_thrust, - vehicle_attitude_setpoint_s &att_sp); + const matrix::Quatf &att, int omni_proj_axes, vehicle_attitude_setpoint_s &att_sp); /** * Converts thrust vector and yaw set-point to a desired-tilt attitude for an omni-directional multirotor. * @param thr_sp a 3D vector * @param yaw_sp the desired yaw + * @param att current attitude of the robot * @param tilt_angle the desired tilt angle * @param tilt_dir the desired tilt direction + * @param omni_proj_axes the axes used for thrust projection (0=calculated, 1=current) * @param att_sp attitude setpoint to fill */ void thrustToFixedTiltAttitude(const matrix::Vector3f &thr_sp, const float yaw_sp, const matrix::Quatf &att, - const float tilt_angle, - const float tilt_dir, vehicle_attitude_setpoint_s &att_sp); + const float tilt_angle, const float tilt_dir, int omni_proj_axes, vehicle_attitude_setpoint_s &att_sp); /** * Converts thrust vector and yaw set-point to a desired given attitude for an omni-directional multirotor. * @param thr_sp a 3D vector * @param yaw_sp the desired yaw + * @param att current attitude of the robot * @param roll_angle the desired roll angle * @param pitch_angle the desired pitch angle + * @param omni_proj_axes the axes used for thrust projection (0=calculated, 1=current) * @param att_sp attitude setpoint to fill */ void thrustToFixedRollPitch(const matrix::Vector3f &thr_sp, const float yaw_sp, const matrix::Quatf &att, - const float roll_angle, - const float pitch_angle, vehicle_attitude_setpoint_s &att_sp); + const float roll_angle, const float pitch_angle, int omni_proj_axes, vehicle_attitude_setpoint_s &att_sp); /** * Outputs the sum of two vectors but respecting the limits and priority. diff --git a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp index 8dc81abfcd..1acc1a732a 100644 --- a/src/modules/mc_pos_control/PositionControl/PositionControl.cpp +++ b/src/modules/mc_pos_control/PositionControl/PositionControl.cpp @@ -366,6 +366,6 @@ void PositionControl::getAttitudeSetpoint(const matrix::Quatf &att, const int om float &omni_att_pitch, vehicle_attitude_setpoint_s &attitude_setpoint) const { ControlMath::thrustToAttitude(_thr_sp, _yaw_sp, att, omni_att_mode, omni_dfc_max_thrust, omni_att_tilt_angle, - omni_att_tilt_dir, omni_att_roll, omni_att_pitch, attitude_setpoint); + omni_att_tilt_dir, omni_att_roll, omni_att_pitch, 1, attitude_setpoint); attitude_setpoint.yaw_sp_move_rate = _yawspeed_sp; }