From e455f4f943479516db99cbdba8dd642d4791d6b4 Mon Sep 17 00:00:00 2001
From: Mirko Waechter <mirko.waechter@kit.edu>
Date: Mon, 4 Apr 2016 12:23:53 +0200
Subject: [PATCH] made kinematic platform unit work
---
.../units/PlatformUnitSimulation.cpp | 127 ++++++++++++------
.../components/units/PlatformUnitSimulation.h | 15 ++-
2 files changed, 102 insertions(+), 40 deletions(-)
diff --git a/source/RobotAPI/components/units/PlatformUnitSimulation.cpp b/source/RobotAPI/components/units/PlatformUnitSimulation.cpp
index 0637a2b03..3666d5636 100644
--- a/source/RobotAPI/components/units/PlatformUnitSimulation.cpp
+++ b/source/RobotAPI/components/units/PlatformUnitSimulation.cpp
@@ -25,20 +25,24 @@
#include "PlatformUnitSimulation.h"
+#include <Eigen/Geometry>
+#include <VirtualRobot/MathTools.h>
+
#include <cmath>
using namespace armarx;
void PlatformUnitSimulation::onInitPlatformUnit()
{
+ platformMode = eUndefined;
referenceFrame = getProperty<std::string>("ReferenceFrame").getValue();
targetPositionX = currentPositionX = getProperty<float>("InitialPosition.x").getValue();
targetPositionY = currentPositionY = getProperty<float>("InitialPosition.y").getValue();
targetRotation = 0.0;
targetRotation = currentRotation = getProperty<float>("InitialRotation").getValue();
- linearVelocity = getProperty<float>("LinearVelocity").getValue();
- angularVelocity = getProperty<float>("AngularVelocity").getValue();
+ maxLinearVelocity = getProperty<float>("LinearVelocity").getValue();
+ maxAngularVelocity = getProperty<float>("AngularVelocity").getValue();
positionalAccuracy = 0.01;
@@ -79,44 +83,78 @@ void PlatformUnitSimulation::simulationFunction()
std::vector<float> vel(3, 0.0f);
{
ScopedLock lock(currentPoseMutex);
- float diff = fabs(targetPositionX - currentPositionX);
-
- if (diff > positionalAccuracy)
- {
- int sign = copysignf(1.0f, (targetPositionX - currentPositionX));
- currentPositionX += sign * std::min<float>(linearVelocity * timeDeltaInSeconds, diff);
- vel[0] = linearVelocity;
- }
-
- diff = fabs(targetPositionY - currentPositionY);
-
- if (diff > positionalAccuracy)
- {
- int sign = copysignf(1.0f, (targetPositionY - currentPositionY));
- currentPositionY += sign * std::min<float>(linearVelocity * timeDeltaInSeconds, diff);
- vel[1] = linearVelocity;
- }
-
- diff = fabs(targetRotation - currentRotation);
-
- if (diff > orientationalAccuracy)
+ switch (platformMode)
{
- int sign = copysignf(1.0f, (targetRotation - currentRotation));
- currentRotation += sign * std::min<float>(angularVelocity * timeDeltaInSeconds, diff);
-
- // stay between +/- M_2_PI
- if (currentRotation > 2 * M_PI)
+ case ePositionControl:
{
- currentRotation -= 2 * M_PI;
+ float diff = fabs(targetPositionX - currentPositionX);
+ ARMARX_INFO << deactivateSpam(0.5) << "Distance to goal X: " << diff;
+
+ if (diff > positionalAccuracy)
+ {
+ int sign = copysignf(1.0f, (targetPositionX - currentPositionX));
+ currentPositionX += sign * std::min<float>(maxLinearVelocity * timeDeltaInSeconds, diff);
+ vel[0] = linearVelocityX;
+ }
+
+ diff = fabs(targetPositionY - currentPositionY);
+
+ if (diff > positionalAccuracy)
+ {
+ int sign = copysignf(1.0f, (targetPositionY - currentPositionY));
+ currentPositionY += sign * std::min<float>(maxLinearVelocity * timeDeltaInSeconds, diff);
+ vel[1] = linearVelocityY;
+ }
+
+ diff = fabs(targetRotation - currentRotation);
+
+ if (diff > orientationalAccuracy)
+ {
+ int sign = copysignf(1.0f, (targetRotation - currentRotation));
+ currentRotation += sign * std::min<float>(maxAngularVelocity * timeDeltaInSeconds, diff);
+
+ // stay between +/- M_2_PI
+ if (currentRotation > 2 * M_PI)
+ {
+ currentRotation -= 2 * M_PI;
+ }
+
+ if (currentRotation < -2 * M_PI)
+ {
+ currentRotation += 2 * M_PI;
+ }
+
+ vel[2] = angularVelocity;
+
+ }
}
-
- if (currentRotation < -2 * M_PI)
+ break;
+ case eVelocityControl:
{
- currentRotation += 2 * M_PI;
+ Eigen::Vector2f targetVel(linearVelocityX, linearVelocityY);
+ Eigen::Rotation2Df rot(currentRotation);
+ targetVel = rot * targetVel;
+ currentPositionX += timeDeltaInSeconds * targetVel[0];
+ currentPositionY += timeDeltaInSeconds * targetVel[1];
+ currentRotation += angularVelocity * timeDeltaInSeconds;
+
+ // stay between +/- M_2_PI
+ if (currentRotation > 2 * M_PI)
+ {
+ currentRotation -= 2 * M_PI;
+ }
+
+ if (currentRotation < -2 * M_PI)
+ {
+ currentRotation += 2 * M_PI;
+ }
+ vel[0] = linearVelocityY;
+ vel[1] = linearVelocityY;
+ vel[2] = angularVelocity;
}
-
- vel[2] = angularVelocity;
-
+ break;
+ default:
+ break;
}
}
listenerPrx->reportPlatformPose(currentPositionX, currentPositionY, currentRotation);
@@ -125,8 +163,10 @@ void PlatformUnitSimulation::simulationFunction()
void PlatformUnitSimulation::moveTo(Ice::Float targetPlatformPositionX, Ice::Float targetPlatformPositionY, Ice::Float targetPlatformRotation, Ice::Float positionalAccuracy, Ice::Float orientationalAccuracy, const Ice::Current& c)
{
+ ARMARX_INFO << "new target pose : " << targetPlatformPositionX << ", " << targetPlatformPositionY << " with angle " << targetPlatformRotation;
{
ScopedLock lock(currentPoseMutex);
+ platformMode = ePositionControl;
targetPositionX = targetPlatformPositionX;
targetPositionY = targetPlatformPositionY;
targetRotation = targetPlatformRotation;
@@ -146,7 +186,13 @@ PropertyDefinitionsPtr PlatformUnitSimulation::createPropertyDefinitions()
void armarx::PlatformUnitSimulation::move(float targetPlatformVelocityX, float targetPlatformVelocityY, float targetPlatformVelocityRotation, const Ice::Current& c)
{
- throw LocalException("NYI");
+ ARMARX_INFO << deactivateSpam(1) << "New velocity: " << targetPlatformVelocityX << ", " << targetPlatformVelocityY << " with angular velocity: " << targetPlatformVelocityRotation;
+ ScopedLock lock(currentPoseMutex);
+ platformMode = eVelocityControl;
+ linearVelocityX = std::min(maxLinearVelocity, targetPlatformVelocityX);
+ linearVelocityY = std::min(maxLinearVelocity, targetPlatformVelocityY);
+ angularVelocity = std::min(maxAngularVelocity, targetPlatformVelocityRotation);
+
}
void PlatformUnitSimulation::moveRelative(float targetPlatformOffsetX, float targetPlatformOffsetY, float targetPlatformOffsetRotation, float positionalAccuracy, float orientationalAccuracy, const Ice::Current& c)
@@ -164,7 +210,12 @@ void PlatformUnitSimulation::moveRelative(float targetPlatformOffsetX, float tar
void PlatformUnitSimulation::setMaxVelocities(float positionalVelocity, float orientaionalVelocity, const Ice::Current& c)
{
ScopedLock lock(currentPoseMutex);
- linearVelocity = positionalVelocity;
- angularVelocity = orientaionalVelocity;
+ maxLinearVelocity = positionalVelocity;
+ maxAngularVelocity = orientaionalVelocity;
+}
+
+void PlatformUnitSimulation::stopPlatform(const Ice::Current& c)
+{
+ move(0, 0, 0);
}
diff --git a/source/RobotAPI/components/units/PlatformUnitSimulation.h b/source/RobotAPI/components/units/PlatformUnitSimulation.h
index 7b56427fe..71444589c 100644
--- a/source/RobotAPI/components/units/PlatformUnitSimulation.h
+++ b/source/RobotAPI/components/units/PlatformUnitSimulation.h
@@ -89,7 +89,7 @@ namespace armarx
void moveRelative(float targetPlatformOffsetX, float targetPlatformOffsetY, float targetPlatformOffsetRotation, float positionalAccuracy, float orientationalAccuracy, const Ice::Current& c = Ice::Current());
void setMaxVelocities(float positionalVelocity, float orientaionalVelocity, const Ice::Current& c = Ice::Current());
-
+ void stopPlatform(const Ice::Current& c = Ice::Current());
/**
* \see PropertyUser::createPropertyDefinitions()
*/
@@ -100,6 +100,14 @@ namespace armarx
IceUtil::Time lastExecutionTime;
int intervalMs;
+ enum PlatformMode
+ {
+ eUndefined,
+ ePositionControl,
+ eVelocityControl
+ }
+ platformMode;
+
::Ice::Float targetPositionX;
::Ice::Float targetPositionY;
::Ice::Float currentPositionX;
@@ -107,8 +115,11 @@ namespace armarx
::Ice::Float targetRotation;
::Ice::Float currentRotation;
- ::Ice::Float linearVelocity;
+ ::Ice::Float linearVelocityX;
+ ::Ice::Float linearVelocityY;
+ ::Ice::Float maxLinearVelocity;
::Ice::Float angularVelocity;
+ ::Ice::Float maxAngularVelocity;
::Ice::Float positionalAccuracy;
::Ice::Float orientationalAccuracy;
--
GitLab