From bc942044e251a0f6aebaf3791d5342f4f4e33966 Mon Sep 17 00:00:00 2001
From: Mirko Waechter <mirko.waechter@kit.edu>
Date: Wed, 18 May 2016 17:53:33 +0200
Subject: [PATCH] kinematic simulation can now deal with continuous joints
---
.../units/KinematicUnitSimulation.cpp | 43 ++++++++++++++-----
.../units/KinematicUnitSimulation.h | 4 ++
2 files changed, 37 insertions(+), 10 deletions(-)
diff --git a/source/RobotAPI/components/units/KinematicUnitSimulation.cpp b/source/RobotAPI/components/units/KinematicUnitSimulation.cpp
index 5b81f9c98..978fedcf5 100644
--- a/source/RobotAPI/components/units/KinematicUnitSimulation.cpp
+++ b/source/RobotAPI/components/units/KinematicUnitSimulation.cpp
@@ -119,7 +119,7 @@ void KinematicUnitSimulation::simulationFunction()
{
float newAngle = 0.0f;
-
+ KinematicUnitSimulationJointInfo& jointInfo = jointInfos[it->first];
// calculate joint positions if joint is in velocity control mode
if (it->second.controlMode == eVelocityControl)
{
@@ -152,11 +152,14 @@ void KinematicUnitSimulation::simulationFunction()
}
// constrain the angle
- KinematicUnitSimulationJointInfo& jointInfo = jointInfos[it->first];
- float maxAngle = (jointInfo.hasLimits()) ? jointInfo.limitHi : 2.0 * M_PI;
- float minAngle = (jointInfo.hasLimits()) ? jointInfo.limitLo : -2.0 * M_PI;
- newAngle = std::max<float>(newAngle, minAngle);
- newAngle = std::min<float>(newAngle, maxAngle);
+ if (!jointInfo.continuousJoint())
+ {
+ float maxAngle = (jointInfo.hasLimits()) ? jointInfo.limitHi : M_PI;
+ float minAngle = (jointInfo.hasLimits()) ? jointInfo.limitLo : -M_PI;
+
+ newAngle = std::max<float>(newAngle, minAngle);
+ newAngle = std::min<float>(newAngle, maxAngle);
+ }
// Check if joint existed before
KinematicUnitSimulationJointStates::iterator itPrev = previousJointStates.find(it->first);
@@ -180,6 +183,18 @@ void KinematicUnitSimulation::simulationFunction()
aVelValueChanged = true;
}
+ if (jointInfo.continuousJoint())
+ {
+ if (newAngle < 0)
+ {
+ newAngle = fmod(newAngle - M_PI, 2 * M_PI) + M_PI;
+ }
+ else
+ {
+ newAngle = fmod(newAngle + M_PI, 2 * M_PI) - M_PI;
+ }
+
+ }
actualJointAngles[it->first] = newAngle;
actualJointVelocities[it->first] = it->second.velocity;
@@ -252,11 +267,11 @@ void KinematicUnitSimulation::setJointAngles(const NameValueMap& targetJointAngl
{
aValueChanged = true;
}
-
- if (jointInfos[targetJointName].hasLimits())
+ KinematicUnitSimulationJointInfo& jointInfo = jointInfos[targetJointName];
+ if (jointInfo.hasLimits() && !jointInfo.continuousJoint())
{
- targetJointAngle = std::max<float>(targetJointAngle, jointInfos[targetJointName].limitLo);
- targetJointAngle = std::min<float>(targetJointAngle, jointInfos[targetJointName].limitHi);
+ targetJointAngle = std::max<float>(targetJointAngle, jointInfo.limitLo);
+ targetJointAngle = std::min<float>(targetJointAngle, jointInfo.limitHi);
}
if (!usePDControllerForPosMode)
@@ -270,8 +285,16 @@ void KinematicUnitSimulation::setJointAngles(const NameValueMap& targetJointAngl
if (pid)
{
pid->reset();
+ if (jointInfo.continuousJoint())
+ {
+ float delta = targetJointAngle - iterJoints->second.angle;
+ float signedSmallestDelta = atan2(sin(delta), cos(delta));
+ targetJointAngle = iterJoints->second.angle + signedSmallestDelta;
+ }
+
pid->setTarget(targetJointAngle);
+
}
}
}
diff --git a/source/RobotAPI/components/units/KinematicUnitSimulation.h b/source/RobotAPI/components/units/KinematicUnitSimulation.h
index 4b730a0f4..87cea5bde 100644
--- a/source/RobotAPI/components/units/KinematicUnitSimulation.h
+++ b/source/RobotAPI/components/units/KinematicUnitSimulation.h
@@ -94,6 +94,10 @@ namespace armarx
{
return (limitLo != limitHi);
}
+ bool continuousJoint() const
+ {
+ return !hasLimits() || limitLo - limitHi >= 360.0f;
+ }
float limitLo;
float limitHi;
--
GitLab