Merge pull request gtri#522 from dmagree/enable-external-force

allowing an external force in the fixed wing model
kemen209 · Apr 9, 2021 · fffeb46 · fffeb46
2 parents b696da4 + 6ac6e33
commit fffeb46
Show file tree

Hide file tree

Showing 2 changed files with 17 additions and 8 deletions.
diff --git a/include/scrimmage/plugins/motion/FixedWing6DOF/FixedWing6DOF.h b/include/scrimmage/plugins/motion/FixedWing6DOF/FixedWing6DOF.h
@@ -185,11 +185,12 @@ class FixedWing6DOF : public scrimmage::motion::RigidBody6DOFBase{
     double launch_start_t_ = 0;
     double launch_time_ = 30;
     LaunchState launch_state_ = POSTLAUNCH;
-    Eigen::Vector3d launch_dir_NED_;
 
     bool use_ground_model_ = true;
 
     Eigen::Quaterniond rot_180_x_axis_;
+
+    bool skip_propagation_ = false;
 };
 } // namespace motion
 } // namespace scrimmage

diff --git a/src/plugins/motion/FixedWing6DOF/FixedWing6DOF.cpp b/src/plugins/motion/FixedWing6DOF/FixedWing6DOF.cpp
@@ -357,11 +357,15 @@ bool FixedWing6DOF::step(double time, double dt) {
     }
 
     // Apply any external forces (todo)
-    // force_ext_body_ = quat_body_.rotate_reverse(ext_force_);
-    // ext_force_ = Eigen::Vector3d::Zero(); // reset ext_force_ member variable
-    force_ext_body_ = F_catapult;
+    Eigen::Vector3d ext_force_NED(ext_force_(0), -ext_force_(1), -ext_force_(2));
+    ext_force_ = Eigen::Vector3d::Zero(); // reset ext_force_ member variable
+    force_ext_body_ = quat_body_.rotate_reverse(ext_force_NED);
+    force_ext_body_ += F_catapult;
 
-    ode_step(dt);
+    if (!skip_propagation_) {
+        ode_step(dt);
+    }
+    skip_propagation_ = false;
 
     quat_body_.set(x_[q0], x_[q1], x_[q2], x_[q3]);
     quat_body_.normalize();
@@ -506,10 +510,10 @@ void FixedWing6DOF::model(const vector_t &x , vector_t &dxdt , double t) {
 
         F_total += F_ground;
     }
-
     if (POSTLAUNCH != launch_state_) {
-        F_total = force_ext_body_;
+        F_total = Eigen::Vector3d::Zero();;
     }
+    F_total += force_ext_body_;
 
     // Calculate body frame linear velocities
     dxdt[U] = x[V]*x[R] - x[W]*x[Q] + F_total(0) / mass_;
@@ -568,7 +572,11 @@ void FixedWing6DOF::model(const vector_t &x , vector_t &dxdt , double t) {
 }
 
 void FixedWing6DOF::teleport(StatePtr &state) {
-    state_ = state;
+    state_->pos() = state->pos();
+    state_->vel() = state->vel();
+    state_->ang_vel() = state->ang_vel();
+    state_->quat() = state->quat();
+    skip_propagation_ = true;
 }
 
 } // namespace motion