diff --git a/source/RobotAPI/components/units/PlatformUnitSimulation.cpp b/source/RobotAPI/components/units/PlatformUnitSimulation.cpp
index 13ad38088049ae1b5403d58407101a92531d8a28..1ce3b0d6281ce011129d4274ee4913c90a9317d9 100644
--- a/source/RobotAPI/components/units/PlatformUnitSimulation.cpp
+++ b/source/RobotAPI/components/units/PlatformUnitSimulation.cpp
@@ -24,8 +24,12 @@
#include "PlatformUnitSimulation.h"
+#include "RobotAPI/libraries/core/FramedPose.h"
#include <Eigen/Geometry>
+#include <Eigen/src/Geometry/AngleAxis.h>
+#include <Eigen/src/Geometry/Translation.h>
+#include <RobotAPI/interface/units/PlatformUnitInterface.h>
#include <VirtualRobot/MathTools.h>
#include <ArmarXCore/core/time/TimeUtil.h>
#include <cmath>
@@ -56,14 +60,19 @@ namespace armarx
}
+ Eigen::Matrix4f PlatformUnitSimulation::currentPlatformPose() const
+ {
+ // FIXME(fabian.reister): check if order is correct
+ return (Eigen::Translation3f(currentPositionX, currentPositionY, 0.F) * Eigen::AngleAxisf(currentRotation, Eigen::Vector3f::UnitZ())).matrix();
+ }
+
void PlatformUnitSimulation::onStartPlatformUnit()
{
- PlatformPose currentPose;
+ PoseStampedPtr currentPose(new PoseStamped);
// FIXME: Take the timestamp from simulation
- currentPose.timestampInMicroSeconds = TimeUtil::GetTime().toMicroSeconds();
- currentPose.x = currentPositionX;
- currentPose.y = currentPositionY;
- currentPose.rotationAroundZ = currentRotation;
+ currentPose->timestampInMicroSeconds = TimeUtil::GetTime().toMicroSeconds();
+ currentPose->pose = currentPlatformPose();
+
listenerPrx->reportPlatformPose(currentPose);
simulationTask->start();
}
@@ -85,6 +94,12 @@ namespace armarx
}
}
+ void PlatformUnitSimulation::fillCommonPlatformFields(FramedAndStamped & framedAndStamped)
+ {
+ framedAndStamped.timestampInMicroSeconds = TimeUtil::GetTime().toMicroSeconds();
+ framedAndStamped.frame = GlobalFrame;
+ framedAndStamped.agent = RobotFrame;
+ }
void PlatformUnitSimulation::simulationFunction()
{
@@ -164,22 +179,14 @@ namespace armarx
}
}
- auto fillCommonPlatformFields = [&](FramedAndStamped & framedAndStamped)
- {
- platformPose.timestampInMicroSeconds = TimeUtil::GetTime().toMicroSeconds();
- platformPose.parentFrame = GlobalFrame;
- platformPose.childFrame = robotFrame;
- };
-
- PoseStamped platformPose;
- fillCommonPlatformFields(platformPose);
- // FIXME(fabian.reister): check if order is correct
- platformPose.pose = (Eigen::Translation3f(currentPositionX, currentPositionY, 0.F) * Eigen::AngleAxisf(currentRotation, Eigen::Vector3f::UnitZ())).matrix();
- VelocityStamped platformVelocity;
- fillCommonPlatformFields(platformVelocity);
- platformVelocity.velocity << vel[0], vel[1], 0.F, 0.F, 0.F, vel[2];
+ PoseStampedPtr platformPose(new PoseStamped);
+ fillCommonPlatformFields(*platformPose);
+ platformPose->pose = currentPlatformPose();
+ VelocityStampedPtr platformVelocity(new VelocityStamped);
+ fillCommonPlatformFields(*platformVelocity);
+ platformVelocity->velocity << vel[0], vel[1], 0.F, 0.F, 0.F, vel[2];
listenerPrx->reportPlatformPose(platformPose);
listenerPrx->reportPlatformVelocity(platformVelocity);
@@ -197,6 +204,10 @@ namespace armarx
this->positionalAccuracy = positionalAccuracy;
this->orientationalAccuracy = orientationalAccuracy;
}
+
+ PoseStamped targetPose;
+
+
listenerPrx->reportNewTargetPose(targetPositionX, targetPositionY, targetRotation);
}
diff --git a/source/RobotAPI/components/units/PlatformUnitSimulation.h b/source/RobotAPI/components/units/PlatformUnitSimulation.h
index c6d9851b1345a8aad3bd296b99f7c06ea774fec6..4a5ab6c2fbaa9b4fc29dbdcead80cd4479755b57 100644
--- a/source/RobotAPI/components/units/PlatformUnitSimulation.h
+++ b/source/RobotAPI/components/units/PlatformUnitSimulation.h
@@ -134,6 +134,10 @@ namespace armarx
PeriodicTask<PlatformUnitSimulation>::pointer_type simulationTask;
+ private:
+ Eigen::Matrix4f currentPlatformPose() const;
+ void fillCommonPlatformFields(FramedAndStamped & framedAndStamped)
+
};
}
diff --git a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp
index 3c4b3812f39531de88138cbfc3080cde293ecf56..cde9b632f59238b9a8c8d107e7e4990a1422aaf1 100755
--- a/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformGlobalPositionController.cpp
@@ -23,6 +23,8 @@
*/
#include "NJointHolonomicPlatformGlobalPositionController.h"
+#include <SimoxUtility/math/convert/mat4f_to_rpy.h>
+
namespace armarx
{
NJointControllerRegistration<NJointHolonomicPlatformGlobalPositionController>
@@ -124,15 +126,17 @@ namespace armarx
{
// ..todo: check if norm is too large
- // TODO(fabian.reister): add check: frame and agent have to "Global" and "robot"
+ // TODO(fabian.reister): add check: frame and agent have to be "Global" and "robot"
+
+ const float yaw = simox::math::mat4f_to_rpy(currentPose.pose).z();
- const float yaw = simox::math::mat4f_2_rpy(currentPose.pose);
+ const Eigen::Affine3f pose(currentPose.pose);
- getWriterControlStruct().globalPosition << currentPose.translation().x(), currentPose.translation().y();
+ getWriterControlStruct().globalPosition << pose.translation().x(), pose.translation().y();
getWriterControlStruct().globalOrientation = yaw;
getWriterControlStruct().startPosition = currentPosition;
- getWriterControlStruct().startOrientation = yaw;
+ getWriterControlStruct().startOrientation = currentOrientation;
getWriterControlStruct().lastUpdate = IceUtil::Time::microSeconds(currentPose.timestampInMicroSeconds);
writeControlStruct();
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.cpp b/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.cpp
index 99ec57ddd276fe00bbc9ca9c76967d8e1d56ac11..0dd7849c2c0fddf4e8757e2c0c6b520712a5f214 100755
--- a/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.cpp
@@ -148,23 +148,19 @@ Eigen::Matrix4f armarx::PlatformSubUnit::getRelativePose() const
}
-void armarx::PlatformSubUnit::reportPlatformPose(PlatformPose const& currentPose, const Ice::Current& c)
+void armarx::PlatformSubUnit::reportPlatformPose(const PoseStampedPtr& platformPose, const Ice::Current& c)
{
- globalPosCtrl->setGlobalPos(currentPose);
+ globalPosCtrl->setGlobalPos(*platformPose);
}
-void armarx::PlatformSubUnit::reportNewTargetPose(Ice::Float newPlatformPositionX, Ice::Float newPlatformPositionY, Ice::Float newPlatformRotation, const Ice::Current& c)
+void armarx::PlatformSubUnit::reportNewTargetPose(const PoseStampedPtr& /*targetPlatformPose*/, const Ice::Current& c)
{
-
}
-void armarx::PlatformSubUnit::reportPlatformVelocity(Ice::Float currentPlatformVelocityX, Ice::Float currentPlatformVelocityY, Ice::Float currentPlatformVelocityRotation, const Ice::Current& c)
+void armarx::PlatformSubUnit::reportPlatformVelocity(const VelocityStampedPtr&, const Ice::Current& c)
{
-
}
-
-
-void armarx::PlatformSubUnit::reportPlatformOdometryPose(Ice::Float, Ice::Float, Ice::Float, const Ice::Current&)
+void armarx::PlatformSubUnit::reportPlatformOdometryPose(const PoseStampedPtr&, const Ice::Current&)
{
}
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.h b/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.h
index 7fb01dd2a3d2f49059a5906c7ea998ed8c95a4d9..08017ff9b08448ab99f32c82f75cae851b19d9fc 100755
--- a/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.h
+++ b/source/RobotAPI/components/units/RobotUnit/Units/PlatformSubUnit.h
@@ -58,12 +58,10 @@ namespace armarx
virtual void setGlobalPose(PoseBasePtr globalPose, const Ice::Current& = Ice::emptyCurrent) /*override*/;
virtual PoseBasePtr getGlobalPose(const Ice::Current& = Ice::emptyCurrent) /*override*/;
-
- void reportPlatformPose(PlatformPose const& currentPose, const Ice::Current& c = ::Ice::Current()) override ;
- void reportNewTargetPose(Ice::Float newPlatformPositionX, Ice::Float newPlatformPositionY, Ice::Float newPlatformRotation, const Ice::Current& c = ::Ice::Current()) override;
- void reportPlatformVelocity(Ice::Float currentPlatformVelocityX, Ice::Float currentPlatformVelocityY, Ice::Float currentPlatformVelocityRotation, const Ice::Current& c = ::Ice::Current()) override;
-
- void reportPlatformOdometryPose(::Ice::Float, ::Ice::Float, ::Ice::Float, const ::Ice::Current& = ::Ice::Current()) override;
+ void reportPlatformPose(const PoseStampedPtr& currentPose, const Ice::Current& c) override ;
+ void reportNewTargetPose(const PoseStampedPtr& targetPlatformPose, const Ice::Current& c) override;
+ void reportPlatformVelocity(const VelocityStampedPtr& platformVelocity, const Ice::Current& c) override;
+ void reportPlatformOdometryPose(const PoseStampedPtr& odometryPose, const ::Ice::Current&) override;
// PlatformUnit interface
void onInitPlatformUnit() override
diff --git a/source/RobotAPI/libraries/core/FramedPose.h b/source/RobotAPI/libraries/core/FramedPose.h
index e76ccb78f4a71ca098efdb5127daa15f1a099770..c312f9f068f176b2b0b4c9fff4449dacdd306710 100644
--- a/source/RobotAPI/libraries/core/FramedPose.h
+++ b/source/RobotAPI/libraries/core/FramedPose.h
@@ -59,6 +59,7 @@ namespace armarx
std::string const GlobalFrame = "Global";
std::string const OdometryFrame = "Odom";
std::string const MapFrame = "Map";
+ std::string const RobotFrame = "robot"; // FIXME(fabian.reister): this has to be variable.