From c508a428021d9345196053883297d9e6ce0729fa Mon Sep 17 00:00:00 2001 From: Yoann Solana Date: Tue, 27 Jun 2017 18:02:59 +0200 Subject: [PATCH] [nextage_ik_plugin] In harmonize function: Limit angles to [-2*PI; 2*PI] --- nextage_ik_plugin/src/nextage_left_arm_ikfast_moveit_plugin.cpp | 2 +- .../src/nextage_right_arm_ikfast_moveit_plugin.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/nextage_ik_plugin/src/nextage_left_arm_ikfast_moveit_plugin.cpp b/nextage_ik_plugin/src/nextage_left_arm_ikfast_moveit_plugin.cpp index 230ae7a0..e2912b09 100644 --- a/nextage_ik_plugin/src/nextage_left_arm_ikfast_moveit_plugin.cpp +++ b/nextage_ik_plugin/src/nextage_left_arm_ikfast_moveit_plugin.cpp @@ -479,7 +479,7 @@ double IKFastKinematicsPlugin::harmonize(const std::vector &ik_seed_stat while(solution[i] > 2*M_PI) { solution[i] -= 2*M_PI; } - while(solution[i] < 2*M_PI) { + while(solution[i] < -2*M_PI) { solution[i] += 2*M_PI; } dist_sqr += fabs(ik_seed_state[i] - solution[i]); diff --git a/nextage_ik_plugin/src/nextage_right_arm_ikfast_moveit_plugin.cpp b/nextage_ik_plugin/src/nextage_right_arm_ikfast_moveit_plugin.cpp index dde2de11..e4d89d6c 100644 --- a/nextage_ik_plugin/src/nextage_right_arm_ikfast_moveit_plugin.cpp +++ b/nextage_ik_plugin/src/nextage_right_arm_ikfast_moveit_plugin.cpp @@ -479,7 +479,7 @@ double IKFastKinematicsPlugin::harmonize(const std::vector &ik_seed_stat while(solution[i] > 2*M_PI) { solution[i] -= 2*M_PI; } - while(solution[i] < 2*M_PI) { + while(solution[i] < -2*M_PI) { solution[i] += 2*M_PI; } dist_sqr += fabs(ik_seed_state[i] - solution[i]);