diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
index a29e434694593cdb0f87f4e524b1fbafbbc25cf3..760af325a522618218a7d39ceaf45d9fa20fa65a 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
+++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
@@ -30,6 +30,9 @@
#include <VirtualRobot/Robot.h>
#include <VirtualRobot/RobotConfig.h>
+#include <ArmarXCore/core/exceptions/local/UnexpectedEnumValueException.h>
+#include <ArmarXCore/observers/variant/Variant.h>
+
#include <RobotAPI/libraries/core/Pose.h>
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
@@ -49,6 +52,7 @@ namespace armarx
defs->defineOptionalProperty<std::string>("ObjectPoseTopicName", "ObjectPoseTopic", "Name of the Object Pose Topic.");
defs->defineOptionalProperty<std::string>("KinematicUnitObserverName", "KinematicUnitObserver", "Name of the kinematic unit observer.");
+ defs->topic(debugObserver);
defs->optional(visu.enabled, "visu.enabled", "Enable or disable visualization of objects.");
defs->optional(visu.inGlobalFrame, "visu.inGlobalFrame", "If true, show global poses. If false, show poses in robot frame.");
@@ -56,9 +60,11 @@ namespace armarx
defs->optional(visu.oobbs, "visu.oobbs", "Enable showing oriented bounding boxes.");
defs->optional(visu.objectFrames, "visu.objectFrames", "Enable showing object frames.");
defs->optional(visu.objectFramesScale, "visu.objectFramesScale", "Scaling of object frames.");
+
defs->optional(calibration.robotNode, "calibration.robotNode", "Robot node which can be calibrated.");
defs->optional(calibration.offset, "calibration.offset", "Offset for the node to be calibrated.");
- defs->optional(robotHead.checkHeadVelocity, "head.checkHeadVelocity", "If true, check whether the head is moving.");
+
+ defs->optional(robotHead.checkHeadVelocity, "head.checkHeadVelocity", "If true, check whether the head is moving and discard updates in the meantime.");
defs->optional(robotHead.maxJointVelocity, "head.maxJointVelocity", "If a head joint's velocity is higher, the head is considered moving.");
defs->optional(robotHead.discardIntervalAfterMoveMS, "head.discardIntervalAfterMoveMS", "For how long new updates are ignored after moving the head.");
@@ -72,6 +78,8 @@ namespace armarx
void ObjectPoseObserver::onInitObserver()
{
+ robotHead.setTag(getName());
+
usingTopicFromProperty("ObjectPoseTopicName");
}
@@ -85,6 +93,7 @@ namespace armarx
}
getProxyFromProperty(robotHead.kinematicUnitObserver, "KinematicUnitObserverName", false, "", false);
+ robotHead.debugObserver = debugObserver;
robotHead.fetchDatafields();
createRemoteGuiTab();
@@ -100,62 +109,6 @@ namespace armarx
}
- void ObjectPoseObserver::createRemoteGuiTab()
- {
- using namespace armarx::RemoteGui::Client;
-
- tab.visuEnabled.setValue(visu.enabled);
- tab.visuInGlobalFrame.setValue(visu.inGlobalFrame);
- tab.visuAlpha.setRange(0, 1.0);
- tab.visuAlpha.setValue(visu.alpha);
- tab.visuOOBBs.setValue(visu.oobbs);
- tab.visuObjectFrames.setValue(visu.objectFrames);
- {
- float max = 10000;
- tab.visuObjectFramesScale.setRange(0, max);
- tab.visuObjectFramesScale.setDecimals(2);
- tab.visuObjectFramesScale.setSteps(int(10 * max));
- tab.visuObjectFramesScale.setValue(visu.objectFramesScale);
- }
-
- GroupBox visuGroup;
- {
- GridLayout grid;
- int row = 0;
- grid.add(Label("Enabled"), {row, 0}).add(tab.visuEnabled, {row, 1});
- row++;
- grid.add(Label("Global Frame"), {row, 0}).add(tab.visuInGlobalFrame, {row, 1});
- row++;
- grid.add(Label("Alpha"), {row, 0}).add(tab.visuAlpha, {row, 1}, {1, 3});
- row++;
- grid.add(Label("OOBB"), {row, 0}).add(tab.visuOOBBs, {row, 1});
- row++;
- grid.add(Label("Object Frames"), {row, 0}).add(tab.visuObjectFrames, {row, 1});
- grid.add(Label("Scale:"), {row, 2}).add(tab.visuObjectFramesScale, {row, 3});
- row++;
-
- visuGroup.setLabel("Visualization");
- visuGroup.addChild(grid);
- }
-
- VBoxLayout root = {visuGroup, VSpacer()};
- RemoteGui_createTab(getName(), root, &tab);
- }
-
- void ObjectPoseObserver::RemoteGui_update()
- {
- // Non-atomic variables need to be guarded by a mutex if accessed by multiple threads
- std::scoped_lock lock(dataMutex);
-
- visu.enabled = tab.visuEnabled.getValue();
- visu.inGlobalFrame = tab.visuInGlobalFrame.getValue();
- visu.alpha = tab.visuAlpha.getValue();
- visu.oobbs = tab.visuOOBBs.getValue();
- visu.objectFrames = tab.visuObjectFrames.getValue();
- visu.objectFramesScale = tab.visuObjectFramesScale.getValue();
- }
-
-
void ObjectPoseObserver::reportProviderAvailable(const std::string& providerName, const objpose::ProviderInfo& info, const Ice::Current&)
{
updateProviderInfo(providerName, info);
@@ -167,25 +120,45 @@ namespace armarx
{
ARMARX_VERBOSE << "Received object poses from '" << providerName << "'.";
- IceUtil::Time discardUpdatesUntil;
- if (robotHead.checkHeadVelocity)
+ std::optional<IceUtil::Time> discardUpdatesUntil;
+ bool discardAll = false;
{
std::scoped_lock lock(robotHeadMutex);
- if (robotHead.isMoving())
+ if (robotHead.checkHeadVelocity)
{
- robotHead.discardUpdatesUntil = TimeUtil::GetTime() + IceUtil::Time::milliSeconds(robotHead.discardIntervalAfterMoveMS);
- // ARMARX_IMPORTANT << "Ignoring update because robot head is moving! until " << robotHead.discardUpdatesUntil;
- return;
+ if (robotHead.isMoving())
+ {
+ robotHead.movementStarts(robotHead.discardIntervalAfterMoveMS);
+ // ARMARX_IMPORTANT << "Ignoring pose update because robot head is moving! until " << robotHead.discardUpdatesUntil;
+ discardAll = true;
+ }
+ else if (TimeUtil::GetTime() < robotHead.discardUpdatesUntil)
+ {
+ discardAll = true;
+ // ARMARX_IMPORTANT << "Ignoring pose update because robot head has moved until: " << robotHead.discardUpdatesUntil;
+ }
+ else
+ {
+ discardUpdatesUntil = robotHead.discardUpdatesUntil;
+ }
}
- if (TimeUtil::GetTime() < robotHead.discardUpdatesUntil)
+ }
+ if (debugObserver)
+ {
+ StringVariantBaseMap map =
{
- // ARMARX_IMPORTANT << "Ignoring update because robot head is moved until: " << robotHead.discardUpdatesUntil;
- return;
+ { "Discarding All Updates", new Variant(discardAll ? 1.f : 0.f) },
+ };
+ if (discardAll)
+ {
+ map["Proportion Updated Poses"] = new Variant(0.f);
}
- discardUpdatesUntil = robotHead.discardUpdatesUntil;
+ debugObserver->setDebugChannel(getName(), map);
+ }
+ if (discardAll)
+ {
+ return;
}
-
- objpose::ObjectPoseSeq newObjectPoses;
{
std::scoped_lock lock(dataMutex);
RobotState::synchronizeLocalClone(robot);
@@ -204,10 +177,14 @@ namespace armarx
previousPoses = it->second;
}
+ // Build new poses.
+ objpose::ObjectPoseSeq newObjectPoses;
+ int numUpdated = 0;
for (const objpose::data::ProvidedObjectPose& provided : providedPoses)
{
IceUtil::Time timestamp = IceUtil::Time::microSeconds(provided.timestampMicroSeconds);
+ // Check whether we have an old pose for this object.
std::optional<objpose::ObjectPose> previousPose;
for (const objpose::ObjectPose& prev : previousPoses)
{
@@ -217,7 +194,7 @@ namespace armarx
}
}
- if (robotHead.checkHeadVelocity && timestamp < discardUpdatesUntil)
+ if (discardUpdatesUntil && timestamp < *discardUpdatesUntil)
{
if (previousPose)
{
@@ -238,6 +215,7 @@ namespace armarx
}
else
{
+ numUpdated++;
objpose::ObjectPose& newPose = newObjectPoses.emplace_back();
newPose.fromProvidedPose(provided, robot);
if (newPose.objectID.dataset().empty())
@@ -250,14 +228,21 @@ namespace armarx
}
if (!provided.localOOBB)
{
+ // Try to load oobb from disk.
newPose.localOOBB = getObjectOOBB(newPose.objectID);
}
}
}
- }
- {
- std::scoped_lock lock(dataMutex);
+ if (debugObserver)
+ {
+ debugObserver->setDebugChannel(getName(),
+ {
+ { "Discarding All Updates", new Variant(discardAll ? 1 : 0) },
+ { "Proportion Updated Poses", new Variant(static_cast<float>(numUpdated) / providedPoses.size()) }
+ });
+ }
+
this->objectPoses[providerName] = newObjectPoses;
handleProviderUpdate(providerName);
}
@@ -619,6 +604,28 @@ namespace armarx
return output;
}
+ objpose::SignalHeadMovementOutput
+ ObjectPoseObserver::signalHeadMovement(const objpose::SignalHeadMovementInput& input, const Ice::Current&)
+ {
+ std::scoped_lock lock(robotHeadMutex);
+ objpose::SignalHeadMovementOutput output;
+ switch (input.action)
+ {
+ case objpose::HeadMovementAction::HeadMovementAction_Starting:
+ robotHead.movementStarts(input.discardUpdatesIntervalMilliSeconds < 0
+ ? robotHead.discardIntervalAfterMoveMS
+ : input.discardUpdatesIntervalMilliSeconds);
+ break;
+ case objpose::HeadMovementAction::HeadMovementAction_Stopping:
+ robotHead.movementStops(input.discardUpdatesIntervalMilliSeconds);
+ break;
+ default:
+ ARMARX_UNEXPECTED_ENUM_VALUE(objpose::HeadMovementaction, input.action);
+ break;
+ }
+ output.discardUpdatesUntilMilliSeconds = robotHead.discardUpdatesUntil.toMilliSeconds();
+ return output;
+ }
void ObjectPoseObserver::handleProviderUpdate(const std::string& providerName)
@@ -826,7 +833,7 @@ namespace armarx
}
}
- void ObjectPoseObserver::RobotHead::fetchDatafields()
+ void ObjectPoseObserver::RobotHeadMovement::fetchDatafields()
{
if (kinematicUnitObserver)
{
@@ -852,21 +859,25 @@ namespace armarx
}
if (error.size() > 0)
{
- ARMARX_WARNING_S << "Could not get datafield for joint '" << jointName << "' in channel '" << jointVelocitiesChannelName << "': \n "
- << error;
+ ARMARX_WARNING << "Could not get datafield for joint '" << jointName << "' in channel '" << jointVelocitiesChannelName << "': \n "
+ << error;
}
}
}
+ else
+ {
+ ARMARX_INFO << "Cannot fetch joint velocity datafields because there is no kinematic unit observer.";
+ }
}
- bool ObjectPoseObserver::RobotHead::isMoving() const
+ bool ObjectPoseObserver::RobotHeadMovement::isMoving() const
{
for (DatafieldRefPtr df : jointVelocitiesDatafields)
{
if (df)
{
float jointVelocity = df->getFloat();
- // ARMARX_IMPORTANT_S << "Is " << df->datafieldName << " " << VAROUT(std::abs(jointVelocity)) << " > " << VAROUT(maxJointVelocity) << "?";
+ // ARMARX_IMPORTANT << "Is " << df->datafieldName << " " << VAROUT(std::abs(jointVelocity)) << " > " << VAROUT(maxJointVelocity) << "?";
if (std::abs(jointVelocity) > maxJointVelocity)
{
return true;
@@ -876,5 +887,123 @@ namespace armarx
return false;
}
+ void ObjectPoseObserver::RobotHeadMovement::movementStarts(long discardIntervalMs)
+ {
+ return movementStarts(IceUtil::Time::milliSeconds(discardIntervalMs));
+ }
+ void ObjectPoseObserver::RobotHeadMovement::movementStarts(IceUtil::Time discardInterval)
+ {
+ discardUpdatesUntil = TimeUtil::GetTime() + discardInterval;
+ }
+ void ObjectPoseObserver::RobotHeadMovement::movementStops(long discardIntervalMs)
+ {
+ return movementStops(IceUtil::Time::milliSeconds(discardIntervalMs));
+ }
+ void ObjectPoseObserver::RobotHeadMovement::movementStops(IceUtil::Time discardInterval)
+ {
+ if (discardInterval.toMilliSeconds() < 0)
+ {
+ // Stop discarding.
+ discardUpdatesUntil = IceUtil::Time::milliSeconds(-1);
+ }
+ else
+ {
+ // Basically the same as starting.
+ discardUpdatesUntil = TimeUtil::GetTime() + discardInterval;
+ }
+ }
+
+
+ void ObjectPoseObserver::createRemoteGuiTab()
+ {
+ using namespace armarx::RemoteGui::Client;
+
+ GroupBox visuGroup;
+ {
+ tab.visu.enabled.setValue(visu.enabled);
+ tab.visu.inGlobalFrame.setValue(visu.inGlobalFrame);
+ tab.visu.alpha.setRange(0, 1.0);
+ tab.visu.alpha.setValue(visu.alpha);
+ tab.visu.oobbs.setValue(visu.oobbs);
+ tab.visu.objectFrames.setValue(visu.objectFrames);
+ {
+ float max = 10000;
+ tab.visu.objectFramesScale.setRange(0, max);
+ tab.visu.objectFramesScale.setDecimals(2);
+ tab.visu.objectFramesScale.setSteps(int(10 * max));
+ tab.visu.objectFramesScale.setValue(visu.objectFramesScale);
+ }
+
+ GridLayout grid;
+ int row = 0;
+ grid.add(Label("Enabled"), {row, 0}).add(tab.visu.enabled, {row, 1});
+ row++;
+ grid.add(Label("Global Frame"), {row, 0}).add(tab.visu.inGlobalFrame, {row, 1});
+ row++;
+ grid.add(Label("Alpha"), {row, 0}).add(tab.visu.alpha, {row, 1}, {1, 3});
+ row++;
+ grid.add(Label("OOBB"), {row, 0}).add(tab.visu.oobbs, {row, 1});
+ row++;
+ grid.add(Label("Object Frames"), {row, 0}).add(tab.visu.objectFrames, {row, 1});
+ grid.add(Label("Scale:"), {row, 2}).add(tab.visu.objectFramesScale, {row, 3});
+ row++;
+
+ visuGroup.setLabel("Visualization");
+ visuGroup.addChild(grid);
+ }
+
+ GroupBox robotHeadGroup;
+ {
+ tab.robotHead.checkHeadVelocity.setValue(robotHead.checkHeadVelocity);
+ {
+ float max = 10.0;
+ tab.robotHead.maxJointVelocity.setRange(0, max);
+ tab.robotHead.maxJointVelocity.setDecimals(3);
+ tab.robotHead.maxJointVelocity.setSteps(int(100 * max)); // = 0.01 steps
+ tab.robotHead.maxJointVelocity.setValue(robotHead.maxJointVelocity);
+ }
+ {
+ int max = 10 * 1000;
+ tab.robotHead.discardIntervalAfterMoveMS.setRange(0, max);
+ tab.robotHead.discardIntervalAfterMoveMS.setValue(robotHead.discardIntervalAfterMoveMS);
+ }
+
+ GridLayout grid;
+ int row = 0;
+ grid.add(Label("Check Head Motion"), {row, 0}).add(tab.robotHead.checkHeadVelocity, {row, 1});
+ row++;
+ grid.add(Label("Max Joint Velocity"), {row, 0}).add(tab.robotHead.maxJointVelocity, {row, 1});
+ row++;
+ grid.add(Label("Discard Interval after Move [ms]"), {row, 0}).add(tab.robotHead.discardIntervalAfterMoveMS, {row, 1});
+ row++;
+
+ robotHeadGroup.setLabel("Robot Head Motion");
+ robotHeadGroup.addChild(grid);
+ }
+
+ VBoxLayout root = {visuGroup, robotHeadGroup, VSpacer()};
+ RemoteGui_createTab(getName(), root, &tab);
+ }
+
+ void ObjectPoseObserver::RemoteGui_update()
+ {
+ // Non-atomic variables need to be guarded by a mutex if accessed by multiple threads
+ {
+ std::scoped_lock lock(dataMutex);
+
+ visu.enabled = tab.visu.enabled.getValue();
+ visu.inGlobalFrame = tab.visu.inGlobalFrame.getValue();
+ visu.alpha = tab.visu.alpha.getValue();
+ visu.oobbs = tab.visu.oobbs.getValue();
+ visu.objectFrames = tab.visu.objectFrames.getValue();
+ visu.objectFramesScale = tab.visu.objectFramesScale.getValue();
+ }
+ {
+ std::scoped_lock lock(robotHeadMutex);
+ robotHead.checkHeadVelocity = tab.robotHead.checkHeadVelocity.getValue();
+ robotHead.maxJointVelocity = tab.robotHead.maxJointVelocity.getValue();
+ robotHead.discardIntervalAfterMoveMS = tab.robotHead.discardIntervalAfterMoveMS.getValue();
+ }
+ }
}
diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h
index 1d5809452645dd1b7952a87ced9d8d05898fc44d..5022afbf2c92b7cba7a7c2455dbe26d0e3c1f88f 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h
+++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h
@@ -26,6 +26,7 @@
#include <SimoxUtility/caching/CacheMap.h>
+#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/observers/Observer.h>
#include <ArmarXCore/observers/variant/DatafieldRef.h>
@@ -74,7 +75,6 @@ namespace armarx
, virtual public armarx::RobotStateComponentPluginUser
, virtual public armarx::LightweightRemoteGuiComponentPluginUser
, virtual public armarx::ArVizComponentPluginUser
-
{
public:
using RobotState = armarx::RobotStateComponentPluginUser;
@@ -116,6 +116,11 @@ namespace armarx
objpose::AgentFramesSeq getAttachableFrames(ICE_CURRENT_ARG) override;
+ // HEAD MOVEMENT SIGNALS
+
+ objpose::SignalHeadMovementOutput signalHeadMovement(const objpose::SignalHeadMovementInput& input, ICE_CURRENT_ARG) override;
+
+
// Remote GUI
void createRemoteGuiTab();
@@ -124,13 +129,14 @@ namespace armarx
protected:
+ armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
+
void onInitObserver() override;
void onConnectObserver() override;
void onDisconnectComponent() override;
void onExitObserver() override;
- armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
private:
@@ -153,45 +159,55 @@ namespace armarx
void toIceWithAttachments(objpose::ObjectPoseSeq& objectPoses, VirtualRobot::RobotPtr agent,
objpose::data::ObjectPoseSeq& result, bool& synchronized);
-
private:
std::mutex dataMutex;
VirtualRobot::RobotPtr robot;
+ DebugObserverInterfacePrx debugObserver;
+
+
+ objpose::ProviderInfoMap providers;
+
+ std::map<std::string, objpose::ObjectPoseSeq> objectPoses;
+ std::map<std::string, int> updateCounters;
+
+ std::map<std::pair<std::string, ObjectID>, objpose::ObjectAttachmentInfo> attachments;
+
+
+ ObjectFinder objectFinder;
+ /// Caches results of attempts to retrieve the OOBB from ArmarXObjects.
+ simox::caching::CacheMap<ObjectID, std::optional<simox::OrientedBoxf>> oobbCache;
+
- struct RobotHead
+ class RobotHeadMovement : public armarx::Logging
{
+ public:
+
bool checkHeadVelocity = true;
std::string jointVelocitiesChannelName = "jointvelocities";
std::vector<std::string> jointNames = {"Neck_1_Yaw", "Neck_2_Pitch"};
- float maxJointVelocity = 0.05;
+ float maxJointVelocity = 0.05f;
int discardIntervalAfterMoveMS = 100;
KinematicUnitObserverInterfacePrx kinematicUnitObserver;
std::vector<DatafieldRefPtr> jointVelocitiesDatafields;
IceUtil::Time discardUpdatesUntil = IceUtil::Time::seconds(-1);
+ DebugObserverInterfacePrx debugObserver;
+
void fetchDatafields();
bool isMoving() const;
+
+ void movementStarts(long discardIntervalMs);
+ void movementStarts(IceUtil::Time discardInterval);
+ void movementStops(long discardIntervalMs);
+ void movementStops(IceUtil::Time discardInterval);
};
- RobotHead robotHead;
+ RobotHeadMovement robotHead;
std::mutex robotHeadMutex;
-
- objpose::ProviderInfoMap providers;
-
- std::map<std::string, objpose::ObjectPoseSeq> objectPoses;
- std::map<std::string, int> updateCounters;
-
- std::map<std::pair<std::string, ObjectID>, objpose::ObjectAttachmentInfo> attachments;
-
-
- ObjectFinder objectFinder;
- /// Caches results of attempts to retrieve the OOBB from ArmarXObjects.
- simox::caching::CacheMap<ObjectID, std::optional<simox::OrientedBoxf>> oobbCache;
-
struct Visu
{
bool enabled = false;
@@ -211,16 +227,26 @@ namespace armarx
Calibration calibration;
-
-
struct RemoteGuiTab : RemoteGui::Client::Tab
{
- RemoteGui::Client::CheckBox visuEnabled;
- RemoteGui::Client::CheckBox visuInGlobalFrame;
- RemoteGui::Client::FloatSlider visuAlpha;
- RemoteGui::Client::CheckBox visuOOBBs;
- RemoteGui::Client::CheckBox visuObjectFrames;
- RemoteGui::Client::FloatSpinBox visuObjectFramesScale;
+ struct Visu
+ {
+ RemoteGui::Client::CheckBox enabled;
+ RemoteGui::Client::CheckBox inGlobalFrame;
+ RemoteGui::Client::FloatSlider alpha;
+ RemoteGui::Client::CheckBox oobbs;
+ RemoteGui::Client::CheckBox objectFrames;
+ RemoteGui::Client::FloatSpinBox objectFramesScale;
+ };
+ Visu visu;
+
+ struct RobotHead
+ {
+ RemoteGui::Client::CheckBox checkHeadVelocity;
+ RemoteGui::Client::FloatSpinBox maxJointVelocity;
+ RemoteGui::Client::IntSpinBox discardIntervalAfterMoveMS;
+ };
+ RobotHead robotHead;
};
RemoteGuiTab tab;
diff --git a/source/RobotAPI/interface/objectpose/ObjectPoseObserver.ice b/source/RobotAPI/interface/objectpose/ObjectPoseObserver.ice
index 602043ab3b17e3afbdfbbd87b835dda80f264f29..850647b279975f4f79adfb95d89b46c10ebb5c8e 100644
--- a/source/RobotAPI/interface/objectpose/ObjectPoseObserver.ice
+++ b/source/RobotAPI/interface/objectpose/ObjectPoseObserver.ice
@@ -104,6 +104,35 @@ module armarx
};
sequence<AgentFrames> AgentFramesSeq;
+
+ enum HeadMovementAction
+ {
+ /// Head movement is about to start.
+ HeadMovementAction_Starting,
+ /// Head movement is stopping.
+ HeadMovementAction_Stopping
+ };
+ struct SignalHeadMovementInput
+ {
+ HeadMovementAction action;
+
+ /**
+ * If starting, for how long (after this call) new poses shall be discarded
+ * due to head movement.
+ * When set to -1 (default), the configured value in the component is used.
+ *
+ * If stopping, for how long (after this call) new poses shall be discarded
+ * due to remaining head movement.
+ * When set to -1 (default), resume processing updates immediately.
+ */
+ long discardUpdatesIntervalMilliSeconds = -1;
+ };
+ struct SignalHeadMovementOutput
+ {
+ /// The time until new updates will be discarded.
+ long discardUpdatesUntilMilliSeconds = -1;
+ };
+
interface ObjectPoseObserverInterface extends ObserverInterface, ObjectPoseTopic
{
// Object poses
@@ -133,6 +162,15 @@ module armarx
AgentFramesSeq getAttachableFrames();
+
+ /*
+ * Notifying about head/camera movement.
+ * While the head is moving (and a short time afterwards), pose updates are discarded
+ * as they can be bad due to camera movement.
+ */
+
+ SignalHeadMovementOutput signalHeadMovement(SignalHeadMovementInput input);
+
};
};
diff --git a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateHelper.cpp b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateHelper.cpp
index 3533eb02cc11c3598813f330e202e9b673155136..c4f221549de4e12a04e1a495e7a8448fa9c64b3e 100644
--- a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateHelper.cpp
+++ b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateHelper.cpp
@@ -38,8 +38,7 @@ GraspCandidateHelper::GraspCandidateHelper(const grasping::GraspCandidatePtr& ca
Eigen::Matrix4f GraspCandidateHelper::getGraspPoseInRobotRoot() const
{
- const Eigen::Matrix4f curRobotPose = robot->getGlobalPose();
- const Eigen::Matrix4f graspPose = curRobotPose.inverse() * getGraspPoseInGlobal();
+ const Eigen::Matrix4f graspPose = getFramedPose().toRootEigen(robot);
return graspPose;
}
@@ -58,11 +57,18 @@ Eigen::Vector3f GraspCandidateHelper::getGraspPositionInRobotRoot() const
return math::Helpers::GetPosition(getGraspPoseInRobotRoot());
}
+armarx::FramedPose GraspCandidateHelper::getFramedPose() const
+{
+ return armarx::FramedPose(candidate->graspPose->position,
+ candidate->graspPose->orientation,
+ candidate->sourceFrame,
+ robot->getName());
+}
+
Eigen::Matrix4f GraspCandidateHelper::getGraspPoseInGlobal() const
{
- const Eigen::Matrix4f oldGraspPose = defrost(candidate->graspPose);
- const Eigen::Matrix4f oldRobotPose = defrost(candidate->robotPose);
- return oldRobotPose * oldGraspPose;
+ auto grasp_pose = getFramedPose();
+ return grasp_pose.toGlobalEigen(robot);
}
Eigen::Matrix3f GraspCandidateHelper::getGraspOrientationInGlobal() const
{
diff --git a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateHelper.h b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateHelper.h
index 050958f91cb13e5ad34f1a87110e759bc5402b1e..6c9ba617bc511278c77bc147eabb2ac18abc3b19 100644
--- a/source/RobotAPI/libraries/GraspingUtility/GraspCandidateHelper.h
+++ b/source/RobotAPI/libraries/GraspingUtility/GraspCandidateHelper.h
@@ -30,6 +30,7 @@
#include <VirtualRobot/Robot.h>
#include <RobotAPI/libraries/core/Pose.h>
+#include <RobotAPI/libraries/core/FramedPose.h>
namespace armarx
{
@@ -50,6 +51,7 @@ namespace armarx
Eigen::Matrix3f getGraspOrientationInRobotRoot() const;
Eigen::Vector3f getGraspPositionInRobotRoot() const;
+ armarx::FramedPose getFramedPose() const;
Eigen::Matrix4f getGraspPoseInGlobal() const;
Eigen::Matrix3f getGraspOrientationInGlobal() const;
Eigen::Vector3f getGraspPositionInGlobal() const;
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/VelocityControllerHelper.cpp b/source/RobotAPI/libraries/RobotStatechartHelpers/VelocityControllerHelper.cpp
index d4d9a88466d407b83d72d9a5db48f2cade93aa42..456671e79cc764fc577121082967fcc52e341361 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/VelocityControllerHelper.cpp
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/VelocityControllerHelper.cpp
@@ -50,7 +50,7 @@ namespace armarx
}
else
{
- ctrl = robotUnit->createNJointController("NJointCartesianVelocityControllerWithRamp", controllerName, config);
+ ctrl = robotUnit->createOrReplaceNJointController("NJointCartesianVelocityControllerWithRamp", controllerName, config);
controllerCreated = true;
}
controller = NJointCartesianVelocityControllerWithRampInterfacePrx::checkedCast(ctrl);