[mini_quadrotor] refactored mpc with crocoddyl

robots/mini_quadrotor/CMakeLists.txt

@@ -6,6 +6,7 @@ add_compile_options(-std=c++17)
 find_package(catkin REQUIRED COMPONENTS
   mujoco_ros_control
   roscpp
+  tf
   )
 
 find_package(Eigen3 REQUIRED)
@@ -14,7 +15,7 @@ find_package(crocoddyl REQUIRED)
 find_package(OpenMP REQUIRED)
 
 catkin_package(
-  CATKIN_DEPENDS aerial_robot_control aerial_robot_estimation aerial_robot_model roscpp
+  CATKIN_DEPENDS aerial_robot_control aerial_robot_estimation aerial_robot_model roscpp tf
   )
 
 ###########

robots/mini_quadrotor/config/mpc_config.yaml

@@ -0,0 +1 @@
+x_state_weight: [20, 20, 100, 100, 100, 300, 10, 10, 50, 40, 40, 100]

robots/mini_quadrotor/launch/crocoddyl_test.launch

@@ -3,6 +3,17 @@
   <node pkg="mini_quadrotor" type="crocoddyl_test" name="crocoddyl_test" output="screen">
     <param name="filename" value="$(find mini_quadrotor)/urdf/robot.urdf"/>
     <param name="num_threads" value="4"/>
-    <param name="hovering_approximate" value="true"/>
+    <param name="hovering_approximate" value="false"/>
+    <param name="horizon" value="0.5"/>
+    <param name="dt" value="0.02"/>
+
+    <param name="x_reg_weight" value="1"/>
+    <param name="x_reg_final_weight" value="5"/>
+    <param name="u_reg_weight" value="5"/>
+
+    <rosparam command="load" file="$(find mini_quadrotor)/config/mpc_config.yaml"/>
   </node>
+
+  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find mini_quadrotor)/config/rviz_config"/>
+
 </launch>

robots/mini_quadrotor/package.xml

@@ -27,6 +27,8 @@
 
   <build_depend>roscpp</build_depend>
   <run_depend>roscpp</run_depend>
+  <build_depend>tf</build_depend>
+  <run_depend>tf</run_depend>
 
   <test_depend>rostest</test_depend>
 

robots/mini_quadrotor/src/crocoddyl_test.cpp

@@ -2,9 +2,11 @@
 #include <pinocchio/parsers/urdf.hpp>
 
 #include <ros/ros.h>
+#include <tf/transform_broadcaster.h>
 
 #include "crocoddyl/core/action-base.hpp"
 #include "crocoddyl/core/activations/weighted-quadratic.hpp"
+#include "crocoddyl/core/activations/quadratic-barrier.hpp"
 #include "crocoddyl/core/costs/cost-sum.hpp"
 #include "crocoddyl/core/costs/residual.hpp"
 #include <crocoddyl/core/diff-action-base.hpp>
@@ -18,16 +20,21 @@
 #include <crocoddyl/multibody/actions/free-fwddyn.hpp>
 #include <crocoddyl/multibody/actuations/floating-base-thrusters.hpp>
 #include <crocoddyl/multibody/states/multibody.hpp>
+#include "crocoddyl/multibody/residuals/com-position.hpp"
 #include "crocoddyl/multibody/residuals/frame-placement.hpp"
+#include "crocoddyl/multibody/residuals/frame-rotation.hpp"
 #include "crocoddyl/multibody/residuals/state.hpp"
 
 #include <vector>
+#include <limits>
 
 int main(int argc, char** argv)
 {
   ros::init(argc, argv, "crocoddyl_test");
   ros::NodeHandle nh;
   ros::NodeHandle nhp("~");
+  tf::TransformBroadcaster robot_base_broadcaster;
+
   std::string fileName = "";
   nhp.getParam("filename", fileName);
 
@@ -45,6 +52,7 @@ int main(int argc, char** argv)
   }
 
   std::vector<crocoddyl::Thruster> thrusters;
+  int rotor_num = 4;
   double rotor_x = 0.085;
   double rotor_z = 0.026;
   double m_f_rate = 0.011;
@@ -58,51 +66,85 @@ int main(int argc, char** argv)
   thrusters.push_back(crocoddyl::Thruster(p4, m_f_rate, crocoddyl::ThrusterType::CCW));
 
   boost::shared_ptr<crocoddyl::StateMultibody> state = boost::make_shared<crocoddyl::StateMultibody>(model);
-  boost::shared_ptr<crocoddyl::ActuationModelFloatingBaseThrusters> actuation = boost::make_shared<crocoddyl::ActuationModelFloatingBaseThrusters>(state, thrusters);
+  ROS_WARN_STREAM("[crocoddyl][state] " << "state->get_ndx(): " << state->get_ndx());
+  ROS_WARN_STREAM("[crocoddyl][state] " << "state->get_nx(): " << state->get_nx());
+  ROS_WARN_STREAM("[crocoddyl][state] " << "state->get_nq(): " << state->get_nq());
+  ROS_WARN_STREAM("[crocoddyl][state] " << "state->get_nv(): " << state->get_nv());
 
+  boost::shared_ptr<crocoddyl::ActuationModelFloatingBaseThrusters> actuation = boost::make_shared<crocoddyl::ActuationModelFloatingBaseThrusters>(state, thrusters);
   int nu = actuation->get_nu();
-  ROS_WARN_STREAM("[actuation] actuation->get_nu(): " << nu);
+  ROS_WARN_STREAM("[crocoddyl][actuation] actuation->get_nu(): " << nu);
+
   boost::shared_ptr<crocoddyl::CostModelSum> running_cost_model = boost::make_shared<crocoddyl::CostModelSum>(state, nu);
   boost::shared_ptr<crocoddyl::CostModelSum> terminal_cost_model = boost::make_shared<crocoddyl::CostModelSum>(state, nu);
 
-
   // costs
-  boost::shared_ptr<crocoddyl::ResidualModelState> x_residual = boost::make_shared<crocoddyl::ResidualModelState>(state, state->zero(), nu);
-  boost::shared_ptr<crocoddyl::ActivationModelWeightedQuad> x_activation = boost::make_shared<crocoddyl::ActivationModelWeightedQuad>(Eigen::VectorXd::Zero(20)); // maybe nx(12) + nu(8)
-  boost::shared_ptr<crocoddyl::CostModelResidual> x_reg_cost = boost::make_shared<crocoddyl::CostModelResidual>(state, x_activation, x_residual);
+  // state point
+  Eigen::VectorXd xref = Eigen::VectorXd::Zero(state->get_nq() + state->get_nv());
+  xref(2) = 1.0;
+  xref(6) = 1.0;
+  boost::shared_ptr<crocoddyl::ResidualModelAbstract> x_residual = boost::make_shared<crocoddyl::ResidualModelState>(state, xref, nu);
+  Eigen::VectorXd x_activation_weights(state->get_nv() * 2);
+  {
+    std::vector<double> whatever(state->get_nv() * 2);
+    nhp.getParam("x_state_weight", whatever);
+    for(int i = 0; i < state->get_nv() * 2; i++)
+      x_activation_weights(i) = whatever.at(i);
+  }
+  std::cout << "x_activation_weights: " << x_activation_weights.transpose() << std::endl;
+  boost::shared_ptr<crocoddyl::CostModelAbstract> x_reg_cost = boost::make_shared<crocoddyl::CostModelResidual>(state, boost::make_shared<crocoddyl::ActivationModelWeightedQuad>(x_activation_weights), x_residual);
 
+  // thrust input
   boost::shared_ptr<crocoddyl::ResidualModelControl> u_residual = boost::make_shared<crocoddyl::ResidualModelControl>(state, nu);
-  boost::shared_ptr<crocoddyl::CostModelResidual> u_reg_cost = boost::make_shared<crocoddyl::CostModelResidual>(state, u_residual);
+  boost::shared_ptr<crocoddyl::ActivationModelAbstract> u_activation = boost::make_shared<crocoddyl::ActivationModelQuadraticBarrier>(crocoddyl::ActivationBounds(Eigen::VectorXd::Zero(rotor_num), 8.0 * Eigen::VectorXd::Ones(rotor_num)));
+  boost::shared_ptr<crocoddyl::CostModelAbstract> u_reg_cost = boost::make_shared<crocoddyl::CostModelResidual>(state, u_activation, u_residual);
+
+  double x_reg_weight, x_reg_final_weight, u_reg_weight;
+  nhp.getParam("x_reg_weight", x_reg_weight);
+  nhp.getParam("x_reg_final_weight", x_reg_final_weight);
+  nhp.getParam("u_reg_weight", u_reg_weight);
 
-  boost::shared_ptr<crocoddyl::ResidualModelFramePlacement> goal_tracking_residual = boost::make_shared<crocoddyl::ResidualModelFramePlacement>(state, model->getFrameId("fc"), pinocchio::SE3(Eigen::Matrix3d::Identity(), Eigen::Vector3d(0, 0, 1.0)), nu);
-  boost::shared_ptr<crocoddyl::CostModelResidual> goal_tracking_cost = boost::make_shared<crocoddyl::CostModelResidual>(state, goal_tracking_residual);
+  running_cost_model->addCost("xReg", x_reg_cost, x_reg_weight);
+  running_cost_model->addCost("uReg", u_reg_cost, u_reg_weight);
+  terminal_cost_model->addCost("xReg_final", x_reg_cost, x_reg_final_weight);
 
-  running_cost_model->addCost("xReg", x_reg_cost, 1e-6);
-  running_cost_model->addCost("uREg", u_reg_cost, 1e-6);
-  running_cost_model->addCost("trackPose", goal_tracking_cost, 1e-2);
-  terminal_cost_model->addCost("goalPose", goal_tracking_cost, 3.0);
+  double horizon, dt;
+  nhp.getParam("horizon", horizon);
+  nhp.getParam("dt", dt);
 
-  double dt = 3e-2;
-  int t = 33;
+  int N = horizon / dt;
 
-  boost::shared_ptr<crocoddyl::IntegratedActionModelEuler> running_model = boost::make_shared<crocoddyl::IntegratedActionModelEuler>(boost::make_shared<crocoddyl::DifferentialActionModelFreeFwdDynamics>(state, actuation, running_cost_model), dt);
-  boost::shared_ptr<crocoddyl::IntegratedActionModelEuler> terminal_model = boost::make_shared<crocoddyl::IntegratedActionModelEuler>(boost::make_shared<crocoddyl::DifferentialActionModelFreeFwdDynamics>(state, actuation, terminal_cost_model), dt);
+  boost::shared_ptr<crocoddyl::ActionModelAbstract> running_model = boost::make_shared<crocoddyl::IntegratedActionModelEuler>(boost::make_shared<crocoddyl::DifferentialActionModelFreeFwdDynamics>(state, actuation, running_cost_model), dt);
+  boost::shared_ptr<crocoddyl::ActionModelAbstract> terminal_model = boost::make_shared<crocoddyl::IntegratedActionModelEuler>(boost::make_shared<crocoddyl::DifferentialActionModelFreeFwdDynamics>(state, actuation, terminal_cost_model), dt);
 
-  std::vector<boost::shared_ptr<crocoddyl::ActionModelAbstract> > running_models(t, running_model);
+  double epsilon = 0.000001;
+  Eigen::VectorXd u_lb = Eigen::VectorXd::Zero(nu);
+  Eigen::VectorXd u_ub = Eigen::VectorXd::Zero(nu);
+  for(int i = 0; i < rotor_num; i++)
+    {
+      u_lb(i) = 0;
+      u_ub(i) = 8;
+    }
+  running_model->set_u_lb(u_lb);
+  running_model->set_u_ub(u_ub);
+  terminal_model->set_u_lb(u_lb);
+  terminal_model->set_u_ub(u_ub);
 
-  Eigen::VectorXd x0 = Eigen::VectorXd::Zero(25); // model->nq + model->nv
+  std::vector<boost::shared_ptr<crocoddyl::ActionModelAbstract> > running_models(N, running_model);
+
+  Eigen::VectorXd x0 = Eigen::VectorXd::Zero(model->nq + model->nv);
+  x0(6) = 1.0;
   bool hovering_approximate;
   nhp.getParam("hovering_approximate", hovering_approximate);
   if(hovering_approximate)
     {
       x0(2) = 1.0;
-      x0(6) = 1.0;
     }
 
   boost::shared_ptr<crocoddyl::ShootingProblem> problem = boost::make_shared<crocoddyl::ShootingProblem>(x0, running_models, terminal_model);
 
-  boost::shared_ptr<crocoddyl::SolverFDDP> solver = boost::make_shared<crocoddyl::SolverFDDP>(problem);
-  std::vector<Eigen::VectorXd> xs_init(t, x0);
+  boost::shared_ptr<crocoddyl::SolverAbstract> solver = boost::make_shared<crocoddyl::SolverFDDP>(problem);
+  std::vector<Eigen::VectorXd> xs_init(N, x0);
   std::vector<Eigen::VectorXd> us_init = solver->get_problem()->quasiStatic_xs(xs_init);
   xs_init.push_back(x0);
 
@@ -115,13 +157,12 @@ int main(int argc, char** argv)
   double time = timer.get_duration();
   std::cout << "total calculation time: " << time << "[ms]" << std::endl;
   std::cout << "Number of iterations: " << solver->get_iter() << std::endl;
-  std::cout << "time per iterate :" << time / solver->get_iter() << std::endl;
+  std::cout << "time per iterate: " << time / solver->get_iter() << std::endl;
   std::cout << "Total cost: " << solver->get_cost() << std::endl;
   std::cout << "Gradient norm: " << solver->get_stop() << std::endl;
 
   std::vector<Eigen::VectorXd> xs = solver->get_xs();
   std::vector<Eigen::VectorXd> us = solver->get_us();
-
   for(int i = 0; i < xs.size(); i++)
     {
       std::cout << xs.at(i).transpose() << std::endl;
@@ -131,4 +172,32 @@ int main(int argc, char** argv)
     {
       std::cout << us.at(i).transpose() << std::endl;
     }
+
+  ros::Rate loop_rate(1.0 / dt);
+  int count = 0;
+  while(ros::ok())
+    {
+      Eigen::VectorXd q = xs[count];
+      geometry_msgs::TransformStamped baseState;
+      baseState.header.stamp = ros::Time::now();
+      baseState.header.frame_id = "world";
+      baseState.child_frame_id  = "BODY";
+      baseState.transform.translation.x = q[0];
+      baseState.transform.translation.y = q[1];
+      baseState.transform.translation.z = q[2];
+      baseState.transform.rotation.x = q[3];
+      baseState.transform.rotation.y = q[4];
+      baseState.transform.rotation.z = q[5];
+      baseState.transform.rotation.w = q[6];
+      robot_base_broadcaster.sendTransform(baseState);
+
+      ros::spinOnce();
+      loop_rate.sleep();
+
+      count++;
+      if(count >= xs.size())
+        {
+          count = 0;
+        }
+    }
 }