Skip to content
Snippets Groups Projects
Commit a2abad0e authored by Mirko Wächter's avatar Mirko Wächter
Browse files

fixed kinematic platform simulation: velocity mode was in global coordinates

parent 3b7b43c3
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
source/RobotAPI/components/units/PlatformUnitSimulation.cpp
+ 2
− 2
View file @ a2abad0e
...@@ -128,14 +128,14 @@ void PlatformUnitSimulation::simulationFunction() ...@@ -128,14 +128,14 @@ void PlatformUnitSimulation::simulationFunction()
case eVelocityControl: case eVelocityControl:
{ {
Eigen::Vector2f targetVel(linearVelocityX, linearVelocityY); Eigen::Vector2f targetVel(linearVelocityX, linearVelocityY);
Eigen::Rotation2Df rot(currentRotation);
targetVel = rot * targetVel;
velPID->update(timeDeltaInSeconds, currentTranslationVelocity, targetVel); velPID->update(timeDeltaInSeconds, currentTranslationVelocity, targetVel);
currentTranslationVelocity += timeDeltaInSeconds * velPID->getControlValue(); currentTranslationVelocity += timeDeltaInSeconds * velPID->getControlValue();
currentPositionX += timeDeltaInSeconds * currentTranslationVelocity[0]; currentPositionX += timeDeltaInSeconds * currentTranslationVelocity[0];
currentPositionY += timeDeltaInSeconds * currentTranslationVelocity[1]; currentPositionY += timeDeltaInSeconds * currentTranslationVelocity[1];
Eigen::Rotation2Df rot(currentRotation);
targetVel = rot * targetVel;
currentRotation += angularVelocity * timeDeltaInSeconds; currentRotation += angularVelocity * timeDeltaInSeconds;
// stay between +/- M_2_PI // stay between +/- M_2_PI
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment