Merge pull request fkanehiro#1082 from snozawa/fix_st_rpy_bug

Fix st damping compensation bug. Enable yaw rotation.
k-okada · Dec 16, 2016 · 197bb08 · 197bb08
2 parents ddf8c61 + 467e5b9
commit 197bb08
Showing 1 changed file with 1 addition and 1 deletion.
diff --git a/rtc/Stabilizer/Stabilizer.cpp b/rtc/Stabilizer/Stabilizer.cpp
@@ -1509,7 +1509,7 @@ void Stabilizer::calcEEForceMomentControl() {
         if (is_ik_enable[i]) {
           // Add damping_control compensation to target value
           if (is_feedback_control_enable[i]) {
-            rats::rotm3times(tmpR[i], target_ee_R[i], hrp::rotFromRpy(-stikp[i].ee_d_foot_rpy(0), -stikp[i].ee_d_foot_rpy(1), 0));
+            rats::rotm3times(tmpR[i], target_ee_R[i], hrp::rotFromRpy(-1*stikp[i].ee_d_foot_rpy));
             // foot force difference control version
             // total_target_foot_p[i](2) = target_foot_p[i](2) + (i==0?0.5:-0.5)*zctrl;
             // foot force independent damping control