From 3e7c33ba738468c85721f7974cb0d25f4e235468 Mon Sep 17 00:00:00 2001
From: Mirko Waechter <mirko.waechter@kit.edu>
Date: Sun, 19 Jun 2016 17:57:37 +0200
Subject: [PATCH] kinematic sim: velocity is reported and aValueChanged flag
corrected
---
.../units/KinematicUnitSimulation.cpp | 23 ++++++++++++-------
.../units/KinematicUnitSimulation.h | 2 ++
2 files changed, 17 insertions(+), 8 deletions(-)
diff --git a/source/RobotAPI/components/units/KinematicUnitSimulation.cpp b/source/RobotAPI/components/units/KinematicUnitSimulation.cpp
index 207b95728..eb3140833 100644
--- a/source/RobotAPI/components/units/KinematicUnitSimulation.cpp
+++ b/source/RobotAPI/components/units/KinematicUnitSimulation.cpp
@@ -119,15 +119,21 @@ void KinematicUnitSimulation::simulationFunction()
{
float newAngle = 0.0f;
+ float newVelocity = 0.0f;
KinematicUnitSimulationJointInfo& jointInfo = jointInfos[it->first];
// calculate joint positions if joint is in velocity control mode
if (it->second.controlMode == eVelocityControl)
{
double change = (it->second.velocity * timeDeltaInSeconds);
-
+ newVelocity = it->second.velocity;
double randomNoiseValue = distribution(generator);
change *= 1 + randomNoiseValue;
newAngle = it->second.angle + change;
+ // check if velocities have changed
+ if (fabs(previousJointStates[it->first].velocity - newVelocity) > 0.00000001)
+ {
+ aVelValueChanged = true;
+ }
}
else if (it->second.controlMode == ePositionControl)
{
@@ -142,11 +148,16 @@ void KinematicUnitSimulation::simulationFunction()
{
float velValue = (it->second.velocity != 0.0f ? signedMin(pid->getControlValue(), it->second.velocity) : pid->getControlValue()); //restrain to max velocity
velValue *= 1 + distribution(generator);
+
pid->update(it->second.angle);
newAngle = it->second.angle +
velValue *
timeDeltaInSeconds;
-
+ if (fabs(previousJointStates[it->first].velocityActual - velValue) > 0.00000001)
+ {
+ aVelValueChanged = true;
+ }
+ it->second.velocityActual = newVelocity = velValue;
}
}
}
@@ -177,11 +188,7 @@ void KinematicUnitSimulation::simulationFunction()
it->second.angle = newAngle;
}
- // check if velocities have changed
- if (fabs(previousJointStates[it->first].velocity - it->second.velocity) > 0.00000001)
- {
- aVelValueChanged = true;
- }
+
if (jointInfo.continuousJoint())
{
@@ -197,7 +204,7 @@ void KinematicUnitSimulation::simulationFunction()
}
actualJointAngles[it->first] = newAngle;
- actualJointVelocities[it->first] = it->second.velocity;
+ actualJointVelocities[it->first] = newVelocity;
}
previousJointStates = jointStates;
diff --git a/source/RobotAPI/components/units/KinematicUnitSimulation.h b/source/RobotAPI/components/units/KinematicUnitSimulation.h
index e720ada33..14813acbd 100644
--- a/source/RobotAPI/components/units/KinematicUnitSimulation.h
+++ b/source/RobotAPI/components/units/KinematicUnitSimulation.h
@@ -57,6 +57,7 @@ namespace armarx
controlMode = eDisabled;
angle = 0.0f;
velocity = 0.0f;
+ velocityActual = 0.0f;
torque = 0.0f;
temperature = 0.0f;
}
@@ -64,6 +65,7 @@ namespace armarx
ControlMode controlMode;
float angle;
float velocity;
+ float velocityActual;
float torque;
float temperature;
};
--
GitLab