Bring MPC to the new flight stack #60

foehnx · 2018-04-25T14:25:54Z

I made some changes to allow compatibility with our new MPC.
It includes mainly templating the autopilot on the controller type and parameter type, which requires the autopilot.cpp to move to the header file, solved as a head include.

Further changes were also mentioned in #5 and we concluded that references are always given in the form of a Trajectory (and no longer TrajectoryPoint) to the controller.
The position controller would then just take the first point in it, while the MPC can use the first (for a single reference) or a whole lookahead on a trajectory.

I made several changes and would be happy to hear your comments.

@Internals: also take a look at: uzh-rpg/rpg_mpc#5

…hod to use trajectory instead of point.

…ed yet

…e instead of break-routine

kohlerj · 2018-04-25T14:37:26Z

control/autopilot/include/autopilot/autopilot_inl.h

@@ -857,7 +878,7 @@ quadrotor_common::ControlCommand AutoPilot::start(
  }

  if (timeInCurrentState() > optitrack_start_land_timeout_
-      || reference_state_.position.z() >= optitrack_start_height_)
+      || state_estimate.position.z() >= optitrack_start_height_)


why the change to state_estimate from reference_state ?

Whats the intention behind checking what the reference height is? Doesn't it make more sense to check what your actual height is and cancel the take-off once you reached this height?

if you look further in the function the reference is moved up and up as the loop is repeated. I would say that using the old method makes it more robust and smoother but this is only a feeling. By that I mean, hover WILL be reached even if you have an offset for idk which reason.

So ideally the drone does what the reference tells it to, and in that case it does not make much of a difference. However, if e.g. the drone has a wrong thrust mapping and flies much lower than the reference, when using the state estimate, it will go up until it reached the start height (with a reference much higher) but then when it switches back to hover the reference is set to the current state estimate and the drone will drop by that much again which also does not result in the desired height. So by checking the reference height at least we get a smooth behavior in this case. Anyhow, if the drone does not follow the reference well it needs to be fixed...
Bottom line: I would change this back to reference state

There you go, the reason I was searching for! Make sense to me

OK, clarification:
When using the MPC, it lags behind since your take-off maneuver has a rising position, fixed velocity but zero acceleration and hover thrust, which is unfeasible at the beginning of the start maneuver.
This is no problem for feasible control with the MPC, but it means that the it lags behind due to the compromise of positon-, velocity-, acceleration- and thrust-error.

Possible solution:

Ignore ~0.15m z-offset after start.

Add a takeoff acceleration (which could be a parameter) at the beginning of the start maneuver.

kohlerj · 2018-05-02T14:31:39Z

control/autopilot/include/autopilot/autopilot_inl.h

+
+  reference_trajectory_ = quadrotor_common::Trajectory();
+  reference_trajectory_.trajectory_type = 
+    quadrotor_common::Trajectory::TrajectoryType::GENERAL;


quick question, how about a constructor with the type as argument for the class quadrotor_common::Trajectory() ? to avoid code duplication. We could even have a constructor with one point as argument and automatically defined it as a general trajectory. It would make sense since we change the interface completely (i mean from point to trajectory in the run command...)

Agree! Will do it tomorrow!

also agree ;)

done in uzh-rpg/rpg_quadrotor_common#11

kohlerj · 2018-05-02T14:34:32Z

control/autopilot/include/autopilot/autopilot_inl.h

-quadrotor_common::ControlCommand AutoPilot::executeTrajectory(
+template <typename Tcontroller, typename Tparams>
+quadrotor_common::ControlCommand
+AutoPilot<Tcontroller, Tparams>::executeTrajectory(
    const quadrotor_common::QuadStateEstimate& state_estimate,
    ros::Duration* trajectory_execution_left_duration,


general comment: really, a pointer to a ros::Duration and int ?!? ---> how about a smart pointer?

meh... lets make it pass-by-reference!
then there's no pointer magic but still avoiding copying, ok?

No, it is our convention that arguments which are changed by the function are passed as pointers.

Conventions are here to be respected. Ok

agree, mark as done

kohlerj · 2018-05-02T14:44:34Z

control/autopilot/include/autopilot/autopilot_inl.h

+
+  std::list<quadrotor_common::TrajectoryPoint>::const_iterator it_points;
+  std::list<quadrotor_common::Trajectory>::const_iterator it_trajectories;
+  bool lookahead_reached(false);


could you comment the code, basic idea behind the heuristic (i'm lazy but so will be the next person working on it)

should it not be a separate function like mpc?

could we use specialization for these functions that change a lot ? I mean a definition for execute(mpc controller templated) and for execute(normal controller) ... Idk if it's possible but I think so. see here

3rd bullet: nope... if so we need to specify the template type in here... then we need the type at compile time.
but this is what we want to avoid, to be able to compile rpg_quadrotor_control without rpg_mpc.
I will think about making it more readable and understandable.

I will look at this in more detail later then

@foehnph Agreed for the 3rd bullet, you are right!

Ok temporary solution:
I commented the code there to make it understandable what it does.
I think this should be a separate class, like autopilot::Tracker, the trajectory queue could be passed as reference and it can have time-based tracking and spacial tracking, as well as restrictions to forward-only tracking (can speed up iterations massively).
Additionally it could also handle the trajectory queue if wanted, but I guess it's easier to leave that in the autopilot.

mfaessle

A bunch of comments but first of all thanks for this effort!

mfaessle · 2018-04-26T07:56:47Z

control/autopilot/src/autopilot_node.cpp

@@ -3,7 +3,8 @@
 int main(int argc, char **argv)
 {
  ros::init(argc, argv, "autopilot");
-  autopilot::AutoPilot autopilot;
+  autopilot::AutoPilot<position_controller::PositionController,


also make the includes for the PositionController and PositionControllerParams in this file

mfaessle · 2018-05-02T18:42:49Z

control/autopilot/include/autopilot/autopilot.h

@@ -228,3 +231,5 @@ class AutoPilot
 };

 } // namespace autopilot
+
+#include "./autopilot_inl.h"


No newline at end of file

mfaessle · 2018-05-02T18:43:19Z

control/autopilot/parameters/default.yaml

@@ -6,7 +6,7 @@ start_land_velocity: 0.5 # [m/s]
 start_idle_duration: 2.0 # [s]
 idle_thrust: 2.0 # [m/s]
 optitrack_start_height: 1.0 # [m]
-optitrack_start_land_timeout: 5 # [s]
+optitrack_start_land_timeout: 6 # [s]


Why this this parameter change in this pull request?

sorry was testing... undone

mfaessle · 2018-05-02T18:43:32Z

control/autopilot/parameters/default.yaml

@@ -27,3 +27,4 @@ emergency_land_thrust: 9.0 # [m/s^2]

 control_command_input_timeout: 0.1 # [s]
 enable_command_feedthrough: false
+predictive_control_lookahead: 2.0 # [s]


No newline at end of file

mfaessle · 2018-05-02T18:57:08Z

simulation/rpg_rotors_interface/parameters/autopilot.yaml

@@ -27,3 +27,4 @@ emergency_land_thrust: 9.0 # [m/s^2]

 control_command_input_timeout: 0.1 # [s]
 enable_command_feedthrough: true
+predictive_control_lookahead: 2.0 # [s]


No new line at end of file

mfaessle · 2018-05-02T19:19:43Z

control/autopilot/include/autopilot/autopilot_inl.h

-quadrotor_common::ControlCommand AutoPilot::executeTrajectory(
+template <typename Tcontroller, typename Tparams>
+quadrotor_common::ControlCommand
+AutoPilot<Tcontroller, Tparams>::executeTrajectory(
    const quadrotor_common::QuadStateEstimate& state_estimate,
    ros::Duration* trajectory_execution_left_duration,


No, it is our convention that arguments which are changed by the function are passed as pointers.

mfaessle · 2018-05-02T19:23:18Z

control/autopilot/include/autopilot/autopilot_inl.h

@@ -1147,20 +1222,57 @@ quadrotor_common::ControlCommand AutoPilot::executeTrajectory(
      *trajectory_execution_left_duration = ros::Duration(0.0);
      *trajectories_left_in_queue = 0;
      trajectory_queue_.pop_front();
-      setAutoPilotState(States::HOVER);
-      return base_controller_.run(state_estimate, reference_state_,
+      setAutoPilotStateForced(States::HOVER);


This should not be forced to avoid jumpy behavior if for whatever reason the trajectory ends with a non zero velocity. So by not forcing it will do a smooth breaking if necessary

Yes but the you do not guarantee that you reach the end state...
the end of a trajectory normally has zero velocity ;-) and even if not it just means that you get to a stop with an offset where the velocity error is compensated by the position error.

I propose to change setAutoPilotStateForced() so that it sets reference state velocity to zero when trigger to hover.

mfaessle · 2018-05-02T19:25:01Z

control/autopilot/include/autopilot/autopilot_inl.h

                                  base_controller_params_);
    }
    else
    {
      time_start_trajectory_execution_ +=
-          trajectory_queue_.front().points.back().time_from_start;
+        trajectory_queue_.front().points.back().time_from_start;


I saw a few of those... Detail but this is just a thing because of non equal auto formatting which doesn't hurt but is also not necessary.

we use two spaces for tabs... so if I see it I change it :-)

mfaessle · 2018-05-02T19:28:26Z

control/autopilot/include/autopilot/autopilot_inl.h

+
+  std::list<quadrotor_common::TrajectoryPoint>::const_iterator it_points;
+  std::list<quadrotor_common::Trajectory>::const_iterator it_trajectories;
+  bool lookahead_reached(false);


I will look at this in more detail later then

mfaessle · 2018-05-02T19:31:07Z

control/autopilot/include/autopilot/autopilot.h

@@ -137,10 +138,11 @@ class AutoPilot

  state_predictor::StatePredictor state_predictor_;

-  position_controller::PositionController base_controller_;
-  position_controller::PositionControllerParams base_controller_params_;
+  Tcontroller base_controller_;


I named this base controller because I thought that there might be new controllers that are only used for certain tasks but some of the basic tasks are always executed with the base controller (like start land etc.). @foehnph would something like this make sense (and then have two controller instances) or not?

I agree with @mfaessle, it makes it easier for you @foehnph since you don't need to implement the velocity mode in mpc, no?

not really... then you would have to switch between controllers, implement state machine for that and generally reimplement a shitload of things if you want to use the autopilot with the new controller and not just have a different controller in feedthrough...

mfaessle · 2018-05-02T19:34:48Z

About transforming the cpp file into a header... I never used templates myself but maybe @tcies has some comments about naming conventions etc of such files.

kohlerj

See new comments

foehnx · 2018-05-03T07:55:47Z

@mfaessle the autopilot_inl.h is done according to @tcies proposed convention

foehnx · 2018-05-04T06:48:40Z

updated rpg_quadrotor_common with new Trajectory constructor, so...
@rpg-jenkins test this please

foehnx · 2018-05-09T12:39:43Z

can someone approve this please?

kohlerj

lgtm

mfaessle · 2018-05-09T13:52:25Z

control/autopilot/include/autopilot/autopilot_inl.h

@@ -877,16 +899,27 @@ quadrotor_common::ControlCommand AutoPilot::start(
          + start_land_velocity_
              * (timeInCurrentState() - start_idle_duration_);
      reference_state_.velocity.z() = start_land_velocity_;
+      if(timeInCurrentState() < start_idle_duration_ +


This is a hack since the velocity does not change with acceleration over time. But as long as it works for both controllers I can live with it ;) The nicest way would be if it would be a real trapezoidal velocity profile during the start maneuver.

I'm gonna change that, test in simulation and request an approve again

I opened an issue regarding this (#64). Feel free to adress it in a new pull request.

already done here

mfaessle · 2018-05-09T14:46:55Z

control/autopilot/include/autopilot/autopilot_inl.h

@@ -877,16 +899,29 @@ quadrotor_common::ControlCommand AutoPilot::start(
          + start_land_velocity_
              * (timeInCurrentState() - start_idle_duration_);
      reference_state_.velocity.z() = start_land_velocity_;
+      if(timeInCurrentState() < start_idle_duration_ +
+         start_land_velocity_/start_land_acceleration_)


spaces around arithmetic symbols please

foehnx · 2018-05-09T15:36:03Z

finally happy? I'll merge tomorrow morning... ;-)

foehnx added 4 commits April 23, 2018 12:06

wip, not working yet. changed to templated autopilot, changed run-met…

1660fdc

…hod to use trajectory instead of point.

first round of adaptions finished

888ea1f

major adaptions to include MPC, working in simulation, not fully test…

0965c4c

…ed yet

changed end of trajectory execution to switch to last trajectory stat…

d6dd6ab

…e instead of break-routine

foehnx added the enhancement label Apr 25, 2018

foehnx self-assigned this Apr 25, 2018

foehnx requested review from tcies and kohlerj April 25, 2018 14:25

kohlerj reviewed Apr 25, 2018

View reviewed changes

kohlerj reviewed May 2, 2018

View reviewed changes

mfaessle reviewed May 2, 2018

View reviewed changes

kohlerj reviewed May 3, 2018

View reviewed changes

foehnx mentioned this pull request May 3, 2018

added constructor for general trajectory from one point uzh-rpg/rpg_quadrotor_common#11

Merged

small modifiactions for PR #60

d57da3b

added start acceleration

2375c91

kohlerj approved these changes May 9, 2018

View reviewed changes

mfaessle reviewed May 9, 2018

View reviewed changes

mfaessle mentioned this pull request May 9, 2018

Bring MPC to new rpq_quadrotor_control #5

Open

added velocity ramp-up at start

273c334

foehnx mentioned this pull request May 9, 2018

Trapezoidal velocity profile for start maneuver #64

Closed

mfaessle reviewed May 9, 2018

View reviewed changes

spaces around math...

c81513c

mfaessle approved these changes May 9, 2018

View reviewed changes

foehnx merged commit c93155e into master May 10, 2018

mfaessle deleted the dev/new_interface branch June 7, 2018 10:27

Bring MPC to the new flight stack #60

Bring MPC to the new flight stack #60

Conversation

foehnx commented Apr 25, 2018 • edited

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

kohlerj May 2, 2018 • edited

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

kohlerj May 2, 2018 • edited

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

mfaessle left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

mfaessle commented May 2, 2018

kohlerj left a comment

Choose a reason for hiding this comment

foehnx commented May 3, 2018

foehnx commented May 4, 2018

foehnx commented May 9, 2018

kohlerj left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

foehnx commented May 9, 2018

foehnx commented Apr 25, 2018 •

edited

kohlerj May 2, 2018 •

edited

kohlerj May 2, 2018 •

edited