From 0c07b170c3df99b8e126cf324f8d649d285c7ad7 Mon Sep 17 00:00:00 2001
From: Mirko Waechter <mirko.waechter@kit.edu>
Date: Mon, 13 Aug 2018 19:29:05 +0200
Subject: [PATCH] made enforcement of joint limits optional
---
VirtualRobot/Nodes/RobotNode.cpp | 28 ++++++++++++++++++++--------
VirtualRobot/Nodes/RobotNode.h | 6 +++++-
2 files changed, 25 insertions(+), 9 deletions(-)
diff --git a/VirtualRobot/Nodes/RobotNode.cpp b/VirtualRobot/Nodes/RobotNode.cpp
index 3675b4842..3c9ff5845 100644
--- a/VirtualRobot/Nodes/RobotNode.cpp
+++ b/VirtualRobot/Nodes/RobotNode.cpp
@@ -10,7 +10,7 @@
#include "../XML/BaseIO.h"
#include <cmath>
#include <iomanip>
-
+#include <boost/optional/optional_io.hpp>
#include <algorithm>
#include <Eigen/Core>
@@ -125,6 +125,16 @@ namespace VirtualRobot
}
+ bool RobotNode::getEnforceJointLimits() const
+ {
+ return enforceJointLimits;
+ }
+
+ void RobotNode::setEnforceJointLimits(bool value)
+ {
+ enforceJointLimits = value;
+ }
+
RobotPtr RobotNode::getRobot() const
{
RobotPtr result(robot);
@@ -181,15 +191,17 @@ namespace VirtualRobot
}
else
{
- // non-limitless joint: clamp to borders
- if (q < jointLimitLo)
+ if(enforceJointLimits)// non-limitless joint: clamp to borders
{
- q = jointLimitLo;
- }
+ if (q < jointLimitLo)
+ {
+ q = jointLimitLo;
+ }
- if (q > jointLimitHi)
- {
- q = jointLimitHi;
+ if (q > jointLimitHi)
+ {
+ q = jointLimitHi;
+ }
}
}
diff --git a/VirtualRobot/Nodes/RobotNode.h b/VirtualRobot/Nodes/RobotNode.h
index 042762848..90dd788ab 100644
--- a/VirtualRobot/Nodes/RobotNode.h
+++ b/VirtualRobot/Nodes/RobotNode.h
@@ -339,7 +339,10 @@ namespace VirtualRobot
*/
virtual void setJointValueNoUpdate(float q);
- protected:
+ bool getEnforceJointLimits() const;
+ void setEnforceJointLimits(bool value);
+
+ protected:
/*!
Queries parent for global pose and updates visualization accordingly
*/
@@ -370,6 +373,7 @@ namespace VirtualRobot
float jointValueOffset;
float jointLimitLo, jointLimitHi;
+ bool enforceJointLimits = true;
bool limitless; // whether this joint has limits or not (ignored if nodeType != Joint).
DHParameter optionalDHParameter; // When the joint is defined via DH parameters they are stored here
float maxVelocity; //! given in m/s
--
GitLab