From 42eed992d0cf65078b793765a31d08f599a96d56 Mon Sep 17 00:00:00 2001 From: Eisoku Kuroiwa Date: Fri, 25 Dec 2015 07:12:48 +0900 Subject: [PATCH] [Stabilizer.cpp] fix com height of LIPM in Capture Point calculation --- rtc/Stabilizer/Stabilizer.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rtc/Stabilizer/Stabilizer.cpp b/rtc/Stabilizer/Stabilizer.cpp index ee671ec37b7..afa2b61d14c 100644 --- 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