From b8c9beb055a60f0fa81c3cc657a0bb5b5277dd06 Mon Sep 17 00:00:00 2001 From: Bin-Ro <75170912+Bin-Ro@users.noreply.github.com> Date: Tue, 17 Jan 2023 23:22:58 -0500 Subject: [PATCH] Update example_position.cpp fix bug which is desired joint position is not continuous --- .../src/exe/example_position.cpp | 19 ++++++++++--------- 1 file changed, 10 insertions(+), 9 deletions(-) diff --git a/unitree_legged_real/src/exe/example_position.cpp b/unitree_legged_real/src/exe/example_position.cpp index e687bbb..2697cae 100644 --- a/unitree_legged_real/src/exe/example_position.cpp +++ b/unitree_legged_real/src/exe/example_position.cpp @@ -75,7 +75,7 @@ int main(int argc, char **argv) qInit[1] = low_state_ros.motorState[FR_1].q; qInit[2] = low_state_ros.motorState[FR_2].q; } - else if(motiontime > 10 && motiontime < 1000) + else if(motiontime > 10 && motiontime < 2500) { Kp[0] = 20.0; Kp[1] = 20.0; @@ -84,19 +84,20 @@ int main(int argc, char **argv) Kd[1] = 2.0; Kd[2] = 2.0; - const float interval = 800.0; + const float interval = 2000.0; + float phase = std::min(std::max((motiontime - 10) / interval, 0.0f), 1.0f); - qDes[0] = (1 - (motiontime - 10) / interval) * qInit[0] + (motiontime - 10) / interval * sin_mid_q[0]; - qDes[1] = (1 - (motiontime - 10) / interval) * qInit[1] + (motiontime - 10) / interval * sin_mid_q[1]; - qDes[2] = (1 - (motiontime - 10) / interval) * qInit[2] + (motiontime - 10) / interval * sin_mid_q[2]; + qDes[0] = (1 - phase) * qInit[0] + phase * sin_mid_q[0]; + qDes[1] = (1 - phase) * qInit[1] + phase * sin_mid_q[1]; + qDes[2] = (1 - phase) * qInit[2] + phase * sin_mid_q[2]; } - else if(motiontime >= 1000) + else if(motiontime >= 2500) { float period = 5.0; qDes[0] = sin_mid_q[0]; - qDes[1] = sin_mid_q[1] + 0.6 * std::sin(2 * M_PI / period * (motiontime - 400) / 1000.0); - qDes[2] = sin_mid_q[2] + (-0.9) * std::sin(2 * M_PI / period * (motiontime - 400) / 1000.0); + qDes[1] = sin_mid_q[1] + 0.6 * std::sin(2 * M_PI / period * (motiontime - 2500) / 1000.0); + qDes[2] = sin_mid_q[2] + (-0.9) * std::sin(2 * M_PI / period * (motiontime - 2500) / 1000.0); } @@ -126,4 +127,4 @@ int main(int argc, char **argv) } return 0; -} \ No newline at end of file +}