1 /***************************************************************************** 2 * Copyright (C) 2004-2018 The pykep development team, * 3 * Advanced Concepts Team (ACT), European Space Agency (ESA) * 4 * * 5 * https://gitter.im/esa/pykep * 6 * https://github.com/esa/pykep * 7 * * 8 * act@esa.int * 9 * * 10 * This program is free software; you can redistribute it and/or modify * 11 * it under the terms of the GNU General Public License as published by * 12 * the Free Software Foundation; either version 2 of the License, or * 13 * (at your option) any later version. * 14 * * 15 * This program is distributed in the hope that it will be useful, * 16 * but WITHOUT ANY WARRANTY; without even the implied warranty of * 17 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * 18 * GNU General Public License for more details. * 19 * * 20 * You should have received a copy of the GNU General Public License * 21 * along with this program; if not, write to the * 22 * Free Software Foundation, Inc., * 23 * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * 24 *****************************************************************************/ 25 26 #ifndef KEP_TOOLBOX_LEG_S_H 27 #define KEP_TOOLBOX_LEG_S_H 28 29 #include <boost/type_traits/is_same.hpp> 30 #include <boost/utility.hpp> 31 #include <iterator> 32 #include <vector> 33 34 #include <keplerian_toolbox/core_functions/array3D_operations.hpp> 35 #include <keplerian_toolbox/core_functions/propagate_taylor_s.hpp> 36 #include <keplerian_toolbox/epoch.hpp> 37 #include <keplerian_toolbox/exceptions.hpp> 38 #include <keplerian_toolbox/sims_flanagan/sc_state.hpp> 39 #include <keplerian_toolbox/sims_flanagan/spacecraft.hpp> 40 #include <keplerian_toolbox/sims_flanagan/throttle.hpp> 41 42 #include <keplerian_toolbox/detail/visibility.hpp> 43 #include <keplerian_toolbox/serialization.hpp> 44 45 namespace kep_toolbox 46 { 47 namespace sims_flanagan 48 { 49 50 /// Single low-thrust leg (phase) using Sundmann variable 51 /** 52 * This class represents, generically, a low-thrust leg (phase) as a sequence of 53 * successive 54 * constant low-thrust segments. The segment duration is equally distributed in 55 * the pseudo 56 * time space \f$dt = c r^\alpha ds \f$ 57 * The leg achieves to transfer a given spacecraft from an initial to a final 58 * state in the 59 * pseudo-time given (and can thus be considered as feasible) whenever the method 60 * evaluate_mismatch 61 * returns all zeros (8 values) and the method get_throttles_con returns all 62 * values less than zero. 63 * Th sequence of different thrusts is represented by the class throttles. These 64 * represent 65 * the cartesian components \f$ \mathbf u = (u_x,u_y,u_y) \f$ of a normalized 66 * thrust and are thus 67 * numbers that need to satisfy the constraint \f$|\mathbf u| \le 1\f$ 68 * 69 * \image html s_leg.png "Visualization of a feasible leg (Earth-Mars)" 70 * \image latex s_leg.png "Visualization of a feasible leg (Earth-Mars)" 71 * width=5cm 72 * 73 * @author Dario Izzo (dario.izzo _AT_ googlemail.com) 74 */ 75 class KEP_TOOLBOX_DLL_PUBLIC leg_s 76 { 77 friend std::ostream &operator<<(std::ostream &s, const leg_s &in); 78 79 public: 80 std::string human_readable() const; 81 /// Constructor. 82 /** 83 * Default constructor. Constructs a meaningless leg. 84 */ leg_s()85 leg_s() 86 : m_ti(), m_xi(), m_throttles(), m_tf(), m_xf(), m_sf(0), m_sc(), m_mu(0), m_c(), m_alpha(), m_tol(-10), 87 m_states(), m_ceq(), m_cineq(), m_dv() 88 { 89 } 90 91 /// Constructor 92 /** 93 * Constructs an empty leg allocating memory for a given number of segments. 94 */ leg_s(unsigned n_seg,double c,double alpha,int tol=-10)95 leg_s(unsigned n_seg, double c, double alpha, int tol = -10) 96 : m_ti(), m_xi(), m_throttles(n_seg), m_tf(), m_xf(), m_sf(0), m_sc(), m_mu(), m_c(c), m_alpha(alpha), 97 m_tol(tol), m_states(n_seg + 2), m_ceq(), m_cineq(n_seg), m_dv(n_seg) 98 { 99 } 100 101 /// Sets the leg's data 102 /** 103 * The throttles are provided via two iterators pointing to the beginning and 104 to the end of 105 * a sequence of doubles (\f$ x_1,y_1,z_1, ..., x_N,y_N,z_N \f$ containing the 106 * cartesian components of each throttle \f$ x_i,y_i,z_i \in [0,1]\f$. The 107 constructed leg 108 * will have equally spaced segments in the pseudo-time 109 * 110 * \param[in] epoch_i Inital epoch 111 * \param[in] state_i Initial sc_state (spacecraft state) 112 * \param[in] throttles_start iterator pointing to the beginning of a cartesian 113 throttle sequence. 114 * \param[in] throttles_end iterator pointing to the end+1 of a cartesian 115 throttle sequence. 116 * \param[in] epoch_f Final epoch. Needs to be later than epoch_i 117 * \param[in] state_f Final sc_state (spacecraft state) 118 * \param[in] s_f Pseudo time of transfer 119 * \param[in] mu_ Primary body gravitational constant 120 * \param[in] sc_ Spacecraft 121 * 122 & \throws value_error if final epoch is before initial epoch, if mu_ not 123 positive if the throttle size is not consistent 124 */ 125 template <typename it_type> 126 void set_leg(const epoch & epoch_i,const sc_state & state_i,it_type throttles_start,it_type throttles_end,const epoch & epoch_f,const sc_state & state_f,const double & sf,const spacecraft & sc_,const double & mu_,typename boost::enable_if<boost::is_same<typename std::iterator_traits<it_type>::value_type,double>>::type * =0)127 set_leg(const epoch &epoch_i, const sc_state &state_i, it_type throttles_start, it_type throttles_end, 128 const epoch &epoch_f, const sc_state &state_f, const double &sf, const spacecraft &sc_, const double &mu_, 129 typename boost::enable_if<boost::is_same<typename std::iterator_traits<it_type>::value_type, double>>::type 130 * = 0) 131 { 132 // We check data consistency 133 if (std::distance(throttles_start, throttles_end) % 3) { 134 throw_value_error("The length of the throttles list must be a multiple of 3"); 135 } 136 if (std::distance(throttles_start, throttles_end) / 3 != (int)m_throttles.size()) { 137 throw_value_error("The number of segments in the leg do not match the " 138 "length of the supplied throttle sequence"); 139 } 140 if (epoch_i.mjd2000() >= epoch_f.mjd2000()) { 141 throw_value_error("Final epoch must be strictly after initial epoch"); 142 } 143 144 if (mu_ <= 0) { 145 throw_value_error("Gravity parameter must be larger than zero (forgot to set it?)"); 146 } 147 if (epoch_i.mjd() >= epoch_f.mjd()) { 148 throw_value_error("Final epoch must be after the initial epoch"); 149 } 150 if (sc_.get_mass() == 0) { 151 throw_value_error("Spacecraft mass must be larger than zero (forgot to set it?)"); 152 } 153 154 // We fill up all leg's data member 155 m_mu = mu_; 156 m_sc = sc_; 157 m_ti = epoch_i; 158 m_xi = state_i; 159 m_tf = epoch_f; 160 m_xf = state_f; 161 m_sf = sf; 162 163 // note: the epochs of the throttles are meaningless at this point as 164 // pseudo-time is used 165 for (decltype(m_throttles.size()) i = 0; i < m_throttles.size(); ++i) { 166 kep_toolbox::array3D tmp 167 = {{*(throttles_start + 3 * i), *(throttles_start + 3 * i + 1), *(throttles_start + 3 * i + 2)}}; 168 m_throttles[i] = throttle(epoch(0.), epoch(1.), tmp); 169 } 170 } 171 172 /// Sets the leg's data set_leg(const epoch & epoch_i,const sc_state & state_i,const std::vector<double> & thrott,const epoch & epoch_f,const sc_state & state_f,const double & sf)173 void set_leg(const epoch &epoch_i, const sc_state &state_i, const std::vector<double> &thrott, const epoch &epoch_f, 174 const sc_state &state_f, const double &sf) 175 { 176 set_leg(epoch_i, state_i, thrott.begin(), thrott.end(), epoch_f, state_f, sf, m_sc, m_mu); 177 } 178 179 /// Sets the leg's data set_leg(const epoch & epoch_i,const sc_state & state_i,const std::vector<double> & thrott,const epoch & epoch_f,const sc_state & state_f,const double & sf,const spacecraft & sc_,const double & mu_)180 void set_leg(const epoch &epoch_i, const sc_state &state_i, const std::vector<double> &thrott, const epoch &epoch_f, 181 const sc_state &state_f, const double &sf, const spacecraft &sc_, const double &mu_) 182 { 183 set_leg(epoch_i, state_i, thrott.begin(), thrott.end(), epoch_f, state_f, sf, sc_, mu_); 184 } 185 186 /** @name Setters*/ 187 //@{ 188 189 /// Sets the leg's spacecraft 190 /** 191 * 192 * In order for the trajectory leg to be able to propagate the states, 193 * information on the 194 * low-thrust propulsion system used needs to be available. This is provided by 195 * the object 196 * spacecraft private member of the class and can be set using this setter. 197 * 198 * \param[in] sc The spacecraft object 199 */ set_sc(const spacecraft & sc)200 void set_sc(const spacecraft &sc) 201 { 202 m_sc = sc; 203 } 204 205 /// Sets the leg's primary body gravitational parameter 206 /** 207 * 208 * Sets the leg's central body gravitational parameter 209 * 210 * \param[in] mu_ The gravitational parameter 211 */ set_mu(const double & mu_)212 void set_mu(const double &mu_) 213 { 214 m_mu = mu_; 215 } 216 217 /** @name Getters*/ 218 //@{ 219 220 /// Gets the leg's spacecraft 221 /** 222 * Returns the spacecraft 223 * 224 * @return sc const reference to spacecraft object 225 */ get_spacecraft() const226 const spacecraft &get_spacecraft() const 227 { 228 return m_sc; 229 } 230 231 /// Gets the gravitational parameter 232 /** 233 * @return the gravitational parameter 234 */ get_mu() const235 double get_mu() const 236 { 237 return m_mu; 238 } 239 240 /// Gets the leg' s number of segments 241 /** 242 * Returns the leg' s number of segments 243 * 244 * @return size_t the leg's number of segments 245 */ get_n_seg() const246 size_t get_n_seg() const 247 { 248 return m_throttles.size(); 249 } 250 251 /// Gets the leg's initial epoch 252 /** 253 * Gets the epoch at the beginning of the leg 254 * 255 * @return const reference to the initial epoch 256 */ get_ti() const257 const epoch &get_ti() const 258 { 259 return m_ti; 260 } 261 262 /// Gets the leg's final epoch 263 /** 264 * Gets the epoch at the end of the leg 265 * 266 * @return const reference to the final epoch 267 */ get_tf() const268 const epoch &get_tf() const 269 { 270 return m_tf; 271 } 272 273 /// Gets the sc_state at the end of the leg 274 /** 275 * Gets the spacecraft state at the end of the leg 276 * 277 * @return const reference to the final sc_state 278 */ get_xf() const279 const sc_state &get_xf() const 280 { 281 return m_xf; 282 } 283 284 /// Gets the initial sc_state 285 /** 286 * Gets the spacecraft state at the beginning of the leg 287 * 288 * @return const reference to the initial sc_state 289 */ get_xi() const290 const sc_state &get_xi() const 291 { 292 return m_xi; 293 } 294 //@} 295 296 /** @name Computations*/ 297 //@{ 298 299 /// Returns the computed state mismatch constraints (8 equality constraints) 300 /** 301 * The main trajectory computations are made here. The method propagates the 302 * leg's throttle sequence 303 * to return the 8-mismatch constraints (leg with the Sundmann variable). For 304 * efficiency purposes the method 305 * does not store any of the computed intermediate steps. If those are also 306 * needed use the slower method 307 * compute_states. 308 * 309 * @return const reference to the m_ceq data member that contains, after the 310 * call to this method, the 8 mismatches 311 */ 312 compute_mismatch_con() const313 const std::array<double, 8> &compute_mismatch_con() const 314 { 315 auto n_seg = m_throttles.size(); 316 auto n_seg_fwd = (n_seg + 1) / 2, n_seg_back = n_seg / 2; 317 318 // Aux variables 319 double max_thrust = m_sc.get_thrust(); 320 double veff = m_sc.get_isp() * ASTRO_G0; 321 array3D thrust; 322 double ds = m_sf / static_cast<double>(n_seg); // pseudo-time interval for each segment 323 double dt = (m_tf.mjd2000() - m_ti.mjd2000()) * ASTRO_DAY2SEC; // length of the leg in seconds 324 325 // Initial state 326 array3D rfwd = m_xi.get_position(); 327 array3D vfwd = m_xi.get_velocity(); 328 double mfwd = m_xi.get_mass(); 329 double tfwd = 0; 330 331 // Forward Propagation 332 for (decltype(n_seg_fwd) i = 0u; i < n_seg_fwd; i++) { 333 for (int j = 0; j < 3; j++) { 334 thrust[j] = max_thrust * m_throttles[i].get_value()[j]; 335 } 336 propagate_taylor_s(rfwd, vfwd, mfwd, tfwd, thrust, ds, m_mu, veff, m_c, m_alpha, m_tol, m_tol); 337 } 338 339 // Final state 340 array3D rback = m_xf.get_position(); 341 array3D vback = m_xf.get_velocity(); 342 double mback = m_xf.get_mass(); 343 double tback = 0; 344 345 // Backward Propagation 346 for (decltype(n_seg_back) i = 0u; i < n_seg_back; ++i) { 347 for (unsigned j = 0u; j < 3u; ++j) { 348 thrust[j] = max_thrust * m_throttles[m_throttles.size() - i - 1].get_value()[j]; 349 } 350 propagate_taylor_s(rback, vback, mback, tback, thrust, -ds, m_mu, veff, m_c, m_alpha, m_tol, m_tol); 351 } 352 353 // Return the mismatch 354 diff(rfwd, rfwd, rback); 355 diff(vfwd, vfwd, vback); 356 357 std::copy(rfwd.begin(), rfwd.end(), m_ceq.begin()); 358 std::copy(vfwd.begin(), vfwd.end(), m_ceq.begin() + 3); 359 m_ceq[6] = mfwd - mback; 360 m_ceq[7] = (tfwd - tback - dt); 361 return m_ceq; 362 } 363 /// Returns the computed throttles constraints (n_seg inequality constraints) 364 /** 365 * Computes the throttles constraints frm the throttle vector performing a 366 * fairly straight forward computation 367 * 368 * @return const reference to the m_ceq data member that contains, after the 369 * call to this method, the 8 mismatches 370 */ compute_throttles_con() const371 const std::vector<double> &compute_throttles_con() const 372 { 373 for (size_t i = 0; i < m_throttles.size(); ++i) { 374 const array3D &t = m_throttles[i].get_value(); 375 m_cineq[i] = std::inner_product(t.begin(), t.end(), t.begin(), -1.); 376 } 377 return m_cineq; 378 } 379 /** 380 * The main trajectory computations are made here. Less efficiently than in 381 * compute_mismatch_con, but storing 382 * more info on the way. The method propagates the leg's throttle sequence to 383 * return the spacecraft state at each time. 384 * The code is redundand with compute_mismatch_con, the choice to have it 385 * redundand stems from the need to have 386 * compute_mismatch_con method to be as efficient as possible, while 387 * compute_states can also be unefficient. 388 * 389 * @return a (n_seg + 2) vector of 8-dim arrays containing t,x,y,z,vx,vy,vz,m 390 */ compute_states() const391 const std::vector<std::array<double, 11>> &compute_states() const 392 { 393 size_t n_seg = m_throttles.size(); 394 auto n_seg_fwd = (n_seg + 1) / 2, n_seg_back = n_seg / 2; 395 396 // Aux variables 397 double max_thrust = m_sc.get_thrust(); 398 double veff = m_sc.get_isp() * ASTRO_G0; 399 array3D thrust = {{0, 0, 0}}; 400 array3D zeros = {{0, 0, 0}}; 401 double ds = m_sf / static_cast<double>(n_seg); // pseudo-time interval for each segment 402 double dt = (m_tf.mjd2000() - m_ti.mjd2000()) * ASTRO_DAY2SEC; // length of the leg in seconds 403 404 // Initial state 405 array3D rfwd = m_xi.get_position(); 406 array3D vfwd = m_xi.get_velocity(); 407 double mfwd = m_xi.get_mass(); 408 double tfwd = 0; 409 410 record_states(tfwd, rfwd, vfwd, mfwd, zeros, 0); 411 412 // Forward Propagation 413 for (decltype(n_seg_fwd) i = 0u; i < n_seg_fwd; ++i) { 414 for (unsigned j = 0u; j < 3u; ++j) { 415 thrust[j] = max_thrust * m_throttles[i].get_value()[j]; 416 } 417 try { 418 propagate_taylor_s(rfwd, vfwd, mfwd, tfwd, thrust, ds, m_mu, veff, m_c, m_alpha, m_tol, m_tol); 419 } catch (...) { 420 throw_value_error("Could not compute the states ... check your data!!!"); 421 } 422 record_states(tfwd, rfwd, vfwd, mfwd, thrust, i + 1); 423 } 424 425 // Final state 426 array3D rback = m_xf.get_position(); 427 array3D vback = m_xf.get_velocity(); 428 double mback = m_xf.get_mass(); 429 double tback = 0; 430 431 record_states(dt + tback, rback, vback, mback, zeros, n_seg + 1); 432 433 // Backward Propagation 434 for (decltype(n_seg_back) i = 0u; i < n_seg_back; ++i) { 435 for (unsigned j = 0u; j < 3u; ++j) { 436 thrust[j] = max_thrust * m_throttles[m_throttles.size() - i - 1].get_value()[j]; 437 } 438 try { 439 propagate_taylor_s(rback, vback, mback, tback, thrust, -ds, m_mu, veff, m_c, m_alpha, m_tol, m_tol); 440 } catch (...) { 441 throw_value_error("Could not compute the states ... check your data!!!"); 442 } 443 record_states(dt + tback, rback, vback, mback, thrust, n_seg - i); 444 } 445 // Return the states 446 return m_states; 447 } 448 449 // const std::vector<double>& compute_dvs() const { return m_dv; } get_throttles()450 const std::vector<throttle> &get_throttles() 451 { 452 size_t n_seg = m_throttles.size(); 453 const size_t n_seg_fwd = (n_seg + 1) / 2; 454 std::vector<std::array<double, 11>> states; 455 states = compute_states(); 456 for (size_t i = 0; i < n_seg_fwd; ++i) { 457 m_throttles[i].set_start(epoch(states[i][0] * ASTRO_SEC2DAY + m_ti.mjd2000())); 458 m_throttles[i].set_end(epoch(states[i + 1][0] * ASTRO_SEC2DAY + m_ti.mjd2000())); 459 } 460 461 for (size_t i = n_seg_fwd; i < n_seg; ++i) { 462 m_throttles[i].set_start(epoch(states[i + 1][0] * ASTRO_SEC2DAY + m_ti.mjd2000())); 463 m_throttles[i].set_end(epoch(states[i + 2][0] * ASTRO_SEC2DAY + m_ti.mjd2000())); 464 } 465 return m_throttles; 466 } 467 //@} 468 469 protected: record_states(double t,const array3D & r,const array3D & v,double m,const array3D & thrust,size_t idx) const470 void record_states(double t, const array3D &r, const array3D &v, double m, const array3D &thrust, 471 size_t idx) const 472 { 473 assert((idx + 1) < m_states.size()); 474 m_states[idx][0] = t; 475 std::copy(r.begin(), r.end(), m_states[idx].begin() + 1); 476 std::copy(v.begin(), v.end(), m_states[idx].begin() + 4); 477 m_states[idx][7] = m; 478 std::copy(thrust.begin(), thrust.end(), m_states[idx].begin() + 8); 479 } 480 481 private: 482 friend class boost::serialization::access; 483 template <class Archive> serialize(Archive & ar,const unsigned int)484 void serialize(Archive &ar, const unsigned int) 485 { 486 ar &m_ti; 487 ar &m_xi; 488 ar &m_throttles; 489 ar &m_tf; 490 ar &m_xf; 491 ar &m_sf; 492 ar &m_sc; 493 ar &m_mu; 494 ar &m_c; 495 ar &m_alpha; 496 ar &m_tol; 497 ar &m_states; 498 ar &m_ceq; 499 ar &m_cineq; 500 ar &m_dv; 501 } 502 503 epoch m_ti; 504 sc_state m_xi; 505 std::vector<throttle> m_throttles; 506 epoch m_tf; 507 sc_state m_xf; 508 double m_sf; 509 spacecraft m_sc; 510 double m_mu; 511 double m_c; 512 double m_alpha; 513 int m_tol; 514 515 mutable std::vector<std::array<double, 11>> m_states; 516 mutable std::array<double, 8> m_ceq; 517 mutable std::vector<double> m_cineq; 518 mutable std::vector<double> m_dv; 519 }; 520 521 KEP_TOOLBOX_DLL_PUBLIC std::ostream &operator<<(std::ostream &s, const leg_s &in); 522 } // namespace sims_flanagan 523 } // namespace kep_toolbox 524 #endif // KEP_TOOLBOX_LEG_S_H 525