From 61e453898bb2771f9c7bcfc0e1df51ba1baf27aa Mon Sep 17 00:00:00 2001
From: Mirko Waechter <mirko.waechter@kit.edu>
Date: Mon, 8 Feb 2016 21:48:35 +0100
Subject: [PATCH] extended RobotStateComponent with a history of robot
configurations, that can be used to query the robot state at any timepoint in
the past (inside the size of the history)
---
.../components/RobotState/CMakeLists.txt | 2 +
.../RobotState/RobotStateComponent.cpp | 155 ++++++++++++++----
.../RobotState/RobotStateComponent.h | 10 +-
.../RobotState/SharedRobotServants.h | 6 +-
.../components/RobotState/test/CMakeLists.txt | 4 +
.../RobotState/test/RobotStateTest.cpp | 103 ++++++++++++
source/RobotAPI/interface/core/RobotState.ice | 36 +++-
7 files changed, 281 insertions(+), 35 deletions(-)
create mode 100644 source/RobotAPI/components/RobotState/test/CMakeLists.txt
create mode 100644 source/RobotAPI/components/RobotState/test/RobotStateTest.cpp
diff --git a/source/RobotAPI/components/RobotState/CMakeLists.txt b/source/RobotAPI/components/RobotState/CMakeLists.txt
index 246213dd5..fcf6bab1c 100644
--- a/source/RobotAPI/components/RobotState/CMakeLists.txt
+++ b/source/RobotAPI/components/RobotState/CMakeLists.txt
@@ -25,3 +25,5 @@ set(LIB_HEADERS
)
armarx_add_library("${LIB_NAME}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
+
+add_subdirectory(test)
diff --git a/source/RobotAPI/components/RobotState/RobotStateComponent.cpp b/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
index 103191364..378150c76 100644
--- a/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
+++ b/source/RobotAPI/components/RobotState/RobotStateComponent.cpp
@@ -38,7 +38,6 @@ using namespace Eigen;
using namespace Ice;
using namespace boost;
-std::ofstream out_file;
namespace armarx
{
@@ -58,6 +57,11 @@ namespace armarx
void RobotStateComponent::onInitComponent()
{
+
+ historyLength = getProperty<int>("HistoryLength").getValue();
+ history.clear();
+
+
relativeRobotFile = getProperty<std::string>("RobotFileName").getValue();
if (!ArmarXDataPath::getAbsolutePath(relativeRobotFile, robotFile))
@@ -123,7 +127,10 @@ namespace armarx
this->_synchronizedPrx = SharedRobotInterfacePrx::uncheckedCast(getObjectAdapter()->addWithUUID(_sharedRobotServant));
ARMARX_INFO << "Started RobotStateComponent" << flush;
- robotStateObs->setRobot(_synchronized);
+ if (robotStateObs)
+ {
+ robotStateObs->setRobot(_synchronized);
+ }
}
void RobotStateComponent::onDisconnectComponent()
@@ -151,16 +158,118 @@ namespace armarx
auto clone = this->_synchronized->clone(_synchronized->getName());
- SharedRobotInterfacePtr p = new SharedRobotServant(clone);
+ SharedRobotServantPtr p = new SharedRobotServant(clone);
+ p->setTimestamp(IceUtil::Time::microSeconds(_sharedRobotServant->getTimestamp()->timestamp));
auto result = getObjectAdapter()->addWithUUID(p);
// virtal robot clone is buggy -> set global pose here
p->setGlobalPose(new Pose(_synchronized->getGlobalPose()));
return SharedRobotInterfacePrx::uncheckedCast(result);
}
+ SharedRobotInterfacePrx RobotStateComponent::getRobotSnapshotAtTimestamp(Ice::Long time, const Current& current)
+ {
+ if (!_synchronized)
+ {
+ throw NotInitializedException("Shared Robot is NULL", "getRobotSnapshot");
+ }
+
+ VirtualRobot::RobotPtr clone = this->_synchronized->clone(_synchronized->getName());
+ RobotStateConfig config;
+ if (!interpolate(time, config))
+ {
+ ARMARX_WARNING << "Could not find or interpolate a robot state for time " << IceUtil::Time::microSeconds(time).toDateTime();
+ return NULL;
+ }
+
+ clone->setJointValues(config.jointMap);
+ clone->setGlobalPose(FramedPosePtr::dynamicCast(config.globalPose)->toEigen());
+ SharedRobotServantPtr p = new SharedRobotServant(clone);
+ p->setTimestamp(IceUtil::Time::microSeconds(_sharedRobotServant->getTimestamp()->timestamp));
+ auto result = getObjectAdapter()->addWithUUID(p);
+ // virtal robot clone is buggy -> set global pose here
+ p->setGlobalPose(new Pose(_synchronized->getGlobalPose()));
+
+
+ return SharedRobotInterfacePrx::uncheckedCast(result);
+ }
+
+
+ NameValueMap RobotStateComponent::getJointConfigAtTimestamp(Ice::Long timestamp, const Current&) const
+ {
+ RobotStateConfig config;
+ if (!interpolate(timestamp, config))
+ {
+ ARMARX_WARNING << "Could not find or interpolate a robot state for time " << IceUtil::Time::microSeconds(timestamp).toDateTime();
+ return NameValueMap();
+ }
+ return config.jointMap;
+ }
+
+
+ RobotStateConfig RobotStateComponent::getRobotStateAtTimestamp(Long timestamp, const Current&) const
+ {
+ RobotStateConfig config;
+ if (!interpolate(timestamp, config))
+ {
+ ARMARX_WARNING << "Could not find or interpolate a robot state for time " << IceUtil::Time::microSeconds(timestamp).toDateTime();
+ return RobotStateConfig();
+ }
+ return config;
+ }
+
+ bool RobotStateComponent::interpolate(long time, RobotStateConfig& config) const
+ {
+
+ auto it = history.lower_bound(time);
+ if (time > history.rbegin()->first)
+ {
+ config = history.rbegin()->second;
+ }
+ else if (it == history.end())
+ {
+ return false;
+ }
+ else
+ {
+ config = it->second;
+ if (it->first != time)
+ {
+ // interpolate
+ auto prevIt = std::prev(it);
+ if (prevIt != history.end())
+ {
+ long deltaT = it->first - prevIt->first;
+ float influenceNext = 1.0f - (float)(it->first - time) / deltaT;
+ float influencePrev = 1.0f - (float)(time - prevIt->first) / deltaT;
+ auto jointIt = prevIt->second.jointMap.begin();
+ for (auto & joint : config.jointMap)
+ {
+ joint.second = joint.second * influenceNext + jointIt->second * influencePrev;
+ jointIt++;
+ }
+ config.timestamp = time;
+ ARMARX_CHECK_EXPRESSION(it->second.globalPose);
+ ARMARX_CHECK_EXPRESSION(prevIt->second.globalPose);
+ Eigen::Matrix4f globalPoseNext = FramedPosePtr::dynamicCast(it->second.globalPose)->toEigen();
+ Eigen::Matrix4f globalPosePref = FramedPosePtr::dynamicCast(prevIt->second.globalPose)->toEigen();
+ Eigen::Quaternionf rotNext(globalPoseNext.block<3, 3>(0, 0));
+ Eigen::Quaternionf rotPrev(globalPosePref.block<3, 3>(0, 0));
+ Eigen::Quaternionf rotNew = rotPrev.slerp(influenceNext, rotNext);
+ Eigen::Matrix4f globalPose;
+ globalPose.block<3, 3>(0, 0) = rotNew.toRotationMatrix();
+ globalPose.block<3, 1>(0, 3) = globalPoseNext.block<3, 1>(0, 3) * influenceNext +
+ globalPosePref.block<3, 1>(0, 3) * influencePrev;
+
+ }
+ }
+ }
+ return true;
+ }
+
void RobotStateComponent::reportJointAngles(const NameValueMap& jointAngles, bool aValueChanged, const Current& c)
{
// IceUtil::Time start = IceUtil::Time::now();
+ auto timestamp = IceUtil::Time::now();
if (aValueChanged)
{
{
@@ -185,9 +294,18 @@ namespace armarx
if (_sharedRobotServant)
{
- _sharedRobotServant->setTimestamp(IceUtil::Time::now());
+ _sharedRobotServant->setTimestamp(timestamp);
}
+ history[timestamp.toMicroSeconds()] = {timestamp.toMicroSeconds(),
+ new FramedPose(_synchronized->getGlobalPose(), GlobalFrame, ""),
+ jointAngles
+ };
+
+ if (history.size() > historyLength)
+ {
+ history.erase(history.begin());
+ }
// ARMARX_VERBOSE << deactivateSpam(2) << "my duration: " << (IceUtil::Time::now() - start).toMicroSecondsDouble() << " ms";
@@ -204,7 +322,7 @@ namespace armarx
auto packages = armarx::Application::GetProjectDependencies();
packages.push_back(Application::GetProjectName());
- for (const std::string& projectName : packages)
+ for (const std::string & projectName : packages)
{
if (projectName.empty())
{
@@ -235,30 +353,6 @@ namespace armarx
void RobotStateComponent::reportJointMotorTemperatures(const NameValueMap& jointMotorTemperatures, bool aValueChanged, const Current& c) {}
void RobotStateComponent::reportJointStatuses(const NameStatusMap& jointStatuses, bool aValueChanged, const Current& c) {}
- /*void RobotStateComponent::reportPlatformPose(float currentPlatformPositionX, float currentPlatformPositionY, float currentPlatformRotation, const Ice::Current& c)
- {
- if (!this->_synchronized->hasRobotNodeSet("Platform") && !this->_synchronized->hasRobotNode("Platform"))
- {
- return;
- }
- {
- WriteLockPtr lock = _synchronized->getWriteLock();
- _synchronized->setJointValue("X_Platform", currentPlatformPositionX);
- _synchronized->setJointValue("Y_Platform", currentPlatformPositionY);
- _synchronized->setJointValue("Yaw_Platform", currentPlatformRotation);
- _synchronized->applyJointValues();
- }
- }
-
- void RobotStateComponent::reportNewTargetPose(float newPlatformPositionX, float newPlatformPositionY, float newPlatformRotation, const Ice::Current& c)
- {
- }
-
- void RobotStateComponent::reportPlatformVelocity(float currentPlatformVelocityX, float currentPlatformVelocityY, float currentPlatformVelocityRotation, const Ice::Current &c)
- {
-
- }*/
-
PropertyDefinitionsPtr RobotStateComponent::createPropertyDefinitions()
{
return PropertyDefinitionsPtr(new RobotStatePropertyDefinitions(
@@ -285,3 +379,6 @@ namespace armarx
}
+
+
+
diff --git a/source/RobotAPI/components/RobotState/RobotStateComponent.h b/source/RobotAPI/components/RobotState/RobotStateComponent.h
index 617d938e6..91699e862 100644
--- a/source/RobotAPI/components/RobotState/RobotStateComponent.h
+++ b/source/RobotAPI/components/RobotState/RobotStateComponent.h
@@ -51,6 +51,7 @@ namespace armarx
defineRequiredProperty<std::string>("RobotNodeSetName", "Set of nodes that is controlled by the KinematicUnit");
defineRequiredProperty<std::string>("RobotFileName", "Filename of VirtualRobot robot model (e.g. robot_model.xml)");
defineRequiredProperty<std::string>("AgentName", "Name of the agent for which the sensor values are provided");
+ defineOptionalProperty<int>("HistoryLength", 10000, "Number of entries in the robot state history");
}
};
@@ -82,7 +83,6 @@ namespace armarx
{
public:
-
/**
* \return SharedRobotInterface proxy to the internal synchronized robot.
*/
@@ -120,6 +120,11 @@ namespace armarx
return "RobotStateComponent";
}
void setRobotStateObserver(RobotStateObserverPtr observer);
+ SharedRobotInterfacePrx getRobotSnapshotAtTimestamp(Ice::Long time, const Ice::Current& current);
+ NameValueMap getJointConfigAtTimestamp(Ice::Long, const Ice::Current&) const;
+ RobotStateConfig getRobotStateAtTimestamp(Ice::Long timestamp, const Ice::Current&) const;
+
+ bool interpolate(long time, RobotStateConfig& config) const;
protected:
/**
* Load and create a VirtualRobot::Robot instance from the RobotFileName
@@ -150,9 +155,12 @@ namespace armarx
SharedRobotInterfacePrx _synchronizedPrx;
SharedRobotServantPtr _sharedRobotServant;
VirtualRobot::RobotPtr _synchronized;
+
std::string robotFile;
std::string relativeRobotFile;
RobotStateObserverPtr robotStateObs;
+ std::map<long, RobotStateConfig> history;
+ size_t historyLength;
};
diff --git a/source/RobotAPI/components/RobotState/SharedRobotServants.h b/source/RobotAPI/components/RobotState/SharedRobotServants.h
index 845ed5465..178ca105a 100644
--- a/source/RobotAPI/components/RobotState/SharedRobotServants.h
+++ b/source/RobotAPI/components/RobotState/SharedRobotServants.h
@@ -49,7 +49,7 @@ namespace armarx
*/
class SharedRobotNodeServant :
virtual public SharedRobotNodeInterface,
- public SharedObjectBase
+ public SharedObjectBase
{
public:
SharedRobotNodeServant(VirtualRobot::RobotNodePtr node /*,const Ice::Current & current = Ice::Current()*/);
@@ -111,12 +111,12 @@ namespace armarx
virtual PoseBasePtr getGlobalPose(const Ice::Current& current = Ice::Current());
NameValueMap getConfig(const Ice::Current& current = Ice::Current());
- virtual void setGlobalPose(const PoseBasePtr& pose, const Ice::Current& current);
+ virtual void setGlobalPose(const PoseBasePtr& pose, const Ice::Current& current = Ice::Current());
VirtualRobot::RobotPtr getRobot() const;
void setTimestamp(const IceUtil::Time& updateTime);
- TimestampBasePtr getTimestamp(const Ice::Current&) const;
+ TimestampBasePtr getTimestamp(const Ice::Current& = Ice::Current()) const;
protected:
VirtualRobot::RobotPtr _robot;
boost::recursive_mutex m;
diff --git a/source/RobotAPI/components/RobotState/test/CMakeLists.txt b/source/RobotAPI/components/RobotState/test/CMakeLists.txt
new file mode 100644
index 000000000..2dfe68fa5
--- /dev/null
+++ b/source/RobotAPI/components/RobotState/test/CMakeLists.txt
@@ -0,0 +1,4 @@
+set(LIBS ${LIBS} RobotAPICore RobotAPIRobotStateComponent)
+
+armarx_add_test(RobotStateTest RobotStateTest.cpp "${LIBS}")
+
diff --git a/source/RobotAPI/components/RobotState/test/RobotStateTest.cpp b/source/RobotAPI/components/RobotState/test/RobotStateTest.cpp
new file mode 100644
index 000000000..2c6d98938
--- /dev/null
+++ b/source/RobotAPI/components/RobotState/test/RobotStateTest.cpp
@@ -0,0 +1,103 @@
+/*
+* This file is part of ArmarX.
+*
+* ArmarX is free software; you can redistribute it and/or modify
+* it under the terms of the GNU General Public License version 2 as
+* published by the Free Software Foundation.
+*
+* ArmarX is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package ArmarX::RobotAPI::Test
+* @author Nils Adermann (naderman at naderman dot de)
+* @date 2010
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#define BOOST_TEST_MODULE RobotAPI::FramedPose::Test
+#define ARMARX_BOOST_TEST
+#include <RobotAPI/Test.h>
+#include <ArmarXCore/util/json/JSONObject.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
+#include <RobotAPI/components/RobotState/RobotStateComponent.h>
+#include <ArmarXCore/core/test/IceTestHelper.h>
+
+using namespace armarx;
+
+#include <ArmarXCore/core/application/Application.h>
+#include <ArmarXCore/core/system/FactoryCollectionBase.h>
+using namespace armarx;
+class RobotStateComponentTestEnvironment
+{
+public:
+ RobotStateComponentTestEnvironment(const std::string& testName, int registryPort = 11220, bool addObjects = true)
+ {
+ Ice::PropertiesPtr properties = Ice::createProperties();
+ armarx::Application::LoadDefaultConfig(properties);
+ CMakePackageFinder finder("RobotAPI");
+ ArmarXDataPath::addDataPaths({finder.getDataDir()});
+ // If you need to set properties (optional):
+ properties->setProperty("ArmarX.RobotStateComponent.AgentName", "Armar3");
+ properties->setProperty("ArmarX.RobotStateComponent.RobotNodeSetName", "Robot");
+ properties->setProperty("ArmarX.RobotStateComponent.RobotFileName", "RobotAPI/robots/Armar3/ArmarIII.xml");
+
+ // The IceTestHelper starts all required Ice processes
+ _iceTestHelper = new IceTestHelper(registryPort, registryPort + 1);
+ _iceTestHelper->startEnvironment();
+
+ // The manager allows you to create new ArmarX components
+ _manager = new TestArmarXManager(testName, _iceTestHelper->getCommunicator(), properties);
+ if (addObjects)
+ {
+ // This is how you create components.
+ _rsc = _manager->createComponentAndRun<RobotStateComponent, RobotStateComponentInterfacePrx>("ArmarX", "RobotStateComponent");
+ }
+ }
+ ~RobotStateComponentTestEnvironment()
+ {
+ _manager->shutdown();
+ }
+ // In your tests, you can access your component through this proxy
+ RobotStateComponentInterfacePrx _rsc;
+ TestArmarXManagerPtr _manager;
+ IceTestHelperPtr _iceTestHelper;
+};
+typedef boost::shared_ptr<RobotStateComponentTestEnvironment> RobotStateComponentTestEnvironmentPtr;
+
+BOOST_AUTO_TEST_CASE(RobotStateComponentHistoryTest)
+//void func()
+{
+ RobotStateComponentTestEnvironment env("RSC");
+
+ NameValueMap joints;
+ while(env._manager->getObjectState("RobotStateComponent") < eManagedIceObjectStarted)
+ {
+ ARMARX_INFO_S << "Waiting";
+ usleep(100000);
+ }
+ joints["Elbow L"] = 0;
+ env._rsc->reportJointAngles(joints, true);
+ auto t1 = env._rsc->getSynchronizedRobot()->getTimestamp()->timestamp;
+ usleep(10000);
+ joints["Elbow L"] = 1;
+ env._rsc->reportJointAngles(joints, true);
+ auto t2 = env._rsc->getSynchronizedRobot()->getTimestamp()->timestamp;
+ auto config = env._rsc->getJointConfigAtTimestamp(t1);
+ BOOST_CHECK(t2 > t1);
+ BOOST_CHECK_EQUAL(config["Elbow L"], 0);
+ config = env._rsc->getJointConfigAtTimestamp(t2);
+ BOOST_CHECK_EQUAL(config["Elbow L"], 1);
+ config = env._rsc->getJointConfigAtTimestamp(t2+1);// future timestamp -> latest values should be returned
+ BOOST_CHECK_EQUAL(config["Elbow L"], 1);
+ config = env._rsc->getJointConfigAtTimestamp((t1+t2)*0.5); // interpolated values in the middle
+ BOOST_CHECK_CLOSE(config["Elbow L"], 0.5f, 1);
+ config = env._rsc->getJointConfigAtTimestamp(t1+(t2-t1)*0.7); // interpolated values
+ ARMARX_INFO_S << "value at t=0.7%: " << config["Elbow L"];
+ BOOST_CHECK_CLOSE(config["Elbow L"], 0.7f, 1);
+}
diff --git a/source/RobotAPI/interface/core/RobotState.ice b/source/RobotAPI/interface/core/RobotState.ice
index d37226e2d..11bf27a06 100644
--- a/source/RobotAPI/interface/core/RobotState.ice
+++ b/source/RobotAPI/interface/core/RobotState.ice
@@ -41,6 +41,13 @@ module armarx
string rootName;
};
+ struct RobotStateConfig
+ {
+ long timestamp;
+ FramedPoseBase globalPose;
+ NameValueMap jointMap;
+ };
+
/**
* The SharedRobotNodeInterface provides access to a limited amount of
* VirtualRobot::RobotNode methods over the Ice network.
@@ -133,8 +140,33 @@ module armarx
/**
* @return proxy to a copy of the shared robot with non updating joint values
*/
- SharedRobotInterface* getRobotSnapshot(string time) throws NotInitializedException;
-
+ SharedRobotInterface* getRobotSnapshot(string deprecated) throws NotInitializedException;
+
+ /**
+ * Create robot snapshot proxy from past timestamp. The time that can be gone back depends on the robotstatecomponent configuration.
+ * @return Snapshot
+ *
+ * */
+ SharedRobotInterface* getRobotSnapshotAtTimestamp(long time) throws NotInitializedException;
+
+ /**
+ * Return the joint values from past timestamp. The time that can be gone back depends on the robotstatecomponent configuration.
+ * @return Snapshot
+ *
+ * */
+ ["cpp:const"]
+ idempotent
+ NameValueMap getJointConfigAtTimestamp(long time) throws NotInitializedException;
+
+ /**
+ * Return the Robot configuration, including joint values and global pose of the root node, from past timestamp. The time that can be gone back depends on the robotstatecomponent configuration.
+ * @return Snapshot
+ *
+ * */
+ ["cpp:const"]
+ idempotent
+ RobotStateConfig getRobotStateAtTimestamp(long time) throws NotInitializedException;
+
/**
* @return the robot xml filename as specified in the configuration
*/
--
GitLab