Skip to content
Snippets Groups Projects
Commit 29ed53de authored by Fabian Reister's avatar Fabian Reister
Browse files

Merge branch 'fix/platform-rotation' into 'master'

bugfix: platform position controller is turning into wrong direction

See merge request ArmarX/RobotAPI!169
parents b2231b63 eb9c9a14
No related branches found
No related tags found
No related merge requests found
...@@ -23,6 +23,10 @@ ...@@ -23,6 +23,10 @@
*/ */
#include "NJointHolonomicPlatformGlobalPositionController.h" #include "NJointHolonomicPlatformGlobalPositionController.h"
#include <cmath>
#include <SimoxUtility/math/periodic/periodic_clamp.h>
namespace armarx namespace armarx
{ {
NJointControllerRegistration<NJointHolonomicPlatformGlobalPositionController> NJointControllerRegistration<NJointHolonomicPlatformGlobalPositionController>
...@@ -34,7 +38,7 @@ namespace armarx ...@@ -34,7 +38,7 @@ namespace armarx
const NJointHolonomicPlatformGlobalPositionControllerConfigPtr& cfg, const NJointHolonomicPlatformGlobalPositionControllerConfigPtr& cfg,
const VirtualRobot::RobotPtr&) : const VirtualRobot::RobotPtr&) :
pid(cfg->p, cfg->i, cfg->d, cfg->maxVelocity, cfg->maxAcceleration), pid(cfg->p, cfg->i, cfg->d, cfg->maxVelocity, cfg->maxAcceleration),
opid(cfg->p_rot, cfg->i_rot, cfg->d_rot, cfg->maxRotationVelocity, cfg->maxRotationAcceleration) opid(cfg->p_rot, cfg->i_rot, cfg->d_rot, cfg->maxRotationVelocity, cfg->maxRotationAcceleration, true)
{ {
const SensorValueBase* sv = useSensorValue(cfg->platformName); const SensorValueBase* sv = useSensorValue(cfg->platformName);
...@@ -78,24 +82,14 @@ namespace armarx ...@@ -78,24 +82,14 @@ namespace armarx
return; return;
} }
float relativeOrientation = currentOrientation - rtGetControlStruct().startOrientation; const float measuredOrientation = rtGetControlStruct().globalOrientation;
Eigen::Vector2f relativeCurrentPosition = currentPosition - rtGetControlStruct().startPosition;
Eigen::Vector2f updatedPosition = rtGetControlStruct().globalPosition;// + relativeCurrentPosition;
float updatedOrientation = rtGetControlStruct().globalOrientation;//+ relativeOrientation;
float relativeGlobalOrientation = rtGetControlStruct().globalOrientation - getWriterControlStruct().startOrientation;
relativeGlobalOrientation = std::atan2(std::sin(relativeGlobalOrientation), std::cos(relativeGlobalOrientation));
float relativeTargetOrientation = rtGetControlStruct().targetOrientation - getWriterControlStruct().startOrientation;
relativeTargetOrientation = std::atan2(std::sin(relativeTargetOrientation), std::cos(relativeTargetOrientation));
pid.update(timeSinceLastIteration.toSecondsDouble(), rtGetControlStruct().globalPosition, rtGetControlStruct().target);
opid.update(timeSinceLastIteration.toSecondsDouble(), static_cast<double>(measuredOrientation), rtGetControlStruct().targetOrientation);
pid.update(timeSinceLastIteration.toSecondsDouble(), updatedPosition, rtGetControlStruct().target); const Eigen::Rotation2Df global_R_local(-measuredOrientation);
//opid.update(timeSinceLastIteration.toSecondsDouble(), static_cast<double>(updatedOrientation), rtGetControlStruct().targetOrientation);
opid.update(timeSinceLastIteration.toSecondsDouble(), static_cast<double>(relativeGlobalOrientation), relativeTargetOrientation);
Eigen::Vector2f velocities = Eigen::Rotation2Df(-updatedOrientation) * Eigen::Vector2f(pid.getControlValue()[0], pid.getControlValue()[1]); Eigen::Vector2f velocities = global_R_local * pid.getControlValue();
target->velocityX = velocities.x(); target->velocityX = velocities.x();
target->velocityY = velocities.y(); target->velocityY = velocities.y();
target->velocityRotation = static_cast<float>(opid.getControlValue()); target->velocityRotation = static_cast<float>(opid.getControlValue());
...@@ -112,7 +106,7 @@ namespace armarx ...@@ -112,7 +106,7 @@ namespace armarx
std::lock_guard<std::recursive_mutex> lock(controlDataMutex); std::lock_guard<std::recursive_mutex> lock(controlDataMutex);
getWriterControlStruct().target << x, y; getWriterControlStruct().target << x, y;
getWriterControlStruct().targetOrientation = std::atan2(std::sin(yaw), std::cos(yaw)); getWriterControlStruct().targetOrientation = simox::math::periodic_clamp(yaw, -M_PIf32, M_PIf32);
getWriterControlStruct().translationAccuracy = translationAccuracy; getWriterControlStruct().translationAccuracy = translationAccuracy;
getWriterControlStruct().rotationAccuracy = rotationAccuracy; getWriterControlStruct().rotationAccuracy = rotationAccuracy;
getWriterControlStruct().newTargetSet = true; getWriterControlStruct().newTargetSet = true;
...@@ -125,7 +119,7 @@ namespace armarx ...@@ -125,7 +119,7 @@ namespace armarx
// ..todo: check if norm is too large // ..todo: check if norm is too large
getWriterControlStruct().globalPosition << currentPose.x, currentPose.y; getWriterControlStruct().globalPosition << currentPose.x, currentPose.y;
getWriterControlStruct().globalOrientation = currentPose.rotationAroundZ; getWriterControlStruct().globalOrientation = simox::math::periodic_clamp(currentPose.rotationAroundZ, -M_PIf32, M_PIf32);
getWriterControlStruct().startPosition = currentPosition; getWriterControlStruct().startPosition = currentPosition;
getWriterControlStruct().startOrientation = currentOrientation; getWriterControlStruct().startOrientation = currentOrientation;
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment