From a2abad0e0acf3f33011708ab392111dd9eb7ccd4 Mon Sep 17 00:00:00 2001
From: Mirko Waechter <mirko.waechter@kit.edu>
Date: Thu, 12 Jan 2017 21:21:13 +0100
Subject: [PATCH] fixed kinematic platform simulation: velocity mode was in
global coordinates
---
source/RobotAPI/components/units/PlatformUnitSimulation.cpp | 4 ++--
1 file changed, 2 insertions(+), 2 deletions(-)
diff --git a/source/RobotAPI/components/units/PlatformUnitSimulation.cpp b/source/RobotAPI/components/units/PlatformUnitSimulation.cpp
index 6a978e801..75fcbca37 100644
--- a/source/RobotAPI/components/units/PlatformUnitSimulation.cpp
+++ b/source/RobotAPI/components/units/PlatformUnitSimulation.cpp
@@ -128,14 +128,14 @@ void PlatformUnitSimulation::simulationFunction()
case eVelocityControl:
{
Eigen::Vector2f targetVel(linearVelocityX, linearVelocityY);
+ Eigen::Rotation2Df rot(currentRotation);
+ targetVel = rot * targetVel;
velPID->update(timeDeltaInSeconds, currentTranslationVelocity, targetVel);
currentTranslationVelocity += timeDeltaInSeconds * velPID->getControlValue();
currentPositionX += timeDeltaInSeconds * currentTranslationVelocity[0];
currentPositionY += timeDeltaInSeconds * currentTranslationVelocity[1];
- Eigen::Rotation2Df rot(currentRotation);
- targetVel = rot * targetVel;
currentRotation += angularVelocity * timeDeltaInSeconds;
// stay between +/- M_2_PI
--
GitLab