diff --git a/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp index a4525305d6e0e73fef7e58d9b6c83f04a957c520..b41d385f32ffcc3c437a3090658e7eedcbec22e9 100644 --- a/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp +++ b/source/RobotAPI/components/ArticulatedObjectLocalizerExample/ArticulatedObjectLocalizerExample.cpp @@ -29,6 +29,8 @@ #include <IceUtil/Time.h> +#include <SimoxUtility/math/pose/pose.h> + #include <VirtualRobot/Robot.h> #include <VirtualRobot/XML/RobotIO.h> #include <VirtualRobot/VirtualRobot.h> @@ -51,7 +53,9 @@ namespace armarx::articulated_object { ArticulatedObjectLocalizerExample::ArticulatedObjectLocalizerExample() : articulatedObjectWriter(new ::armarx::armem::articulated_object::Writer(memoryNameSystem)), - articulatedObjectReader(new ::armarx::armem::articulated_object::Reader(memoryNameSystem)) {} + articulatedObjectReader(new ::armarx::armem::articulated_object::Reader(memoryNameSystem)) + { + } armarx::PropertyDefinitionsPtr ArticulatedObjectLocalizerExample::createPropertyDefinitions() { @@ -83,7 +87,7 @@ namespace armarx::articulated_object ARMARX_IMPORTANT << "Running example."; start = armem::Time::now(); - task = new PeriodicTask<ArticulatedObjectLocalizerExample>(this, &ArticulatedObjectLocalizerExample::run, 1000 / p.updateFrequency); + task = new PeriodicTask<ArticulatedObjectLocalizerExample>(this, &ArticulatedObjectLocalizerExample::run, 1000.f / p.updateFrequency); task->start(); } @@ -152,7 +156,7 @@ namespace armarx::articulated_object ARMARX_DEBUG << "Reporting articulated objects"; const IceUtil::Time now = TimeUtil::GetTime(); - const float t = float((now - start).toSecondsDouble()); + const float t = float((now - start).toSecondsDouble()); // move joints at certain frequency const float k = (1 + std::sin(t / (M_2_PIf32))) / 2; // in [0,1] @@ -163,9 +167,11 @@ namespace armarx::articulated_object {"drawer_joint", 350 * k} }; + dishwasher->setGlobalPose(simox::math::pose(Eigen::Vector3f(1000, 0, 0))); dishwasher->setJointValues(jointValues); - armarx::armem::articulated_object::ArticulatedObject armemDishwasher = convert(*dishwasher, IceUtil::Time::now()); + armarx::armem::articulated_object::ArticulatedObject armemDishwasher = + convert(*dishwasher, IceUtil::Time::now()); articulatedObjectWriter->store(armemDishwasher); }