diff --git a/scenarios/KinematicSimulationArmar3/CMakeLists.txt b/scenarios/KinematicSimulationArmar3/CMakeLists.txt
index 6e5939b2cbdcad3aff5ca50e00b8403627ee72e0..308205d639c3f88e49a61332d5b8fd908c90813d 100644
--- a/scenarios/KinematicSimulationArmar3/CMakeLists.txt
+++ b/scenarios/KinematicSimulationArmar3/CMakeLists.txt
@@ -2,12 +2,6 @@
set(SCENARIO_CONFIG_COMPONENTS
- config/CommonStorage.cfg
- config/PriorKnowledge.cfg
- config/LongtermMemory.cfg
- config/WorkingMemory.cfg
- config/ObjectMemoryObserver.cfg
-
config/KinematicUnitSimulation.cfg
config/KinematicUnitObserver.cfg
config/HeadIKUnit.cfg
@@ -21,8 +15,6 @@ set(SCENARIO_CONFIG_COMPONENTS
config/ConditionHandler.cfg
config/SystemObserver.cfg
config/RobotStateComponent.cfg
-
- config/ImageSequenceProvider.cfg
)
# RobotControl
diff --git a/scenarios/KinematicSimulationArmar3/config/ConditionHandler.cfg b/scenarios/KinematicSimulationArmar3/config/ConditionHandler.cfg
index a938a4186b94e0159b027fc30e3ccc3fc5c62d3a..7f9f11af5b9b683419b741b867a32b5f4f6a993a 100644
--- a/scenarios/KinematicSimulationArmar3/config/ConditionHandler.cfg
+++ b/scenarios/KinematicSimulationArmar3/config/ConditionHandler.cfg
@@ -69,7 +69,7 @@
# - Default: ""
# - Case sensitivity: no
# - Required: no
- ArmarX.ConditionHandler.Observers = Armar3KinematicUnitObserver,SystemObserver,ObjectMemoryObserver
+ArmarX.ConditionHandler.Observers = Armar3KinematicUnitObserver,SystemObserver
# ArmarX.ConditionHandler.MinimumLoggingLevel: Local logging level only for this component
diff --git a/scenarios/tests/StatechartProfilesTestArmar3Simulation/config/RobotControl.cfg b/scenarios/tests/StatechartProfilesTestArmar3Simulation/config/RobotControl.cfg
index 01fcafba4fec1e48dbcca6eb8abde6ec4582352c..6c8b174ef8ed1ea4d7a1aee4bf64f939d87bee4b 100644
--- a/scenarios/tests/StatechartProfilesTestArmar3Simulation/config/RobotControl.cfg
+++ b/scenarios/tests/StatechartProfilesTestArmar3Simulation/config/RobotControl.cfg
@@ -112,6 +112,6 @@ ArmarX.RobotControlStateOfferer.proxyName = "StatechartProfilesTestGroupRemoteSt
# - Case sensitivity: no
# - Required: no
# ArmarX.RobotControlStateOfferer.ObjectName = ""
-
+ArmarX.RobotControlStateOfferer.XMLStatechartProfile = "Armar3Simulation"
diff --git a/scenarios/tests/StatechartProfilesTestArmar3a/config/RobotControl.cfg b/scenarios/tests/StatechartProfilesTestArmar3a/config/RobotControl.cfg
index 01fcafba4fec1e48dbcca6eb8abde6ec4582352c..0de35bad4f0b589d4eee3c528a38b196d3119eeb 100644
--- a/scenarios/tests/StatechartProfilesTestArmar3a/config/RobotControl.cfg
+++ b/scenarios/tests/StatechartProfilesTestArmar3a/config/RobotControl.cfg
@@ -113,5 +113,5 @@ ArmarX.RobotControlStateOfferer.proxyName = "StatechartProfilesTestGroupRemoteSt
# - Required: no
# ArmarX.RobotControlStateOfferer.ObjectName = ""
-
+ArmarX.XMLStateComponent.XMLStatechartProfile = "Armar3a"
diff --git a/source/RobotAPI/components/RobotState/CMakeLists.txt b/source/RobotAPI/components/RobotState/CMakeLists.txt
index 246213dd5994fd1a196ebbcb087962382581b6b3..fcf6bab1cd6f40dbceed78121dbb8a67cc25ce53 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 d967974ffb713619f554f08a2a2aa898fc9c700f..e9de3e5a39c967e864cdfa670c2561a4d41f1efd 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))
@@ -132,7 +136,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()
@@ -160,16 +167,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)
{
{
@@ -194,9 +303,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";
@@ -249,30 +367,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(
@@ -309,3 +403,6 @@ namespace armarx
}
+
+
+
diff --git a/source/RobotAPI/components/RobotState/RobotStateComponent.h b/source/RobotAPI/components/RobotState/RobotStateComponent.h
index f1a3b2d827facc4e15c0e0dc9a7d2b6fc8093297..c42ed7cb6fe25358c884ca2e5498b24c38078211 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");
defineOptionalProperty<float>("RobotModelScaling", 1.0f, "Scaling of the robot model");
}
};
@@ -83,7 +84,6 @@ namespace armarx
{
public:
-
/**
* \return SharedRobotInterface proxy to the internal synchronized robot.
*/
@@ -128,6 +128,12 @@ namespace armarx
}
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;
+
float getScaling(const Ice::Current&) const;
protected:
/**
@@ -159,9 +165,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;
std::string robotNodeSetName;
diff --git a/source/RobotAPI/components/RobotState/SharedRobotServants.h b/source/RobotAPI/components/RobotState/SharedRobotServants.h
index 845ed5465c4d5a4a43ad52f7ad9551b9c878f340..178ca105a2dcb4e176c56ce2378e16d6c96a0b02 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 0000000000000000000000000000000000000000..2dfe68fa517eaf200803eaf8b997fcf738e76110
--- /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 0000000000000000000000000000000000000000..2c6d98938b7d20b177764afddbedb2f6f8286804
--- /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/components/units/ForceTorqueObserver.cpp b/source/RobotAPI/components/units/ForceTorqueObserver.cpp
index 5118f2a2e14bce96accc5592d992dfd22d263778..edf26cd26e856103945f568019cf8798335cd5d4 100644
--- a/source/RobotAPI/components/units/ForceTorqueObserver.cpp
+++ b/source/RobotAPI/components/units/ForceTorqueObserver.cpp
@@ -86,7 +86,7 @@ void ForceTorqueObserver::onConnectObserver()
debugDrawer = getTopic<DebugDrawerInterfacePrx>("DebugDrawerUpdates");
if (getProperty<bool>("VisualizeForce").getValue())
{
- visualizerTask = new PeriodicTask<ForceTorqueObserver>(this, &ForceTorqueObserver::visualizerFunction, getProperty<int>("VisualizeForceUpdateFrequency").getValue(), false, "visualizerFunction");
+ visualizerTask = new PeriodicTask<ForceTorqueObserver>(this, &ForceTorqueObserver::visualizerFunction, 1000 / getProperty<int>("VisualizeForceUpdateFrequency").getValue(), false, "visualizerFunction");
visualizerTask->start();
}
@@ -164,38 +164,61 @@ PropertyDefinitionsPtr ForceTorqueObserver::createPropertyDefinitions()
}
-void ForceTorqueObserver::offerValue(std::string nodeName, const std::string& type, FramedDirectionBasePtr value, const DataFieldIdentifierPtr& id)
+void ForceTorqueObserver::offerValue(const std::string& nodeName, const std::string& type, const FramedDirectionBasePtr& value, const DataFieldIdentifierPtr& id)
{
FramedDirectionPtr vec = FramedDirectionPtr::dynamicCast(value);
- if (!existsDataField(id->channelName, id->datafieldName))
+
+ try
{
- if (!existsChannel(id->channelName))
- {
- offerChannel(id->channelName, type + " vectors on specific parts of the robot.");
- }
- offerDataFieldWithDefault(id->channelName, id->datafieldName, Variant(value), "3D vector for " + type + " of " + nodeName);
+ setDataFieldFlatCopy(id->channelName, id->datafieldName, new Variant(value));
}
- else
+ catch (...)
{
- setDataFieldFlatCopy(id->channelName, id->datafieldName, new Variant(value));
+ ARMARX_INFO << "Creating force/torque fields";
+ if (!existsDataField(id->channelName, id->datafieldName))
+ {
+ if (!existsChannel(id->channelName))
+ {
+ offerChannel(id->channelName, type + " vectors on specific parts of the robot.");
+ }
+ offerDataFieldWithDefault(id->channelName, id->datafieldName, Variant(value), "3D vector for " + type + " of " + nodeName);
+ }
}
// pod = plain old data
std::string pod_channelName = id->channelName + POD_SUFFIX;
- if (!existsChannel(pod_channelName))
+
+
+ try
+ {
+ StringVariantBaseMap values;
+ values[id->datafieldName + "_x" ] = new Variant(vec->x);
+ values[id->datafieldName + "_y" ] = new Variant(vec->y);
+ values[id->datafieldName + "_z" ] = new Variant(vec->z);
+ values[id->datafieldName + "_frame"] = new Variant(vec->frame);
+ setDataFieldsFlatCopy(pod_channelName, values);
+ }
+ catch (...)
{
- offerChannel(pod_channelName, id->datafieldName + " on " + nodeName + " as plain old data (pod)");
+ ARMARX_INFO << "Creating force/torque pod fields";
+ if (!existsChannel(pod_channelName))
+ {
+ offerChannel(pod_channelName, id->datafieldName + " on " + nodeName + " as plain old data (pod)");
+
+ }
+ offerOrUpdateDataField(pod_channelName, id->datafieldName + "_x", Variant(vec->x), type + " on X axis");
+ offerOrUpdateDataField(pod_channelName, id->datafieldName + "_y", Variant(vec->y), type + " on Y axis");
+ offerOrUpdateDataField(pod_channelName, id->datafieldName + "_z", Variant(vec->z), type + " on Z axis");
+ offerOrUpdateDataField(pod_channelName, id->datafieldName + "_frame", Variant(vec->frame), "Frame of " + value->frame);
}
- offerOrUpdateDataField(pod_channelName, id->datafieldName + "_x", Variant(vec->x), type + " on X axis");
- offerOrUpdateDataField(pod_channelName, id->datafieldName + "_y", Variant(vec->y), type + " on Y axis");
- offerOrUpdateDataField(pod_channelName, id->datafieldName + "_z", Variant(vec->z), type + " on Z axis");
- offerOrUpdateDataField(pod_channelName, id->datafieldName + "_frame", Variant(vec->frame), "Frame of " + value->frame);
+
+
}
void armarx::ForceTorqueObserver::reportSensorValues(const std::string& sensorNodeName, const FramedDirectionBasePtr& forces, const FramedDirectionBasePtr& torques, const Ice::Current&)
@@ -223,7 +246,7 @@ void armarx::ForceTorqueObserver::reportSensorValues(const std::string& sensorNo
updateChannel(id->channelName);
updateChannel(id->channelName + POD_SUFFIX);
}
- catch (std::exception& e)
+ catch (std::exception&)
{
ARMARX_ERROR << "Reporting force torque for " << sensorNodeName << " failed! ";
handleExceptions();
diff --git a/source/RobotAPI/components/units/ForceTorqueObserver.h b/source/RobotAPI/components/units/ForceTorqueObserver.h
index 6e17f88e8dbf7f7efabfd327a5e1463a542bb616..e291a52e49d65f872febe613e777f97fc3b8c771 100644
--- a/source/RobotAPI/components/units/ForceTorqueObserver.h
+++ b/source/RobotAPI/components/units/ForceTorqueObserver.h
@@ -43,8 +43,8 @@ namespace armarx
{
defineRequiredProperty<std::string>("ForceTorqueTopicName", "Name of the ForceTorqueUnit Topic");
defineOptionalProperty<bool>("VisualizeForce", true, "Visualize the force with an arrow in the debug drawer");
- defineOptionalProperty<int>("VisualizeForceUpdateFrequency", 20, "Frequency with which the force is visualized");
- defineOptionalProperty<float>("ForceVisualizerFactor", 0.01, "Factor by which the forces are scaled to fit into 0..1 (only for visulization) ");
+ defineOptionalProperty<int>("VisualizeForceUpdateFrequency", 30, "Frequency with which the force is visualized");
+ defineOptionalProperty<float>("ForceVisualizerFactor", 0.01f, "Factor by which the forces are scaled to fit into 0..1 (only for visulization) ");
defineOptionalProperty<float>("MaxForceArrowLength", 150, "Length of the force visu arrow in mm");
defineOptionalProperty<std::string>("RobotStateComponentName", "RobotStateComponent", "Name of the RobotStateComponent that should be used");
@@ -99,7 +99,7 @@ namespace armarx
DebugDrawerInterfacePrx debugDrawer;
PeriodicTask<ForceTorqueObserver>::pointer_type visualizerTask;
- void offerValue(std::string nodeName, const std::string& type, FramedDirectionBasePtr value, const DataFieldIdentifierPtr& id);
+ void offerValue(const std::string& nodeName, const std::string& type, const FramedDirectionBasePtr& value, const DataFieldIdentifierPtr& id);
// ForceTorqueUnitObserverInterface interface
public:
diff --git a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitConfigDialog.cpp b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitConfigDialog.cpp
index 01596e7e599a2fe8cf22a57e095e5fae4ed1336a..f70637e0f1b7fba0562011159b5bfd6e0fd45eb2 100644
--- a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitConfigDialog.cpp
+++ b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitConfigDialog.cpp
@@ -49,10 +49,11 @@ KinematicUnitConfigDialog::KinematicUnitConfigDialog(QWidget* parent) :
connect(this->ui->buttonBox, SIGNAL(accepted()), this, SLOT(verifyConfig()));
ui->buttonBox->button(QDialogButtonBox::Ok)->setDefault(true);
proxyFinder = new IceProxyFinder<KinematicUnitInterfacePrx>(this);
- proxyFinder->setSearchMask("*");
+ proxyFinder->setSearchMask("*Unit");
ui->gridLayout->addWidget(proxyFinder, 0, 1, 1, 2);
connect(proxyFinder->getUi()->cbProxyName, SIGNAL(currentIndexChanged(int)), this, SLOT(selectionChanged(int)));
+ connect(proxyFinder->getUi()->cbProxyName, SIGNAL(editTextChanged(QString)), this, SLOT(proxyNameChanged(QString)));
}
KinematicUnitConfigDialog::~KinematicUnitConfigDialog()
@@ -88,20 +89,13 @@ void KinematicUnitConfigDialog::verifyConfig()
}
}
-void KinematicUnitConfigDialog::selectionChanged(int nr)
+void KinematicUnitConfigDialog::updateSubconfig(std::string kinematicUnitName)
{
- ARMARX_LOG << "Selected entry:" << nr;
- ui->labelTopic->setText("");
- ui->labelRobotModel->setText("");
- ui->labelRNS->setText("");
- if (nr < 0)
- {
- return;
- }
- std::string kinematicUnitName = proxyFinder->getUi()->cbProxyName->currentText().toStdString();
+
try
{
ARMARX_INFO << "Connecting to KinematicUnitProxy " << kinematicUnitName;
+
KinematicUnitInterfacePrx kinematicUnitInterfacePrx = getProxy<KinematicUnitInterfacePrx>(kinematicUnitName);
std::string topicName = kinematicUnitInterfacePrx->getReportTopicName();
std::string robotNodeSetName = kinematicUnitInterfacePrx->getRobotNodeSetName();
@@ -114,7 +108,30 @@ void KinematicUnitConfigDialog::selectionChanged(int nr)
{
ARMARX_INFO << "Could not connect to KinematicUnitProxy " << kinematicUnitName;
}
+}
+
+void KinematicUnitConfigDialog::selectionChanged(int nr)
+{
+ ARMARX_LOG << "Selected entry:" << nr;
+ ui->labelTopic->setText("");
+ ui->labelRobotModel->setText("");
+ ui->labelRNS->setText("");
+ if (nr < 0)
+ {
+ return;
+ }
+ std::string kinematicUnitName = proxyFinder->getUi()->cbProxyName->currentText().toStdString();
+
+ updateSubconfig(kinematicUnitName);
+
+}
+void KinematicUnitConfigDialog::proxyNameChanged(QString kinematicUnitName)
+{
+ ui->labelTopic->setText("");
+ ui->labelRobotModel->setText("");
+ ui->labelRNS->setText("");
+ updateSubconfig(kinematicUnitName.toStdString());
}
diff --git a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitConfigDialog.h b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitConfigDialog.h
index 9e0ab1ea9821fe878dd71014dbc54abef8a6d026..21e73d58f30a570633355075a159d19852203ca7 100644
--- a/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitConfigDialog.h
+++ b/source/RobotAPI/gui-plugins/KinematicUnitPlugin/KinematicUnitConfigDialog.h
@@ -65,6 +65,10 @@ namespace armarx
void verifyConfig();
void selectionChanged(int nr);
+ void proxyNameChanged(QString);
+ protected slots:
+ void updateSubconfig(std::string kinematicUnitName);
+
private:
IceProxyFinderBase* proxyFinder;
diff --git a/source/RobotAPI/interface/core/RobotState.ice b/source/RobotAPI/interface/core/RobotState.ice
index 0e91dc6c0a332f310a35dea9764e8ff926515f72..1021ea461ab0dcc7300eae518856f2cfb959978d 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
*/
diff --git a/source/RobotAPI/libraries/core/FramedPose.cpp b/source/RobotAPI/libraries/core/FramedPose.cpp
index facb1e3932e9b1a7ac1b4c2510f32cdfbd9bfb80..2ea1a8f0fd280b3e495cab1cdf49eed109012810 100644
--- a/source/RobotAPI/libraries/core/FramedPose.cpp
+++ b/source/RobotAPI/libraries/core/FramedPose.cpp
@@ -4,9 +4,8 @@
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/LinkedCoordinate.h>
-#include <VirtualRobot/LinkedCoordinate.h>
#include <VirtualRobot/VirtualRobot.h>
-
+#include <VirtualRobot/RobotConfig.h>
using namespace Eigen;
using namespace std;
@@ -76,7 +75,7 @@ namespace armarx
{
if (frame == "")
{
- frame == GlobalFrame;
+ frame = GlobalFrame;
}
@@ -205,7 +204,7 @@ namespace armarx
else
{
auto node = robotState->getRobotNode(oldFrame);
- ARMARX_CHECK_EXPRESSION_W_HINT(node, "old frame: " + oldFrame);
+ ARMARX_CHECK_EXPRESSION_W_HINT(node, "old frame: " + oldFrame + ValueToString(robotState->getConfig()->getRobotNodeJointValueMap()));
auto newNode = robotState->getRobotNode(newFrame);
ARMARX_CHECK_EXPRESSION_W_HINT(newNode, newFrame);
toNewFrame = node->getTransformationTo(newNode);
diff --git a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp
index 93c00449519a6ee7f37ff84a846348b76bf6de23..f640b6f7d5ba4c2d724271d1d967fe966f102e70 100644
--- a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp
+++ b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.cpp
@@ -155,6 +155,11 @@ namespace armarx
return p->toEigen(); // convert to eigen first
}
+ SharedRobotInterfacePrx RemoteRobot::getSharedRobot() const
+ {
+ return this->_robot;
+ }
+
string RemoteRobot::getName()
{
return _robot->getName();
@@ -403,6 +408,40 @@ namespace armarx
return true;
}
+ bool RemoteRobot::synchronizeLocalCloneToTimestamp(RobotPtr robot, RobotStateComponentInterfacePrx robotStatePrx, Ice::Long timestamp)
+ {
+ if (!robotStatePrx || !robot)
+ {
+ ARMARX_ERROR_S << "NULL data. Aborting..." << endl;
+ return false;
+ }
+
+ RobotConfigPtr c(new RobotConfig(robot, "synchronizeLocalClone"));
+ RobotStateConfig config = robotStatePrx->getRobotStateAtTimestamp(timestamp);
+ if (config.jointMap.empty())
+ {
+ return false;
+ }
+
+ for (NameValueMap::const_iterator it = config.jointMap.begin(); it != config.jointMap.end(); it++)
+ {
+ // joint values
+ const std::string& jointName = it->first;
+ float jointAngle = it->second;
+
+ if (!c->setConfig(jointName, jointAngle))
+ {
+ ARMARX_WARNING_S << "Joint not known in local copy:" << jointName << ". Skipping..." << endl;
+ }
+ }
+
+ robot->setConfig(c);
+ auto pose = PosePtr::dynamicCast(config.globalPose);
+ robot->setGlobalPose(pose->toEigen());
+ return true;
+ }
+
+
// Private (unused methods)
@@ -432,4 +471,5 @@ namespace armarx
}
}
+
}
diff --git a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.h b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.h
index 5d33c4bb88006f985f89cab605dc2d1670225785..1a7b24532f5baca5567912a50b836e40a56f5747 100644
--- a/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.h
+++ b/source/RobotAPI/libraries/core/remoterobot/RemoteRobot.h
@@ -171,10 +171,7 @@ namespace armarx
/// Use this method to share the robot instance over Ice.
- inline SharedRobotInterfacePrx getSharedRobot()
- {
- return this->_robot;
- }
+ SharedRobotInterfacePrx getSharedRobot() const;
std::string getName();
@@ -192,11 +189,21 @@ namespace armarx
/*!
Use this method to synchronize (i.e. copy the joint values) from the remote robot to the local clone.
The local clone must have the identical structure as the remote robot model, otherwise an error will be reported.
+ This is the fastest way to get update a local robot.
*/
static bool synchronizeLocalClone(VirtualRobot::RobotPtr robot, RobotStateComponentInterfacePrx robotStatePrx);
static bool synchronizeLocalClone(VirtualRobot::RobotPtr robot, SharedRobotInterfacePrx sharedRobotPrx);
+ /**
+ * @brief Synchronizes a local robot to a robot state at timestamp.
+ * @param robot Local robot that should be updated
+ * @param robotStatePrx Proxy to the RobotStateComponent
+ * @param timestamp Timestamp to which the local robot should be sychronized. Must be in range of the state history of the RobotStateComponent.
+ * @return True if successfully synced.
+ */
+ static bool synchronizeLocalCloneToTimestamp(VirtualRobot::RobotPtr robot, RobotStateComponentInterfacePrx robotStatePrx, Ice::Long timestamp);
+
// VirtualRobot::RobotPtr getRobotPtr() { return shared_from_this();} // only for debugging
//! Clones the structure of this remote robot to a local instance
diff --git a/source/RobotAPI/statecharts/operations/RobotControl.cpp b/source/RobotAPI/statecharts/operations/RobotControl.cpp
index 47c178faafd6d44ddb8c597dc59cad5fdb4085c2..d99d262ac3203a9a9037f747b80cc057b02f12e8 100644
--- a/source/RobotAPI/statecharts/operations/RobotControl.cpp
+++ b/source/RobotAPI/statecharts/operations/RobotControl.cpp
@@ -80,7 +80,7 @@ namespace armarx
robotFunctionalState = stateList.begin()->second;
callRemoteState(stateId, StringVariantContainerBaseMap());
- const std::string proxyName = getProperty<std::string>("proxyName").getValue();
+ const std::string proxyName = getProperty<std::string>("XMLStatechartProfile").getValue() + getProperty<std::string>("proxyName").getValue();
const std::string stateName = getProperty<std::string>("stateName").getValue();
ARMARX_IMPORTANT << VAROUT(proxyName) << VAROUT(stateName);
diff --git a/source/RobotAPI/statecharts/operations/RobotControl.h b/source/RobotAPI/statecharts/operations/RobotControl.h
index c0d2cb409b03da05d24c8ea87e18ee8d7ee16949..fa875b00e5da55fc8a6e58a4232bf481f4af342d 100644
--- a/source/RobotAPI/statecharts/operations/RobotControl.h
+++ b/source/RobotAPI/statecharts/operations/RobotControl.h
@@ -38,6 +38,8 @@ namespace armarx
RobotControlContextProperties(std::string prefix):
StatechartContextPropertyDefinitions(prefix)
{
+
+ defineOptionalProperty<std::string>("XMLStatechartProfile", "", "Name of the statechart profile to be used. This is used as prefix to the proxyName. So GraspGroupRemoteStateOfferer will be Armar3aGraspGroupRemoteStateOfferer");
defineOptionalProperty<std::string>("proxyName", "", "name of the proxy to load");
defineOptionalProperty<std::string>("stateName", "", "name of the state to load");
}