diff --git a/MotionPlanning/CSpace/CSpace.cpp b/MotionPlanning/CSpace/CSpace.cpp
index fecac18d4d0b87bb2ebc1f38d73b4d15bca033ad..fb511271a2677dfb2aa7bcc290992f2916d162c1 100644
--- a/MotionPlanning/CSpace/CSpace.cpp
+++ b/MotionPlanning/CSpace/CSpace.cpp
@@ -629,11 +629,13 @@ namespace Saba
else
{
// borderless mode
-
+ // we map 'start' and 'end' to [0,2pi] (temporarily)
float start = q1[dim] - robotJoints[dim]->getJointLimitLo();
float end = q2[dim] - robotJoints[dim]->getJointLimitLo();
float res = interpolateRotational(start, end , step);
+ // using fmod is okay here, because 'start' and 'end' where mapped to [0,2pi] and therefore 'res' is also in [0,2pi]
res = (float)fmod((double)res, 2.0 * M_PI);
+ // map back to original interval
res = res + robotJoints[dim]->getJointLimitLo();
return res;
}
@@ -834,16 +836,15 @@ namespace Saba
{
SABA_ASSERT(dim < dimension)
- if (!(robotJoints[dim]->isTranslationalJoint()))
- {
- // rotational joint
- if (abs((double)(robotJoints[dim]->getJointLimitHi() - robotJoints[dim]->getJointLimitLo())) > (1.9999999999999 * M_PI))
- {
- return true;
- }
- }
-
- return false;
+// if (!(robotJoints[dim]->isTranslationalJoint()))
+// {
+// // rotational joint
+// if (abs((double)(robotJoints[dim]->getJointLimitHi() - robotJoints[dim]->getJointLimitLo())) > (1.9999999999999 * M_PI))
+// {
+// return true;
+// }
+// }
+ return robotJoints[dim]->isLimitless();
}
unsigned int CSpace::getDimension() const
diff --git a/MotionPlanning/CSpace/CSpace.h b/MotionPlanning/CSpace/CSpace.h
index 12be49ea7e93194e573ca4173418360654c8898b..7266c19713725346e6e62246ecb7b30302f0c8b6 100644
--- a/MotionPlanning/CSpace/CSpace.h
+++ b/MotionPlanning/CSpace/CSpace.h
@@ -289,7 +289,7 @@ namespace Saba
/*!
Interpolates between two values in dimension dim.
Checks weather the corresponding joint moves translational or a rotational and performs the interpolation accordingly.
- When joint boundaries of a rotational joint are >= 2PI (and checkForBorderlessDimensions was not disabled), the correct direction for interpolating is determined automatically.
+ When a rotational joint is limitless (and checkForBorderlessDimensions was not disabled), the correct direction for interpolating is determined automatically.
*/
float interpolate(const Eigen::VectorXf& q1, const Eigen::VectorXf& q2, int dim, float step);
Eigen::VectorXf interpolate(const Eigen::VectorXf& q1, const Eigen::VectorXf& q2, float step);
@@ -323,7 +323,8 @@ namespace Saba
//! interpolates linear between a and b using step as step size
float interpolateLinear(float a, float b, float step);
- //! interpolates rotational between a and b using step as step size
+ //! interpolates rotational between a and b using step as step size ('a' and 'b' are expected to be in [0,2pi]).
+ //! The interpolation takes the direction of the shorter path between 'a' and 'b'.
float interpolateRotational(float a, float b, float step);
unsigned int dimension; //!< dimension of this c-space
diff --git a/MotionPlanning/CSpace/CSpacePath.cpp b/MotionPlanning/CSpace/CSpacePath.cpp
index 353f80018ed2b619d82433c7c1fae575009092cd..22edc4a40020b8d802c073809ab80db94371d96c 100644
--- a/MotionPlanning/CSpace/CSpacePath.cpp
+++ b/MotionPlanning/CSpace/CSpacePath.cpp
@@ -382,12 +382,12 @@ namespace Saba
{
float diff = (c2[j] - c1[j]);
- if (diff > M_PI)
+ if (diff > M_PI) // TODO maybe better: while (diff > M_PI) diff -= M_PI * 2.0f;
{
diff -= (float)M_PI * 2.0f;
}
- if (diff < -M_PI)
+ if (diff < -M_PI) // TODO same as above
{
diff += (float)M_PI * 2.0f;
}
diff --git a/MotionPlanning/Planner/Rrt.cpp b/MotionPlanning/Planner/Rrt.cpp
index bf9bea2eb61596c45571508aaeb5c07af24fa402..fb94168991a571564af29aa29bcbc166a744730a 100644
--- a/MotionPlanning/Planner/Rrt.cpp
+++ b/MotionPlanning/Planner/Rrt.cpp
@@ -263,7 +263,8 @@ namespace Saba
{
float factor = extendStepSize / totalLength;
// go a specific length in the direction of randomly found configuration
- tmpConfig = nn->configuration + ((c - nn->configuration) * factor);
+ tmpConfig = cspace->interpolate(nn->configuration, c, factor);
+ //tmpConfig = nn->configuration + ((c - nn->configuration) * factor);
//for (unsigned int i = 0; i < m_nDimension; i++)
// m_pExtendRrtNewValue[i] = m_nnNode->configuration[i] + ((extGoal[i] - m_nnNode->configuration[i]) * factor);
}
diff --git a/VirtualRobot/Nodes/RobotNode.cpp b/VirtualRobot/Nodes/RobotNode.cpp
index 51570cdde85e90f8122d15bcf6aabfffde0625d9..363031fcc5bc9e533b6b4c1c929fc8723634b88d 100644
--- a/VirtualRobot/Nodes/RobotNode.cpp
+++ b/VirtualRobot/Nodes/RobotNode.cpp
@@ -13,10 +13,6 @@
#include <algorithm>
-
-
-
-
#include <Eigen/Core>
using namespace boost;
@@ -51,6 +47,7 @@ namespace VirtualRobot
optionalDHParameter.isSet = false;
//globalPosePostJoint = Eigen::Matrix4f::Identity();
jointValue = 0.0f;
+ limitless = false;
}
@@ -147,16 +144,22 @@ namespace VirtualRobot
{
VR_ASSERT_MESSAGE((!boost::math::isnan(q) && !boost::math::isinf(q)) , "Not a valid number...");
- //std::cout << "######## Setting Joint to: " << q << " degrees" << std::endl;
-
- if (q < jointLimitLo)
+ if (limitless)
{
- q = jointLimitLo;
+ while (q > jointLimitHi) q -= 2.0f * M_PI;
+ while (q < jointLimitLo) q += 2.0f * M_PI;
}
-
- if (q > jointLimitHi)
+ else
{
- q = jointLimitHi;
+ if (q < jointLimitLo)
+ {
+ q = jointLimitLo;
+ }
+
+ if (q > jointLimitHi)
+ {
+ q = jointLimitHi;
+ }
}
jointValue = q;
@@ -166,16 +169,24 @@ namespace VirtualRobot
VR_ASSERT_MESSAGE(initialized, "Not initialized");
VR_ASSERT_MESSAGE((!boost::math::isnan(q) && !boost::math::isinf(q)) , "Not a valid number...");
- //std::cout << "######## Setting Joint to: " << q << " degrees" << std::endl;
-
- if (q < jointLimitLo)
+ if (limitless)
{
- q = jointLimitLo;
+ // limitless joint: map q to allowed interval
+ while (q > jointLimitHi) q -= 2.0f * M_PI;
+ while (q < jointLimitLo) q += 2.0f * M_PI;
}
-
- if (q > jointLimitHi)
+ else
{
- q = jointLimitHi;
+ // non-limitless joint: clamp to borders
+ if (q < jointLimitLo)
+ {
+ q = jointLimitLo;
+ }
+
+ if (q > jointLimitHi)
+ {
+ q = jointLimitHi;
+ }
}
jointValue = q;
@@ -359,7 +370,8 @@ namespace VirtualRobot
physics.print();
- cout << "* Limits: Lo:" << jointLimitLo << ", Hi:" << jointLimitHi << endl;
+ cout << "* Limits: Lo: " << jointLimitLo << ", Hi: " << jointLimitHi << endl;
+ cout << "* Limitless: " << (limitless ? "true" : "false") << endl;
std::cout << "* max velocity " << maxVelocity << " [m/s]" << std::endl;
std::cout << "* max acceleration " << maxAcceleration << " [m/s^2]" << std::endl;
std::cout << "* max torque " << maxTorque << " [Nm]" << std::endl;
@@ -507,7 +519,7 @@ namespace VirtualRobot
result->setMaxVelocity(maxVelocity);
result->setMaxAcceleration(maxAcceleration);
result->setMaxTorque(maxTorque);
-
+ result->setLimitless(limitless);
std::map< std::string, float>::iterator it = propagatedJointValues.begin();
@@ -551,6 +563,45 @@ namespace VirtualRobot
return false;
}
+ void RobotNode::setLimitless(bool limitless)
+ {
+ this->limitless = limitless;
+ }
+
+ bool RobotNode::isLimitless()
+ {
+ return limitless;
+ }
+
+ float RobotNode::getDelta(float target)
+ {
+ float delta = 0.0f;
+
+ if (nodeType != Joint)
+ {
+ return delta;
+ }
+
+ // we check if the given target value violates our joint limits
+ if (!limitless)
+ {
+ if (target < jointLimitLo || target > jointLimitHi)
+ {
+ return delta;
+ }
+ }
+
+ delta = target - jointValue;
+
+ // eventually take the other way around if it is shorter and if this joint is limitless.
+ if (limitless && (std::abs(delta) > M_PI))
+ {
+ delta = (-1) * ((delta > 0) - (delta < 0)) * ((2 * M_PI) - std::abs(delta));
+ }
+
+ return delta;
+ }
+
void RobotNode::showCoordinateSystem(bool enable, float scaling, std::string* text, const std::string& visualizationType)
{
diff --git a/VirtualRobot/Nodes/RobotNode.h b/VirtualRobot/Nodes/RobotNode.h
index 6096a3d1e44ee2dc9b08cb15a7e0f67cb92c5ebf..d7a35f286a927886677fae0085d4e3d26d25fef3 100644
--- a/VirtualRobot/Nodes/RobotNode.h
+++ b/VirtualRobot/Nodes/RobotNode.h
@@ -101,7 +101,7 @@ namespace VirtualRobot
/*!
Set a joint value [rad].
The internal matrices and visualizations are updated accordingly.
- If you intend to update multiple joints, use \ref setJointValues for faster access.
+ If you intend to update multiple joints, use \ref setJointValueNoUpdate(float) for faster access.
*/
void setJointValue(float q);
@@ -191,6 +191,18 @@ namespace VirtualRobot
virtual bool isTranslationalJoint() const;
virtual bool isRotationalJoint() const;
+ /**
+ * @param limitless wheter this node has joint limits or not.
+ */
+ virtual void setLimitless(bool limitless);
+ bool isLimitless();
+
+ /**
+ * @param target the target joint value in [rad]
+ * @return the signed distance between current and target joint values in [rad].
+ * If the given target value violates joint limits or this robotnode is not a joint, 0.0f is returned instead.
+ */
+ virtual float getDelta(float target);
/*!
Visualize the structure of this RobotNode.
@@ -321,9 +333,7 @@ namespace VirtualRobot
After setting all jointvalues the transformations are calculated by calling \ref applyJointValues()
This method is used when multiple joints should be updated at once.
Access by RobotNodeSets, Robots or RobotConfigs must be protected by a \ref WriteLock.
- \param q The joint value.
- \param updateTransformations When true, the transformation matrices of this joint and all child joints are updated (by calling applyJointValue()).
- \param clampToLimits Consider joint limits. When false an exception is thrown in case of invalid values.
+ \param q The joint value [rad] eventually clamped to limits.
*/
virtual void setJointValueNoUpdate(float q);
@@ -358,6 +368,7 @@ namespace VirtualRobot
float jointValueOffset;
float jointLimitLo, jointLimitHi;
+ 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
float maxAcceleration; //! given in m/s^2
diff --git a/VirtualRobot/XML/RobotIO.cpp b/VirtualRobot/XML/RobotIO.cpp
index 731ca4737c0143509a84f7ecb2fa0026dbbe8421..f8f1d872fc697558637a19aa79fff1360288fb34 100644
--- a/VirtualRobot/XML/RobotIO.cpp
+++ b/VirtualRobot/XML/RobotIO.cpp
@@ -90,7 +90,7 @@ namespace VirtualRobot
*
* The values are stored in \p jointLimitLo and \p jointLimitHi
*/
- void RobotIO::processLimitsNode(rapidxml::xml_node<char>* limitsXMLNode, float& jointLimitLo, float& jointLimitHi)
+ void RobotIO::processLimitsNode(rapidxml::xml_node<char>* limitsXMLNode, float& jointLimitLo, float& jointLimitHi, bool &limitless)
{
THROW_VR_EXCEPTION_IF(!limitsXMLNode, "NULL data for limitsXMLNode in processLimitsNode()");
@@ -141,13 +141,30 @@ namespace VirtualRobot
{
jointLimitLo = unit.toRadian(jointLimitLo);
jointLimitHi = unit.toRadian(jointLimitHi);
+ unit = Units("rad");
}
if (unit.isLength())
{
jointLimitLo = unit.toMillimeter(jointLimitLo);
jointLimitHi = unit.toMillimeter(jointLimitHi);
+ unit = Units("mm");
}
+
+ // limitless attribute
+ rapidxml::xml_attribute<>* llAttr = limitsXMLNode->first_attribute("limitless");
+ if (llAttr != NULL)
+ {
+ limitless = isTrue(llAttr->value());
+ if (limitless && unit.isAngle() && (unit.toDegree(jointLimitHi) - unit.toDegree(jointLimitLo) != 360))
+ {
+ VR_WARNING << "Limitless Joint: Angular distance between 'lo' and 'hi' attributes should equal 2*pi [rad] or 360 [deg]." << endl
+ << "Setting 'lo' to -pi and 'hi' to pi [rad]..." << endl;
+ jointLimitLo = -M_PI;
+ jointLimitHi = M_PI;
+ }
+ }
+
}
bool RobotIO::processSensor(RobotNodePtr rn, rapidxml::xml_node<char>* sensorXMLNode, RobotDescription loadMode, const std::string& basePath)
@@ -202,6 +219,7 @@ namespace VirtualRobot
{
float jointLimitLow = (float) - M_PI;
float jointLimitHigh = (float)M_PI;
+ bool limitless = false;
Eigen::Matrix4f preJointTransform = transformationMatrix;//Eigen::Matrix4f::Identity();
Eigen::Vector3f axis = Eigen::Vector3f::Zero();
@@ -282,7 +300,7 @@ namespace VirtualRobot
{
THROW_VR_EXCEPTION_IF(limitsNode, "Multiple limits definitions in <Joint> tag of robot node <" << robotNodeName << ">." << endl);
limitsNode = node;
- processLimitsNode(limitsNode, jointLimitLow, jointLimitHigh);
+ processLimitsNode(limitsNode, jointLimitLow, jointLimitHigh, limitless);
}
else if (nodeName == "prejointtransform")
{
@@ -547,6 +565,7 @@ namespace VirtualRobot
robotNode->setMaxVelocity(maxVelocity);
robotNode->setMaxAcceleration(maxAcceleration);
robotNode->setMaxTorque(maxTorque);
+ robotNode->setLimitless(limitless);
robotNode->jointValue = initialvalue;
diff --git a/VirtualRobot/XML/RobotIO.h b/VirtualRobot/XML/RobotIO.h
index 35fc0d66f20a3efbe36b4e1261d2295ac7592382..86e14af745bff607724d9be59cec309f5010d5c0 100644
--- a/VirtualRobot/XML/RobotIO.h
+++ b/VirtualRobot/XML/RobotIO.h
@@ -127,7 +127,7 @@ namespace VirtualRobot
RobotPtr robot, VisualizationNodePtr visualizationNode, CollisionModelPtr collisionModel,
SceneObject::Physics& physics, RobotNode::RobotNodeType rntype, Eigen::Matrix4f& transformationMatrix);
static void processChildFromRobotNode(rapidxml::xml_node<char>* childXMLNode, const std::string& nodeName, std::vector< ChildFromRobotDef >& childrenFromRobot);
- static void processLimitsNode(rapidxml::xml_node<char>* limitsXMLNode, float& jointLimitLo, float& jointLimitHi);
+ static void processLimitsNode(rapidxml::xml_node<char>* limitsXMLNode, float& jointLimitLo, float& jointLimitHi, bool& limitless);
static bool processSensor(RobotNodePtr rn, rapidxml::xml_node<char>* sensorXMLNode, RobotDescription loadMode, const std::string& basePath);
static std::map<std::string, int> robot_name_counter;
static VisualizationNodePtr checkUseAsColModel(rapidxml::xml_node<char>* visuXMLNode, const std::string& robotNodeName, const std::string& basePath);