diff --git a/march_gazebo_plugins/src/com_controller.cpp b/march_gazebo_plugins/src/com_controller.cpp index e965cf5..00c3e38 100644 --- a/march_gazebo_plugins/src/com_controller.cpp +++ b/march_gazebo_plugins/src/com_controller.cpp @@ -32,7 +32,7 @@ class ComController : public ModelPlugin this->p_pitch = 60; this->d_pitch = 10; this->p_roll = 150; - this->d_roll = 7.5; + this->d_roll = 0; this->p_yaw = 500; this->d_yaw = 25; @@ -107,6 +107,11 @@ class ComController : public ModelPlugin goal_position_x += 0.25 * swing_step_size - 0.25 * time_since_start * swing_step_size / subgait_duration; } + if (time_since_start > subgait_duration) + { + this->subgait_name = "home_stand"; + } + double error_x = model_com.X() - goal_position_x; double error_y = model_com.Y() - goal_position_y; double error_yaw = this->foot_left->WorldPose().Rot().Z();