search

ethz-asl / mav_control_rw

Control strategies for rotary wing Micro Aerial Vehicles using ROS
Apache License 2.0
352 stars 160 forks source link

Assertion failed in in mav_linear_mpc #4

Closed alexbuyval closed 7 years ago

alexbuyval commented 7 years ago

Hi,

I was trying to run the simulation with linear MPC as described in wiki. However, I faced with the assertion failed. Below my log:

[ INFO] [1486490091.169850171]: start initializing mav_disturbance_observer:KF
[ INFO] [1486490091.171269822]: mav_disturbance_observer:KF dynamic config is called successfully
[ INFO] [1486490091.186612678]: KF parameters loaded successfully
[ INFO] [1486490091.187209009]: state_covariance_: 
0.1   0   0   0   0   0   0   0   0   0   0   0   0   0   0   0   0   0
  0 0.1   0   0   0   0   0   0   0   0   0   0   0   0   0   0   0   0
  0   0 0.1   0   0   0   0   0   0   0   0   0   0   0   0   0   0   0
  0   0   0 0.1   0   0   0   0   0   0   0   0   0   0   0   0   0   0
  0   0   0   0 0.1   0   0   0   0   0   0   0   0   0   0   0   0   0
  0   0   0   0   0 0.1   0   0   0   0   0   0   0   0   0   0   0   0
  0   0   0   0   0   0 0.1   0   0   0   0   0   0   0   0   0   0   0
  0   0   0   0   0   0   0 0.1   0   0   0   0   0   0   0   0   0   0
  0   0   0   0   0   0   0   0 0.1   0   0   0   0   0   0   0   0   0
  0   0   0   0   0   0   0   0   0 0.1   0   0   0   0   0   0   0   0
  0   0   0   0   0   0   0   0   0   0 0.1   0   0   0   0   0   0   0
  0   0   0   0   0   0   0   0   0   0   0 0.1   0   0   0   0   0   0
  0   0   0   0   0   0   0   0   0   0   0   0 0.1   0   0   0   0   0
  0   0   0   0   0   0   0   0   0   0   0   0   0 0.1   0   0   0   0
  0   0   0   0   0   0   0   0   0   0   0   0   0   0 0.1   0   0   0
  0   0   0   0   0   0   0   0   0   0   0   0   0   0   0 0.1   0   0
  0   0   0   0   0   0   0   0   0   0   0   0   0   0   0   0 0.1   0
  0   0   0   0   0   0   0   0   0   0   0   0   0   0   0   0   0 0.1
[ INFO] [1486490091.187399030]: Kalman Filter Initialized!
[ INFO] [1486490091.212209782]: Linear MPC: Steady State calculation is initialized correctly
[ INFO] [1486490091.212391470]: A: 
         1          0          0    0.09995          0          0          0  0.0431908
         0          1          0          0    0.09995          0 -0.0430852          0
         0          0          1          0          0        0.1          0          0
         0          0          0      0.999          0          0          0   0.810794
         0          0          0          0      0.999          0  -0.807829          0
         0          0          0          0          0          1          0          0
         0          0          0          0          0          0    0.67032          0
         0          0          0          0          0          0          0   0.675598
[ INFO] [1486490091.212478409]: B: 
         0 0.00516734          0
 -0.005261          0          0
         0          0    0.00495
         0   0.151006          0
 -0.153652          0          0
         0          0        0.1
  0.297306          0          0
         0   0.292535          0
[ INFO] [1486490091.212546850]: B_d: 
0.00494836          0          0
         0 0.00494836          0
         0          0    0.00495
 0.0999505          0          0
         0  0.0999505          0
         0          0        0.1
         0          0          0
         0          0          0
[ INFO] [1486490091.212668776]: Linear MPC: initialized correctly
[ INFO] [1486490091.368251690, 16.800000000]: Linear MPC: Tuning parameters updated...
[ INFO] [1486490091.368404294, 16.800000000]: diag(Q) = 
40 40 60 20 20 25 20 20
[ INFO] [1486490091.368559821, 16.800000000]: diag(R) = 
35 35  2
[ INFO] [1486490091.368584806, 16.800000000]: diag(R_delta) = 
    0.3    0.3 0.0025
[ INFO] [1486490091.368750368, 16.800000000]: Q_final = 
 428.621        0        0  108.103        0        0        0  108.491
       0  427.748        0        0  107.214        0 -106.405        0
       0        0  563.584        0        0   111.51        0        0
 108.103        0        0  109.174        0        0        0  108.329
       0  107.214        0        0  108.248        0 -105.983        0
       0        0   111.51        0        0  111.486        0        0
       0 -106.405        0        0 -105.983        0  176.989        0
 108.491        0        0  108.329        0        0        0  182.576
[ INFO] [1486490091.402386001, 16.840000000]: Created control interface for controller linear_model_predictive_controller and RC ACI rc interface
mav_linear_mpc_node: /usr/include/boost/msm/front/state_machine_def.hpp:203: void boost::msm::front::state_machine_def<Derived, BaseState>::no_transition(const Event&, FSM&, int) [with FSM = boost::msm::back::state_machine<mav_control_interface::state_machine::StateMachineDefinition, boost::msm::back::mpl_graph_fsm_check>; Event = mav_control_interface::state_machine::OdometryWatchdog; Derived = mav_control_interface::state_machine::StateMachineDefinition; BaseState = boost::msm::front::default_base_state]: Assertion `false' failed.
[firefly/mav_linear_mpc-1] process has died [pid 31591, exit code -6, cmd /home/alex/catkin_ws/devel/lib/mav_linear_mpc/mav_linear_mpc_node odometry:=msf_core/odometry __name:=mav_linear_mpc __log:=/home/alex/.ros/log/60b83636-ed5e-11e6-ac08-10c37bc7c393/firefly-mav_linear_mpc-1.log].
log file: /home/alex/.ros/log/60b83636-ed5e-11e6-ac08-10c37bc7c393/firefly-mav_linear_mpc-1*.log
[firefly/mav_linear_mpc-1] restarting process

How can I solve it? What is a reason such fail?

Alex

fmina commented 7 years ago

Hi, I cannot reproduce this fail. Which version of ubuntu/boost are you using?

alexbuyval commented 7 years ago

Hi,

I am using the Ubuntu 14.04 with ROS Jade. The boost version is 1.54.

fmina commented 7 years ago

5 should fix it. Can you please try it before we merge?

alexbuyval commented 7 years ago

Thank you!

There is no the error now.

I have got only the following warnings: [ WARN] [1486792151.938673606, 9.120000000]: No odometry message received in the last 1 seconds!

Also I don't see any messages in /firefly/msf_core/odometry topic by rqt.

I run the following command in separate terminals:

roslaunch rotors_gazebo mav.launch mav_name:=firefly
roslaunch mav_linear_mpc mav_linear_mpc_sim_msf.launch mav_name:=firefly
rostopic pub /firefly/command/pose geometry_msgs/PoStamped '{header:{stamp: now, frame_id: "world"}, pose: {position: {x: 0, y: 0, z: 1}, orientation: {w: 1.0}}}'

Did I miss something?

Best Regards, Alex

fmina commented 7 years ago

you have to initialize MSF. There is a service called init_msf_scale, call it with scale 1.

alexbuyval commented 7 years ago

Hm...

There is no such service in my ROS environment:

/firefly/PID_attitude_controller/get_loggers
/firefly/PID_attitude_controller/set_logger_level
/firefly/PID_attitude_controller/set_parameters
/firefly/back_to_position_hold
/firefly/mav_linear_mpc/KF_observer/StartCalibrateKF
/firefly/mav_linear_mpc/KF_observer/set_parameters
/firefly/mav_linear_mpc/get_loggers
/firefly/mav_linear_mpc/set_logger_level
/firefly/mav_linear_mpc/set_parameters
/firefly/pose_sensor/core/set_parameters
/firefly/pose_sensor/get_loggers
/firefly/pose_sensor/pose_sensor/initialize_msf_height
/firefly/pose_sensor/pose_sensor/initialize_msf_scale
/firefly/pose_sensor/pose_sensor/set_parameters
/firefly/pose_sensor/set_logger_level
/firefly/reset_integrator
/firefly/takeoff
/gazebo/apply_body_wrench
/gazebo/apply_joint_effort
/gazebo/clear_body_wrenches
/gazebo/clear_joint_forces
/gazebo/delete_model
/gazebo/get_joint_properties
/gazebo/get_link_properties
/gazebo/get_link_state
/gazebo/get_loggers
/gazebo/get_model_properties
/gazebo/get_model_state
/gazebo/get_physics_properties
/gazebo/get_world_properties
/gazebo/pause_physics
/gazebo/reset_simulation
/gazebo/reset_world
/gazebo/set_joint_properties
/gazebo/set_link_properties
/gazebo/set_link_state
/gazebo/set_logger_level
/gazebo/set_model_configuration
/gazebo/set_model_state
/gazebo/set_parameters
/gazebo/set_physics_properties
/gazebo/spawn_gazebo_model
/gazebo/spawn_sdf_model
/gazebo/spawn_urdf_model
/gazebo/unpause_physics
/rosout/get_loggers
/rosout/set_logger_level
/rostopic_8255_1486817709036/get_loggers
/rostopic_8255_1486817709036/set_logger_level
/rqt_gui_py_node_8349/get_loggers
/rqt_gui_py_node_8349/set_logger_level

Should I run the MSF using separate launch file?

fmina commented 7 years ago

the service name is /firefly/pose_sensor/pose_sensor/initialize_msf_scale

alexbuyval commented 7 years ago

Sorry for my inattention

Now I see :)

Anyway, I am receiving odometry warnings again:


[ INFO] [1486819352.677665051, 25.830000000]: Initialize filter with scale 1.000000
[ INFO] [1486819352.677789797, 25.830000000]: initial measurement pos:[1.21129e-11 1.2113e-11       0.08] orientation: [1, -7.57e-11, 7.57e-11, 2.93e-17]
[ WARN] [1486819352.677823670, 25.830000000]: No measurements received yet to initialize attitude - using [1 0 0 0]
[ WARN] [1486819352.783981432, 25.940000000]: Using simulated core plus fixed diag initial error state covariance.
[ INFO] [1486819352.794267071, 25.950000000]: Initializing msf_core (built: Feb  7 2017)
[ INFO] [1486819352.794325045, 25.950000000]: Core parameters: 
    fixed_bias: 0
    fuzzythres: 0.1
    noise_acc:  0.083
    noise_accbias:  0.0083
    noise_gyr:  0.0013
    noise_gyrbias:  0.00013

[ INFO] [1486819352.794436148, 25.950000000]: Core init with state: 
--------- State at time 25.84s: ---------
0 : [0-2]    : Matrix<3, 1>         : [1.21129e-11 1.2113e-11       0.08]
1 : [3-5]    : Matrix<3, 1>         : [0 0 0]
2 : [6-9]    : Quaternion (w,x,y,z) : [1, -7.57e-11, 7.57e-11, 2.93e-17]
3 : [10-12]  : Matrix<3, 1>         : [0 0 0]
4 : [13-15]  : Matrix<3, 1>         : [0 0 0]
5 : [16-16]  : Matrix<1, 1>         : [1]
6 : [17-20]  : Quaternion (w,x,y,z) : [1, 0, 0, 0]
7 : [21-23]  : Matrix<3, 1>         : [0 0 0]
8 : [24-27]  : Quaternion (w,x,y,z) : [1, 0, 0, 0]
9 : [28-30]  : Matrix<3, 1>         : [0 0 0]
-------------------------------------------------------
[ WARN] [1486819353.110419404, 26.270000000]: No odometry message received in the last 1 seconds!
[ WARN] [1486819354.111219948, 27.270000000]: No odometry message received in the last 1 seconds!

Also the firefly does not move still.

Maybe I forgot something else?

fmina commented 7 years ago

I fixed the odom warning now.

alexbuyval commented 7 years ago

Hi,

I have just pulled all your last commits from 'feature/missing_odom_handle' and rebuild, however, nothing changed. There are odometry warnings anyway. Also I was trying to call 'firefly\takeoff' service, bu I have got new assert failed.

[ WARN] [1486912210.017259685, 30.010000000]: Pose measurement throttling is on, dropping messagesto be below 20.000000 Hz
[ WARN] [1486912211.007784320, 31.000000000]: No odometry message received in the last 1 seconds!
[ INFO] [1486912211.060821923, 31.050000000]: Initialize filter with scale 1.000000
[ INFO] [1486912211.060922806, 31.050000000]: initial measurement pos:[ -1.2113e-11 -1.21129e-11         0.08] orientation: [1, 7.57e-11, -7.57e-11, 3.5e-17]
[ WARN] [1486912211.060958991, 31.050000000]: No measurements received yet to initialize attitude - using [1 0 0 0]
[ WARN] [1486912211.166954581, 31.150000000]: Using simulated core plus fixed diag initial error state covariance.
[ INFO] [1486912211.177147563, 31.160000000]: Initializing msf_core (built: Feb  7 2017)
[ INFO] [1486912211.177201238, 31.160000000]: Core parameters: 
    fixed_bias: 0
    fuzzythres: 0.1
    noise_acc:  0.083
    noise_accbias:  0.0083
    noise_gyr:  0.0013
    noise_gyrbias:  0.00013

[ INFO] [1486912211.177300848, 31.170000000]: Core init with state: 
--------- State at time 31.05s: ---------
0 : [0-2]    : Matrix<3, 1>         : [ -1.2113e-11 -1.21129e-11         0.08]
1 : [3-5]    : Matrix<3, 1>         : [0 0 0]
2 : [6-9]    : Quaternion (w,x,y,z) : [1, 7.57e-11, -7.57e-11, 3.5e-17]
3 : [10-12]  : Matrix<3, 1>         : [0 0 0]
4 : [13-15]  : Matrix<3, 1>         : [0 0 0]
5 : [16-16]  : Matrix<1, 1>         : [1]
6 : [17-20]  : Quaternion (w,x,y,z) : [1, 0, 0, 0]
7 : [21-23]  : Matrix<3, 1>         : [0 0 0]
8 : [24-27]  : Quaternion (w,x,y,z) : [1, 0, 0, 0]
9 : [28-30]  : Matrix<3, 1>         : [0 0 0]
-------------------------------------------------------

[ WARN] [1486912353.562381721, 173.500000000]: No odometry message received in the last 1 seconds!
[ WARN] [1486912354.562292311, 174.500000000]: No odometry message received in the last 1 seconds!
[ INFO] [1486912354.843859861, 174.770000000]: Take off event sent
mav_linear_mpc_node: /usr/include/boost/msm/front/state_machine_def.hpp:203: void boost::msm::front::state_machine_def<Derived, BaseState>::no_transition(const Event&, FSM&, int) [with FSM = boost::msm::back::state_machine<mav_control_interface::state_machine::StateMachineDefinition, boost::msm::back::mpl_graph_fsm_check>; Event = mav_control_interface::state_machine::Takeoff; Derived = mav_control_interface::state_machine::StateMachineDefinition; BaseState = boost::msm::front::default_base_state]: Assertion `false' failed.
[firefly/mav_linear_mpc-1] process has died [pid 16600, exit code -6, cmd /home/alex/catkin_ws/devel/lib/mav_linear_mpc/mav_linear_mpc_node odometry:=msf_core/odometry __name:=mav_linear_mpc __log:=/home/alex/.ros/log/3db61362-f135-11e6-a465-10c37bc7c393/firefly-mav_linear_mpc-1.log].
log file: /home/alex/.ros/log/3db61362-f135-11e6-a465-10c37bc7c393/firefly-mav_linear_mpc-1*.log

Alex

fmina commented 7 years ago

I think you need to clean your workspace and recompile. The assertion happens if you compile in Debug mode, and there is no valid transition in the state machine. It is more like debugging information than actual error.

Just delete mav_control_interface from the build folder and recompile mav_linear_mpc. As per now, the takeoff service would send takeoff command only on the real platform, I will update the state machine to accept takeoff commands in simulation mode as well.

alexbuyval commented 7 years ago

I have clean the build and devel folders and recompiled the catkin workspace. However, nothing changed. :(

Tell me after which action the firefly have to takeoff? After a publishing a new pose into /firefly/command/pose?

fmina commented 7 years ago

to take off you need to send command on /firefly/command/pose, make sure your odometry message is getting into the controller, and your gazebo simulator is running.

alexbuyval commented 7 years ago

thank you!

Finally, I have found that I have a issue with gazebo plugins, so there is no /imu topic. I have fixed it and everything works properly now.

image

fmina commented 7 years ago

Great! I will merge #5 after we test it on the real platform.