Merge pull request #144 from Pitt-RAS/xy-pterm

P terms on XY position in addition to Z

Author: asaba96

cfg/LowLevelMotion.cfg

@@ -5,7 +5,9 @@ from dynamic_reconfigure.parameter_generator_catkin import *
 
 gen = ParameterGenerator()
 
-gen.add('height_p', double_t, 0, '', 0.0, 0.0, 100.0)
+gen.add('position_p_x', double_t, 0, '', 0.0, 0.0, 100.0)
+gen.add('position_p_y', double_t, 0, '', 0.0, 0.0, 100.0)
+gen.add('position_p_z', double_t, 0, '', 0.0, 0.0, 100.0)
 
 gen.add('throttle_p', double_t, 0, '', 0.0, 0.0, 20.0)
 gen.add('throttle_i', double_t, 0, '',0.0, 0.0, 20.0)

include/iarc7_motion/QuadVelocityController.hpp

@@ -50,7 +50,7 @@ class QuadVelocityController
     QuadVelocityController(double throttle_pid_settings[6],
                            double pitch_pid_settings[6],
                            double roll_pid_settings[6],
-                           double& height_p,
+                           double (&position_p)[3],
                            const ThrustModel& thrust_model,
                            const ThrustModel& thrust_model_side,
                            const ros::Duration& battery_timeout,
@@ -118,8 +118,8 @@ class QuadVelocityController
     // The XY plan mixer to use
     std::string xy_mixer_;
 
-    // P term for the position control
-    double& height_p_;
+    // P terms for the position control
+    double (&position_p_)[3];
 
     // Last time an update was successful
     ros::Time last_update_time_;

param/low_level_motion_2.0.yaml

@@ -1,6 +1,10 @@
+# The PID constants are applied to positional errors in the map frame
+position_p_x: 0.0
+position_p_y: 0.0
+position_p_z: 1.5
+
 # PID values are positive for forward acting PID loops and negative for
 # reverse acting PID loops. P, I, and D should all be the same sign.
-height_p: 1.5
 throttle_p: 2.5
 throttle_i: 0.2
 throttle_d: 0.4

param/low_level_motion_sim.yaml

@@ -1,6 +1,10 @@
+# The PID constants are applied to positional errors in the map frame
+position_p_x: 0.0
+position_p_y: 0.0
+position_p_z: 5.0
+
 # PID values are positive for forward acting PID loops and negative for
 # reverse acting PID loops. P, I, and D should all be the same sign.
-height_p: 5.0
 throttle_p: 5.0
 throttle_i: 0.5
 throttle_d: 0.5

src/LowLevelMotionController.cpp

@@ -93,7 +93,7 @@ int main(int argc, char **argv)
     double throttle_pid[pid_param_array_size];
     double pitch_pid[pid_param_array_size];
     double roll_pid[pid_param_array_size];
-    double height_p = 0.0;
+    double position_p[3];
 
     ThrustModel thrust_model(private_nh, "thrust_model");
     ThrustModel thrust_model_side;
@@ -136,7 +136,9 @@ int main(int argc, char **argv)
             roll_pid[4] = config.roll_accumulator_min;
             roll_pid[5] = config.roll_accumulator_enable_threshold;
 
-            height_p = config.height_p;
+            position_p[0] = config.position_p_x;
+            position_p[1] = config.position_p_y;
+            position_p[2] = config.position_p_z;
 
             dynamic_reconfigure_called = true;
         };
@@ -190,7 +192,7 @@ int main(int argc, char **argv)
     QuadVelocityController quadController(throttle_pid,
                                           pitch_pid,
                                           roll_pid,
-                                          height_p,
+                                          position_p,
                                           thrust_model,
                                           thrust_model_side,
                                           ros::Duration(battery_timeout),
@@ -292,7 +294,7 @@ int main(int argc, char **argv)
 
             //cancellation inputs
             if(server.isPreemptRequested()) {
-                
+
                 if(motion_state == MotionState::PASSTHROUGH) {
                     ROS_INFO("Transitioning out of PASSTHROUGH");
                     bool success = quadController.prepareForTakeover();
@@ -391,7 +393,7 @@ int main(int argc, char **argv)
             iarc7_msgs::OrientationThrottleStamped uav_command;
 
             // Check for a safety state in which case we should execute our safety response
-            if(safety_client.isSafetyActive() 
+            if(safety_client.isSafetyActive()
                && !safety_client.isSafetyResponseActive())
             {
                 // This is the safety response
@@ -466,7 +468,7 @@ int main(int argc, char **argv)
                 if (last_msg != nullptr && last_msg->header.stamp >= passthrough_start_time) {
                     geometry_msgs::Twist twist;
                     twist.linear.z = last_msg->throttle;
-                    
+
                     MotionPointStamped motion_point;
                     motion_point.motion_point.twist.linear.z = twist.linear.z;
 

src/QuadVelocityController.cpp

@@ -34,7 +34,7 @@ QuadVelocityController::QuadVelocityController(
         double vz_pid_settings[6],
         double vx_pid_settings[6],
         double vy_pid_settings[6],
-        double& height_p,
+        double (&position_p)[3],
         const ThrustModel& thrust_model,
         const ThrustModel& thrust_model_side,
         const ros::Duration& battery_timeout,
@@ -59,7 +59,7 @@ QuadVelocityController::QuadVelocityController(
       xy_mixer_(ros_utils::ParamUtils::getParam<std::string>(
               private_nh,
               "xy_mixer")),
-      height_p_(height_p),
+      position_p_(position_p),
       startup_timeout_(ros_utils::ParamUtils::getParam<double>(
               private_nh,
               "startup_timeout")),
@@ -237,7 +237,7 @@ bool QuadVelocityController::update(const ros::Time& time,
     double local_y_setpoint_accel = -std::sin(current_yaw) * setpoint_accel.x
                                   +  std::cos(current_yaw) * setpoint_accel.y;
 
-    if (level_flight_active_ && col_height 
+    if (level_flight_active_ && col_height
                                   > level_flight_required_height_
                                     + level_flight_required_hysteresis_) {
         level_flight_active_ = false;
@@ -428,19 +428,25 @@ bool QuadVelocityController::waitUntilReady()
 
 void QuadVelocityController::updatePidSetpoints(double current_yaw, const Eigen::VectorXd& odometry)
 {
-    double position_velocity_request = height_p_ * 
-      (setpoint_.motion_point.pose.position.z - odometry[5]);
+    double position_velocity_request[3] = {
+        position_p_[0] * (setpoint_.motion_point.pose.position.x - odometry[3]),
+        position_p_[1] * (setpoint_.motion_point.pose.position.y - odometry[4]),
+        position_p_[2] * (setpoint_.motion_point.pose.position.z - odometry[5])
+    };
 
-    double velocity_request = position_velocity_request + setpoint_.motion_point.twist.linear.z;
+    double map_z_velocity = position_velocity_request[2] + setpoint_.motion_point.twist.linear.z;
 
-    vz_pid_.setSetpoint(velocity_request);
+    vz_pid_.setSetpoint(map_z_velocity);
 
     // x and y velocities are transformed according to the last yaw
     // angle because the incoming target velocities are in the map frame
-    double local_x_velocity = setpoint_.motion_point.twist.linear.x * std::cos(current_yaw)
-                            + setpoint_.motion_point.twist.linear.y * std::sin(current_yaw);
-    double local_y_velocity = setpoint_.motion_point.twist.linear.x * -std::sin(current_yaw)
-                            + setpoint_.motion_point.twist.linear.y *  std::cos(current_yaw);
+    double map_x_velocity = setpoint_.motion_point.twist.linear.x + position_velocity_request[0];
+    double map_y_velocity = setpoint_.motion_point.twist.linear.y + position_velocity_request[1];
+
+    double local_x_velocity = map_x_velocity * std::cos(current_yaw)
+                            + map_y_velocity * std::sin(current_yaw);
+    double local_y_velocity = map_x_velocity * -std::sin(current_yaw)
+                            + map_y_velocity *  std::cos(current_yaw);
 
     vx_pid_.setSetpoint(local_x_velocity);
     vy_pid_.setSetpoint(local_y_velocity);