[Stabilizer.cpp] fix com height of LIPM in Capture Point calculation

snozawa · Dec 25, 2015 · 42eed99 · 42eed99
1 parent 05e5eba
commit 42eed99
Showing 1 changed file with 2 additions and 2 deletions.
diff --git a/rtc/Stabilizer/Stabilizer.cpp b/rtc/Stabilizer/Stabilizer.cpp
@@ -740,7 +740,7 @@ void Stabilizer::getActualParameters ()
  target_ee_diff_p[i] -= foot_origin_rot.transpose() * (act_ee_p - foot_origin_pos);
  }
  // capture point
- act_cp = act_cog + act_cogvel / std::sqrt(eefm_gravitational_acceleration / act_cog(2));
+ act_cp = act_cog + act_cogvel / std::sqrt(eefm_gravitational_acceleration / (act_cog - act_zmp)(2));
  rel_act_cp = hrp::Vector3(act_cp(0), act_cp(1), act_zmp(2));
  rel_act_cp = m_robot->rootLink()->R.transpose() * ((foot_origin_pos + foot_origin_rot * rel_act_cp) - m_robot->rootLink()->p);
  // <= Actual foot_origin frame
@@ -1034,7 +1034,7 @@ void Stabilizer::getTargetParameters ()
  }
  target_foot_origin_rot = foot_origin_rot;
  // capture point
- ref_cp = ref_cog + ref_cogvel / std::sqrt(eefm_gravitational_acceleration / ref_cog(2));
+ ref_cp = ref_cog + ref_cogvel / std::sqrt(eefm_gravitational_acceleration / (ref_cog - ref_zmp)(2));
  rel_ref_cp = hrp::Vector3(ref_cp(0), ref_cp(1), ref_zmp(2));
  rel_ref_cp = m_robot->rootLink()->R.transpose() * ((foot_origin_pos + foot_origin_rot * rel_ref_cp) - m_robot->rootLink()->p);
  // <= Reference foot_origin frame