From 11fb6fe03b37af09b49d7788bd46972b772b1b5d Mon Sep 17 00:00:00 2001
From: Mirko Waechter <mirko.waechter@kit.edu>
Date: Wed, 12 Sep 2018 00:42:29 +0200
Subject: [PATCH] change interpolated solution of limitless joints to actual
defined limits instead of 0-2PI check limits in any case after creation of
random config
---
MotionPlanning/CSpace/CSpace.cpp | 2 ++
MotionPlanning/CSpace/CSpaceSampled.cpp | 2 +-
2 files changed, 3 insertions(+), 1 deletion(-)
diff --git a/MotionPlanning/CSpace/CSpace.cpp b/MotionPlanning/CSpace/CSpace.cpp
index 65c913935..839b63002 100644
--- a/MotionPlanning/CSpace/CSpace.cpp
+++ b/MotionPlanning/CSpace/CSpace.cpp
@@ -625,6 +625,8 @@ namespace Saba
float start = q1[dim];
float end = q2[dim];
float res = interpolateRotational(start, end , step);
+ // change to actual defined borders
+ res = VirtualRobot::MathTools::angleModX(res, 0.5f*(boundaryMax[dim]+boundaryMin[dim]));
return res;
}
diff --git a/MotionPlanning/CSpace/CSpaceSampled.cpp b/MotionPlanning/CSpace/CSpaceSampled.cpp
index 2794ecb0c..91c7119d0 100644
--- a/MotionPlanning/CSpace/CSpaceSampled.cpp
+++ b/MotionPlanning/CSpace/CSpaceSampled.cpp
@@ -208,7 +208,7 @@ namespace Saba
generateNewConfig(goal, tmpConfig, tmpConfig, colCheckDist, dist);
LOCAL_DEBUG("--> " << endl << tmpConfig << endl);
- if (!(isConfigValid(tmpConfig, false, true, true)))
+ if (!(isConfigValid(tmpConfig, true, true, true)))
{
return p;
}
--
GitLab