Skip to content

Commit db5740c

Browse files
pockermanpockerman
authored
Merge pull request #143 from pockerman/141_fix_bug_with_quadrotor_dynamics
141 fix bug with quadrotor dynamics
2 parents 3237005 + a201e95 commit db5740c

File tree

6 files changed

+94
-41
lines changed

6 files changed

+94
-41
lines changed

‎examples/example_10/example_10.cpp‎

Lines changed: 41 additions & 17 deletions
Original file line numberDiff line numberDiff line change
@@ -2,9 +2,6 @@
22
* In this example we simulate a quadrotor.
33
* The quadrotor data is taken from
44
* https://sal.aalto.fi/publications/pdf-files/eluu11_public.pdf
5-
*
6-
*
7-
*
85
*/
96

107
#include "rlenvs/rlenvs_types_v2.h"
@@ -27,23 +24,46 @@ namespace example_10{
2724
using rlenvscpp::RealVec;
2825
using rlenvscpp::utils::io::CSVWriter;
2926

30-
real_t compute_motor_speed(real_t t){
27+
RealVec compute_motor_speed(real_t t){
28+
29+
30+
RealVec w(4);
3131

3232
if(t <= 0.1){
33-
return 500 * t + 625.0;
33+
34+
w[0] = 500 * t + 625.0;
35+
w[1] = 500 * t + 625.0;
36+
w[2] = 500 * t + 625.0;
37+
w[3] = 500 * t + 625.0;
38+
39+
return w;
3440
}
3541
else if( t > 0.1 && t <= 0.4){
36-
return -416.66 * t + 716.66;
42+
43+
w[0] = -416.66 * t + 716.66;
44+
w[1] = -416.66 * t + 716.66;
45+
w[2] = -416.66 * t + 716.66;
46+
w[3] = -416.66 * t + 716.66;
47+
48+
return w;
3749
}
3850
else if( t > 0.4 && t <= 0.5){
39-
return 750.0 * t + 250.0;
51+
52+
w[0] = 750.0 * t + 250.0;
53+
w[1] = 750.0 * t + 250.0;
54+
w[2] = 750.0 * t + 250.0;
55+
w[3] = 750.0 * t + 250.0;
56+
return w;
4057
}
4158
else{
42-
return 625.0;
59+
60+
w[0] = 625.0;
61+
w[1] = 625.0;
62+
w[2] = 625.0;
63+
w[3] = 625.0;
64+
return w;
4365
}
44-
4566
}
46-
4767
}
4868

4969

@@ -75,7 +95,7 @@ int main(){
7595
values[4] = std::make_pair("v", 0.0);
7696
values[5] = std::make_pair("w", 0.0);
7797

78-
// initial rotational velocities body coords
98+
// initial rotational velocities
7999
values[6] = std::make_pair("p", 0.0);
80100
values[7] = std::make_pair("q", 0.0);
81101
values[8] = std::make_pair("r", 0.0);
@@ -101,21 +121,21 @@ int main(){
101121
"x", "y", "z",
102122
"u", "v", "w",
103123
"p", "q", "r",
104-
"phi", "theta", "psi"};
124+
"phi", "theta", "psi"
125+
"w0", "w1", "w2", "w3"};
105126

106127
csv_writer.write_column_names(names);
107128

108129
const real_t T = 2.0;
109130
real_t time = 0.0;
110131

111-
std::vector<real_t> row(13, 0.0);
132+
std::vector<real_t> row(17, 0.0);
112133
while(time < T){
113134

114135
std::cout<<"Time: "<<time<<std::endl;
115136

116137
auto omega_motor = compute_motor_speed(time);
117-
omegas[0] = omegas[1] = omegas[2] = omegas[3] = omega_motor;
118-
dynamics.integrate(omegas);
138+
dynamics.integrate(omega_motor);
119139

120140
auto p = dynamics.get_position();
121141
auto v = dynamics.get_velocity();
@@ -125,7 +145,7 @@ int main(){
125145
row[0] = time;
126146
row[1] = p[0];
127147
row[2] = p[1];
128-
row[3] = p[2];
148+
row[3] = -p[2];
129149

130150
row[4] = v[0];
131151
row[5] = v[1];
@@ -139,10 +159,14 @@ int main(){
139159
row[11] = rlenvscpp::utils::unit_converter::rad_to_degrees(euler[1]);
140160
row[12] = rlenvscpp::utils::unit_converter::rad_to_degrees(euler[2]);
141161

162+
row[13] = omega_motor[0];
163+
row[14] = omega_motor[1];
164+
row[15] = omega_motor[2];
165+
row[16] = omega_motor[3];
166+
142167
csv_writer.write_row(row);
143168
std::cout<<"Current position: ";
144169
std::cout<<p<<std::endl;
145-
std::cout<<euler<<std::endl;
146170
time += config.dt;
147171
}
148172

‎examples/example_10/plot.py‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -29,7 +29,7 @@ def main(filename):
2929
if not row[0].startswith('#'):
3030
try:
3131

32-
assert len(row) == 13
32+
assert len(row) == 17
3333

3434
time.append(float(row[0]))
3535
x.append(float(row[1]))

‎src/rlenvs/dynamics/quadrotor_dynamics.cpp‎

Lines changed: 30 additions & 18 deletions
Original file line numberDiff line numberDiff line change
@@ -5,6 +5,9 @@
55

66
namespace rlenvscpp{
77
namespace dynamics {
8+
9+
10+
811

912

1013
QuadrotorDynamics::QuadrotorDynamics(QuadrotorDynamicsConfig config,
@@ -97,25 +100,25 @@ QuadrotorDynamics::update_rotation_matrix_(){
97100
const auto psi = euler_angles_old[2];
98101

99102

100-
auto ctheta = std::cos(theta);
101-
auto cpsi = std::cos(psi);
102-
auto cphi = std::cos(phi);
103+
auto c_theta = std::cos(theta);
104+
auto c_psi = std::cos(psi);
105+
auto c_phi = std::cos(phi);
103106

104-
auto stheta = std::sin(theta);
105-
auto spsi = std::sin(psi);
106-
auto sphi = std::sin(phi);
107+
auto s_theta = std::sin(theta);
108+
auto s_psi = std::sin(psi);
109+
auto s_phi = std::sin(phi);
107110

108-
rotation_mat_(0,0) = ctheta * cpsi;
109-
rotation_mat_(0,1) = spsi * stheta * cpsi - cphi * spsi;
110-
rotation_mat_(0,2) = cphi * stheta * cpsi + sphi * spsi;
111+
rotation_mat_(0,0) = c_theta * c_psi;
112+
rotation_mat_(0,1) = s_psi * s_theta * c_psi - c_phi * s_psi;
113+
rotation_mat_(0,2) = c_phi * s_theta * c_psi + s_phi * s_psi;
111114

112-
rotation_mat_(1,0) = ctheta * spsi;
113-
rotation_mat_(1,1) = sphi * stheta * spsi + cphi * cpsi;
114-
rotation_mat_(1,2) = cphi * stheta * spsi - sphi * cpsi;
115+
rotation_mat_(1,0) = c_theta * s_psi;
116+
rotation_mat_(1,1) = s_phi * s_theta * s_psi + c_phi * c_psi;
117+
rotation_mat_(1,2) = c_phi * s_theta * s_psi - s_phi * c_psi;
115118

116-
rotation_mat_(0,0) = stheta;
117-
rotation_mat_(0,1) = -sphi * ctheta;
118-
rotation_mat_(0,2) = -cphi * ctheta;
119+
rotation_mat_(2,0) = s_theta;
120+
rotation_mat_(2,1) = -s_phi * c_theta;
121+
rotation_mat_(2,2) = -c_phi * c_theta;
119122
}
120123

121124
void
@@ -179,7 +182,14 @@ QuadrotorDynamics::translational_dynamics(const RealVec& motor_w){
179182
RealColVec3d fg = RealColVec3d::Zero(3);
180183

181184
if(config_.use_gravity){
182-
fg[2] = mass * rlenvscpp::consts::maths::G;
185+
186+
const auto phi_ = phi();
187+
const auto theta_ = theta();
188+
189+
// we do not contain the mass constant so don't use it below
190+
fg[0] = -rlenvscpp::consts::maths::G * std::sin(theta_);
191+
fg[1] = rlenvscpp::consts::maths::G * std::cos(theta_) * std::sin(phi_);
192+
fg[2] = rlenvscpp::consts::maths::G * std::cos(theta_) * std::cos(phi_);
183193
}
184194

185195
// compute the total thurst force
@@ -192,8 +202,10 @@ QuadrotorDynamics::translational_dynamics(const RealVec& motor_w){
192202
// form the vector ωb/i × v
193203
RealColVec3d omega_cross_v = old_omega.cross(old_v);
194204

195-
// compute the velocity increment
196-
RealColVec3d v = dt * omega_cross_v + (1.0 / mass ) * dt * fg - (1.0 / mass ) * dt * ft - old_v;
205+
// compute the velocity increment.
206+
// Note that the mass is dropped when accounting for gravity
207+
// as fg does not contain it
208+
RealColVec3d v = dt * omega_cross_v + dt * fg - (1.0 / mass ) * dt * ft - old_v;
197209

198210
// compute the time velocity derivative
199211
v_dot_ = (v - old_v) / dt;

‎src/rlenvs/dynamics/quadrotor_dynamics.h‎

Lines changed: 18 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -158,7 +158,9 @@ class QuadrotorDynamics: public MotionModelDynamicsBase<SysState<12>,
158158
void rotational_dynamics(const RealVec& motor_w);
159159

160160
///
161-
/// \brief Returns the current position NED frame
161+
/// \brief Returns the current position NED frame.
162+
/// Note that the z-coordinate has to be multiplied with -1
163+
/// in order to represent an upwards looking z-axis
162164
///
163165
RealColVec3d get_position()const{return get_position_from_state_();}
164166

@@ -177,6 +179,21 @@ class QuadrotorDynamics: public MotionModelDynamicsBase<SysState<12>,
177179
///
178180
RealColVec3d get_euler_angles()const{return get_euler_angles_from_state_();}
179181

182+
///
183+
/// \brief The phi Euler angle in rad
184+
///
185+
real_t phi()const noexcept {return get_euler_angles_from_state_()[0];}
186+
187+
///
188+
/// \brief The theta Euler angle in rad
189+
///
190+
real_t theta()const noexcept {return get_euler_angles_from_state_()[1];}
191+
192+
///
193+
/// \brief The psi Euler angle in rad
194+
///
195+
real_t psi()const noexcept {return get_euler_angles_from_state_()[2];}
196+
180197
private:
181198

182199
///

‎src/rlenvs/rlenvscpp_config.h‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -11,7 +11,7 @@
1111
/* #undef RLENVSCPP_WEBOTS */
1212

1313
/*Use Ray*/
14-
#define RLENVSCPP_RAY
14+
/* #undef RLENVSCPP_RAY */
1515

1616
#endif
1717

‎src/rlenvs/rlenvscpp_version.h‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -2,7 +2,7 @@
22
#define GYMFCPP_VERSION_H
33

44
#define RLENVSCPP_VERSION_MAJOR 1
5-
#define RLENVSCPP_VERSION_MINOR 10
6-
#define RLENVSCPP_VERSION_PATCH 1
7-
#define RLENVSCPP_VERSION "1.10.1"
5+
#define RLENVSCPP_VERSION_MINOR 11
6+
#define RLENVSCPP_VERSION_PATCH 0
7+
#define RLENVSCPP_VERSION "1.11.0"
88
#endif

0 commit comments

Comments
 (0)