From 6dfcdafd96f70ad60284056e120f18218a01cf09 Mon Sep 17 00:00:00 2001
From: Andreas Vinter-Hviid <andreas@odico.dk>
Date: Wed, 10 Jul 2019 14:51:25 +0200
Subject: [PATCH] Add IK solutions beyond +-PI for axis 4 and 6

Using code suggested in https://github.com/ros-industrial-consortium/descartes/issues/193
---
 .../src/ikfast_moveit_state_adapter.cpp       | 19 +++++++++++++++++--
 1 file changed, 17 insertions(+), 2 deletions(-)

diff --git a/descartes_moveit/src/ikfast_moveit_state_adapter.cpp b/descartes_moveit/src/ikfast_moveit_state_adapter.cpp
index 2b623ab5..a2c53d29 100644
--- a/descartes_moveit/src/ikfast_moveit_state_adapter.cpp
+++ b/descartes_moveit/src/ikfast_moveit_state_adapter.cpp
@@ -88,10 +88,25 @@ bool descartes_moveit::IkFastMoveitStateAdapter::getAllIK(const Eigen::Isometry3
     return false;
   }
 
+  const auto last_index = 5;
+  const auto index4 = 3;
+
   for (auto& sol : joint_results)
   {
-    if (isValid(sol))
-      joint_poses.push_back(std::move(sol));
+    const double joint6 = sol[last_index];
+    const double joint4 = sol[index4];
+
+    for (int i = -1; i <= 1; ++i)
+    {
+      sol[last_index] = joint6 + i * 2.0 * M_PI;
+      for (int j = -1; j <= 1; ++j)
+      {
+        sol[index4] = joint4 + j * 2.0 * M_PI;
+
+        if (isValid(sol))
+          joint_poses.push_back(sol);
+      }
+}
   }
 
   return joint_poses.size() > 0;