[spinal][control] support multiple type of thrust motors

aerial_robot_control/include/aerial_robot_control/control/base/base.h

@@ -40,7 +40,7 @@
 #include <aerial_robot_estimation/state_estimation.h>
 #include <aerial_robot_model/model/aerial_robot_model.h>
 #include <ros/ros.h>
-#include <spinal/PwmInfo.h>
+#include <spinal/PwmInfos.h>
 #include <spinal/UavInfo.h>
 
 namespace aerial_robot_control
@@ -61,7 +61,7 @@ namespace aerial_robot_control
     {
       nh_ = nh;
       nhp_ = nhp;
-      motor_info_pub_ = nh_.advertise<spinal::PwmInfo>("motor_info", 10);
+      motor_info_pub_ = nh_.advertise<spinal::PwmInfos>("motor_info", 10);
       uav_info_pub_ = nh_.advertise<spinal::UavInfo>("uav_info", 10);
 
       robot_model_ = robot_model;
@@ -80,34 +80,57 @@ namespace aerial_robot_control
       getParam<bool>(control_nh, "control_verbose", control_verbose_, false);
 
       ros::NodeHandle motor_nh(nh_, "motor_info");
-      getParam<double>(motor_nh, "max_pwm", max_pwm_, 0.0);
-      getParam<double>(motor_nh, "min_pwm", min_pwm_, 0.0);
-      getParam<double>(motor_nh, "min_thrust", min_thrust_, 0.0);
-      getParam<double>(motor_nh, "force_landing_thrust", force_landing_thrust_, 0.0);
-      getParam<double>(motor_nh, "m_f_rate", m_f_rate_, 0.0);
-      getParam<int>(motor_nh, "pwm_conversion_mode", pwm_conversion_mode_, -1);
-
-      int vel_ref_num;
-      getParam<int>(motor_nh, "vel_ref_num", vel_ref_num, 0);
-      motor_info_.resize(vel_ref_num);
-      for(int i = 0; i < vel_ref_num; i++)
+      motor_types_.resize(0);
+      int motor_type_max = 0;
+      if(motor_nh.hasParam("motor_types"))
         {
-          std::stringstream ss;
-          ss << i + 1;
-          double val;
-          ros::NodeHandle nh(motor_nh, "ref" + ss.str());
-          getParam<double>(nh, "voltage", val, 0);
-          motor_info_[i].voltage = val;
-          nh.param("max_thrust", val, 0.0);
-          motor_info_[i].max_thrust = val;
-
-          /* hardcode: up to 4 dimension */
-          for(int j = 0; j < 5; j++)
+          motor_nh.getParam("motor_types", motor_types_);
+          for(int i = 0; i < motor_types_.size(); i++)
+            if(motor_types_[i] > motor_type_max) motor_type_max = motor_types_[i]; // assume motor type is started from 0 and continuous
+        }
+
+      max_pwm_.resize(motor_type_max + 1, 0.0);
+      min_pwm_.resize(motor_type_max + 1, 0.0);
+      min_thrust_.resize(motor_type_max + 1, 0.0);
+      motor_info_.resize(motor_type_max + 1);
+      force_landing_thrust_.resize(motor_type_max + 1, 0.0);
+      pwm_conversion_mode_.resize(motor_type_max + 1, spinal::MotorInfo::POLYNOMINAL_MODE);
+
+      for(int i = 0; i < motor_type_max + 1; i++)
+        {
+          std::stringstream ss1;
+          if(i != 0) ss1 << i; // use "motor_info" for type 0, "motor_info1" for type 1, etc.
+          ros::NodeHandle motor_type_nh(nh_, "motor_info" + ss1.str());
+
+          getParam<double>(motor_type_nh, "max_pwm", max_pwm_[i], 0.0);
+          getParam<double>(motor_type_nh, "min_pwm", min_pwm_[i], 0.0);
+          getParam<double>(motor_type_nh, "min_thrust", min_thrust_[i], 0.0);
+          getParam<double>(motor_type_nh, "force_landing_thrust", force_landing_thrust_[i], 0.0);
+          getParam<int>(motor_type_nh, "pwm_conversion_mode", pwm_conversion_mode_[i], -1);
+
+          int vel_ref_num;
+          getParam<int>(motor_type_nh, "vel_ref_num", vel_ref_num, 0);
+          motor_info_.at(i).resize(vel_ref_num);
+
+          for(int j = 0; j < vel_ref_num; j++)
             {
               std::stringstream ss2;
-              ss2 << j;
-              getParam<double>(nh, "polynominal" + ss2.str(), val, 0);
-              motor_info_[i].polynominal[j] = val;
+              ss2 << j + 1; // use "ref1", "ref2", etc.
+              double val;
+              ros::NodeHandle nh(motor_type_nh, "ref" + ss2.str());
+              getParam<double>(nh, "voltage", val, 0);
+              motor_info_[i][j].voltage = val;
+              nh.param("max_thrust", val, 0.0);
+              motor_info_[i][j].max_thrust = val;
+
+              /* hardcode: up to 4 dimension */
+              for(int k = 0; k < 5; k++)
+                {
+                  std::stringstream ss3;
+                  ss3 << k;
+                  getParam<double>(nh, "polynominal" + ss3.str(), val, 0);
+                  motor_info_[i][j].polynominal[k] = val;
+                }
             }
         }
     }
@@ -141,16 +164,23 @@ namespace aerial_robot_control
       if(ros::Time::now().toSec() - activate_timestamp_  > 0.1)
         {
           /* send motor and uav info to uav, about 10Hz */
-          spinal::PwmInfo motor_info_msg;
-          motor_info_msg.max_pwm = max_pwm_;
-          motor_info_msg.min_pwm = min_pwm_;
-          motor_info_msg.min_thrust = min_thrust_;
-          motor_info_msg.force_landing_thrust = force_landing_thrust_;
-          motor_info_msg.pwm_conversion_mode = pwm_conversion_mode_;
-          motor_info_msg.motor_info.resize(0);
+          spinal::PwmInfos pwm_infos;
+          for(int i = 0; i < motor_types_.size(); i++)
+            pwm_infos.motor_types.push_back(motor_types_[i]);
           for(int i = 0; i < motor_info_.size(); i++)
-            motor_info_msg.motor_info.push_back(motor_info_[i]);
-          motor_info_pub_.publish(motor_info_msg);
+            {
+              spinal::PwmInfo motor_info_msg;
+              motor_info_msg.max_pwm = max_pwm_[i];
+              motor_info_msg.min_pwm = min_pwm_[i];
+              motor_info_msg.min_thrust = min_thrust_[i];
+              motor_info_msg.force_landing_thrust = force_landing_thrust_[i];
+              motor_info_msg.pwm_conversion_mode = pwm_conversion_mode_[i];
+              motor_info_msg.motor_info.resize(0);
+              for(int j = 0; j < motor_info_.at(i).size(); j++)
+                motor_info_msg.motor_info.push_back(motor_info_[i][j]);
+              pwm_infos.pwm_infos.push_back(motor_info_msg);
+            }
+          motor_info_pub_.publish(pwm_infos);
 
           spinal::UavInfo uav_info_msg;
           uav_info_msg.motor_num = motor_num_;
@@ -186,12 +216,12 @@ namespace aerial_robot_control
     int uav_model_;
 
     double m_f_rate_;
-    double max_pwm_, min_pwm_;
-    double min_thrust_;
-    std::vector<spinal::MotorInfo> motor_info_;
-
-    double force_landing_thrust_; //pwm
-    int pwm_conversion_mode_;
+    std::vector<int> motor_types_;
+    std::vector<double> max_pwm_, min_pwm_;
+    std::vector<double> min_thrust_;
+    std::vector<std::vector<spinal::MotorInfo>> motor_info_;
+    std::vector<double> force_landing_thrust_; //pwm
+    std::vector<int> pwm_conversion_mode_;
 
     int estimate_mode_;
     bool param_verbose_;

aerial_robot_model/include/aerial_robot_model/model/aerial_robot_model.h

@@ -140,13 +140,13 @@ namespace aerial_robot_model {
 
     const Eigen::VectorXd& getGravity() const {return gravity_;}
     const Eigen::VectorXd& getGravity3d() const {return gravity_3d_;}
-    const double getMFRate() const  {return m_f_rate_;}
+    const double getMFRate(int index = 0) const  {return m_f_rate_.at(index);}
     const Eigen::VectorXd& getStaticThrust() const {return static_thrust_;}
     const std::vector<Eigen::MatrixXd>& getThrustWrenchAllocations() const {return thrust_wrench_allocations_;}
     const Eigen::MatrixXd& getThrustWrenchMatrix() const {return q_mat_;}
     const std::vector<Eigen::VectorXd>& getThrustWrenchUnits() const {return thrust_wrench_units_;}
-    const double getThrustUpperLimit() const {return thrust_max_;}
-    const double getThrustLowerLimit() const {return thrust_min_;}
+    const double getThrustUpperLimit(int index = 0) const {return thrust_max_.at(index);}
+    const double getThrustLowerLimit(int index = 0) const {return thrust_min_.at(index);}
 
     // control stability
     virtual void calcFeasibleControlFDists();
@@ -203,12 +203,12 @@ namespace aerial_robot_model {
     // statics (static thrust, joint torque)
     Eigen::VectorXd gravity_;
     Eigen::VectorXd gravity_3d_;
-    double m_f_rate_; //moment / force rate
+    std::vector<double> m_f_rate_; //moment / force rate
     Eigen::MatrixXd q_mat_;
     std::map<int, int> rotor_direction_;
     Eigen::VectorXd static_thrust_;
-    double thrust_max_;
-    double thrust_min_;
+    std::vector<double> thrust_max_;
+    std::vector<double> thrust_min_;
     std::vector<Eigen::VectorXd> thrust_wrench_units_;
     std::vector<Eigen::MatrixXd> thrust_wrench_allocations_;
 

aerial_robot_model/src/model/base_model/robot_model.cpp

@@ -110,22 +110,36 @@ namespace aerial_robot_model {
 
     /* set rotor property */
     TiXmlElement* m_f_rate_attr = robot_model_xml.FirstChildElement("robot")->FirstChildElement("m_f_rate");
+    double m_f_rate;
     if(!m_f_rate_attr)
       ROS_ERROR("Can not get m_f_rate attribute from urdf model");
     else
-      m_f_rate_attr->Attribute("value", &m_f_rate_);
+      m_f_rate_attr->Attribute("value", &m_f_rate);
+    m_f_rate_.resize(rotor_num_, m_f_rate);
 
     std::vector<urdf::LinkSharedPtr> urdf_links;
     model_.getLinks(urdf_links);
-    for(const auto& link: urdf_links)
+    thrust_max_.resize(rotor_num_, 0);
+    thrust_min_.resize(rotor_num_, 0);
+    for(int i = 0; i < rotor_num_; i++)
       {
-        if(link->parent_joint)
+        std::string rotor_name = "rotor" + std::to_string(i + 1);
+        for(const auto& link: urdf_links)
           {
-            if(link->parent_joint->name == "rotor1")
+            if(link->parent_joint)
               {
-                thrust_max_ = link->parent_joint->limits->upper;
-                thrust_min_ = link->parent_joint->limits->lower;
-                break;
+                if(link->parent_joint->name == rotor_name)
+                  {
+                    double thrust_max = link->parent_joint->limits->upper;
+                    double thrust_min = link->parent_joint->limits->lower;
+                    thrust_max_.at(i) = thrust_max;
+                    thrust_min_.at(i) = thrust_min;
+
+                    double m_f_rate = link->parent_joint->limits->effort;
+                    if(m_f_rate != 0) // use the value from urdf if it is set as 0
+                      m_f_rate_.at(i) = -std::abs(m_f_rate); // sign should be resolved by rotor direction
+                    break;
+                  }
               }
           }
       }
@@ -444,13 +458,12 @@ namespace aerial_robot_model {
     const std::vector<Eigen::Vector3d> u = getRotorsNormalFromCog<Eigen::Vector3d>();
     const auto& sigma = getRotorDirection();
     const int rotor_num = getRotorNum();
-    const double m_f_rate = getMFRate();
 
     //Q : WrenchAllocationMatrix
     Eigen::MatrixXd Q(6, rotor_num);
     for (unsigned int i = 0; i < rotor_num; ++i) {
       Q.block(0, i, 3, 1) = u.at(i);
-      Q.block(3, i, 3, 1) = p.at(i).cross(u.at(i)) + m_f_rate * sigma.at(i + 1) * u.at(i);
+      Q.block(3, i, 3, 1) = p.at(i).cross(u.at(i)) + m_f_rate_.at(i) * sigma.at(i + 1) * u.at(i);
     }
     return Q;
   }
@@ -461,7 +474,6 @@ namespace aerial_robot_model {
     const std::vector<Eigen::Vector3d>& u = getRotorsNormalFromCog<Eigen::Vector3d>();
     const auto& sigma = getRotorDirection();
     const int rotor_num = getRotorNum();
-    const double m_f_rate = getMFRate();
 
     Eigen::MatrixXd root_rot = aerial_robot_model::kdlToEigen(getCogDesireOrientation<KDL::Rotation>() * seg_frames.at(baselink_).M.Inverse());
 
@@ -474,7 +486,7 @@ namespace aerial_robot_model {
 
       Eigen::VectorXd wrench_unit = Eigen::VectorXd::Zero(6);
       wrench_unit.head(3) = u.at(i);
-      wrench_unit.tail(3) = m_f_rate * sigma.at(i + 1) * u.at(i);
+      wrench_unit.tail(3) = m_f_rate_.at(i) * sigma.at(i + 1) * u.at(i);
 
       thrust_wrench_units_.at(i) = wrench_unit;
       thrust_wrench_allocations_.at(i) = q_i;
@@ -506,11 +518,14 @@ namespace aerial_robot_model {
         return false;
       }
 
-    if(static_thrust_.maxCoeff() > thrust_max_ || static_thrust_.minCoeff() < thrust_min_)
+    for(int i = 0; i < rotor_num_; i++)
       {
-        if(verbose)
-          ROS_ERROR("Invalid static thrust, max: %f, min: %f", static_thrust_.maxCoeff(), static_thrust_.minCoeff());
-        return false;
+        if(static_thrust_(i) > thrust_max_.at(i) || static_thrust_(i) < thrust_min_.at(i))
+          {
+            if(verbose)
+              ROS_ERROR("the static thrust of rotor %d is invalid, thrust: %f, min: %f, max: %f", i + 1, static_thrust_(i), thrust_min_.at(i), thrust_max_.at(i));
+            return false;
+          }
       }
 
     return true;
@@ -531,11 +546,10 @@ namespace aerial_robot_model {
     const std::vector<Eigen::Vector3d> u = getRotorsNormalFromCog<Eigen::Vector3d>();
     const auto& sigma = getRotorDirection();
     const int rotor_num = getRotorNum();
-    const double m_f_rate = getMFRate();
     std::vector<Eigen::Vector3d> v(rotor_num);
 
     for (int i = 0; i < rotor_num; ++i)
-      v.at(i) = p.at(i).cross(u.at(i)) + m_f_rate * sigma.at(i + 1) * u.at(i);
+      v.at(i) = p.at(i).cross(u.at(i)) + m_f_rate_.at(i) * sigma.at(i + 1) * u.at(i);
     return v;
   }
 

aerial_robot_nerve/spinal/CMakeLists.txt

@@ -37,6 +37,7 @@ add_message_files(
   RollPitchYawTerms.msg
   MotorInfo.msg
   PwmInfo.msg
+  PwmInfos.msg
   Pwms.msg
   PwmTest.msg
   UavInfo.msg