diff --git a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
index 31c31934b335d79b7eb426e6606179c21d910546..760af325a522618218a7d39ceaf45d9fa20fa65a 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
+++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.cpp
@@ -30,10 +30,14 @@
#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>
+
namespace armarx
{
@@ -47,6 +51,8 @@ namespace armarx
armarx::PropertyDefinitionsPtr defs(new ObjectPoseObserverPropertyDefinitions(getConfigIdentifier()));
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.");
@@ -54,8 +60,13 @@ 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(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 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.");
return defs;
}
@@ -67,6 +78,8 @@ namespace armarx
void ObjectPoseObserver::onInitObserver()
{
+ robotHead.setTag(getName());
+
usingTopicFromProperty("ObjectPoseTopicName");
}
@@ -79,6 +92,10 @@ namespace armarx
robot = RobotState::addRobot("robot", VirtualRobot::RobotIO::RobotDescription::eStructure);
}
+ getProxyFromProperty(robotHead.kinematicUnitObserver, "KinematicUnitObserverName", false, "", false);
+ robotHead.debugObserver = debugObserver;
+ robotHead.fetchDatafields();
+
createRemoteGuiTab();
RemoteGui_startRunningTask();
}
@@ -92,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);
@@ -159,7 +120,45 @@ namespace armarx
{
ARMARX_VERBOSE << "Received object poses from '" << providerName << "'.";
- objpose::ObjectPoseSeq objectPoses;
+ std::optional<IceUtil::Time> discardUpdatesUntil;
+ bool discardAll = false;
+ {
+ std::scoped_lock lock(robotHeadMutex);
+ if (robotHead.checkHeadVelocity)
+ {
+ 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 (debugObserver)
+ {
+ StringVariantBaseMap map =
+ {
+ { "Discarding All Updates", new Variant(discardAll ? 1.f : 0.f) },
+ };
+ if (discardAll)
+ {
+ map["Proportion Updated Poses"] = new Variant(0.f);
+ }
+ debugObserver->setDebugChannel(getName(), map);
+ }
+ if (discardAll)
+ {
+ return;
+ }
{
std::scoped_lock lock(dataMutex);
RobotState::synchronizeLocalClone(robot);
@@ -171,28 +170,80 @@ namespace armarx
robotNode->setJointValue(value + calibration.offset);
}
+ // This stays empty on the first report.
+ objpose::ObjectPoseSeq previousPoses;
+ if (auto it = this->objectPoses.find(providerName); it != this->objectPoses.end())
+ {
+ previousPoses = it->second;
+ }
+
+ // Build new poses.
+ objpose::ObjectPoseSeq newObjectPoses;
+ int numUpdated = 0;
for (const objpose::data::ProvidedObjectPose& provided : providedPoses)
{
- objpose::ObjectPose& pose = objectPoses.emplace_back();
- pose.fromProvidedPose(provided, robot);
- if (pose.objectID.dataset().empty())
+ 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)
{
- // Try to find the data set. (It might be good to cache this.)
- if (std::optional<ObjectInfo> objectInfo = objectFinder.findObject(pose.objectID))
+ if (prev.objectID == fromIce(provided.objectID))
+ {
+ previousPose = prev;
+ }
+ }
+
+ if (discardUpdatesUntil && timestamp < *discardUpdatesUntil)
+ {
+ if (previousPose)
+ {
+ // Keep the old one
+ newObjectPoses.push_back(*previousPose);
+ }
+ else
{
- pose.objectID = { objectInfo->dataset(), pose.objectID.className(), pose.objectID.instanceName() };
+ // Discard the new pose.
+ // ARMARX_IMPORTANT << "Ignoring update of object " << provided.objectID << " because robot head is moved.\n"
+ // << "timestamp " << timestamp << " < " << robotHead.discardUpdatesUntil;
}
}
- if (!provided.localOOBB)
+ else if (previousPose && timestamp == previousPose->timestamp)
{
- pose.localOOBB = getObjectOOBB(pose.objectID);
+ // Keep the old one.
+ newObjectPoses.push_back(*previousPose);
+ }
+ else
+ {
+ numUpdated++;
+ objpose::ObjectPose& newPose = newObjectPoses.emplace_back();
+ newPose.fromProvidedPose(provided, robot);
+ if (newPose.objectID.dataset().empty())
+ {
+ // Try to find the data set. (It might be good to cache this.)
+ if (std::optional<ObjectInfo> objectInfo = objectFinder.findObject(newPose.objectID))
+ {
+ newPose.objectID = { objectInfo->dataset(), newPose.objectID.className(), newPose.objectID.instanceName() };
+ }
+ }
+ if (!provided.localOOBB)
+ {
+ // Try to load oobb from disk.
+ newPose.localOOBB = getObjectOOBB(newPose.objectID);
+ }
}
}
- }
- {
- std::scoped_lock lock(dataMutex);
- this->objectPoses[providerName] = objectPoses;
+ 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);
}
}
@@ -553,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)
@@ -760,5 +833,177 @@ namespace armarx
}
}
+ void ObjectPoseObserver::RobotHeadMovement::fetchDatafields()
+ {
+ if (kinematicUnitObserver)
+ {
+ for (const std::string& jointName : jointNames)
+ {
+ std::string error = "";
+ try
+ {
+ DatafieldRefBasePtr datafield = kinematicUnitObserver->getDatafieldRefByName(jointVelocitiesChannelName, jointName);
+ DatafieldRefPtr casted = DatafieldRefPtr::dynamicCast(datafield);
+ if (casted)
+ {
+ jointVelocitiesDatafields.push_back(casted);
+ }
+ }
+ catch (const InvalidDatafieldException& e)
+ {
+ error = e.what();
+ }
+ catch (const InvalidChannelException& e)
+ {
+ error = e.what();
+ }
+ if (error.size() > 0)
+ {
+ 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::RobotHeadMovement::isMoving() const
+ {
+ for (DatafieldRefPtr df : jointVelocitiesDatafields)
+ {
+ if (df)
+ {
+ float jointVelocity = df->getFloat();
+ // ARMARX_IMPORTANT << "Is " << df->datafieldName << " " << VAROUT(std::abs(jointVelocity)) << " > " << VAROUT(maxJointVelocity) << "?";
+ if (std::abs(jointVelocity) > maxJointVelocity)
+ {
+ return true;
+ }
+ }
+ }
+ 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 70a0e80abdb6983e58205c44d3a3c3ccc2ea3de3..5022afbf2c92b7cba7a7c2455dbe26d0e3c1f88f 100644
--- a/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h
+++ b/source/RobotAPI/components/ObjectPoseObserver/ObjectPoseObserver.h
@@ -26,11 +26,14 @@
#include <SimoxUtility/caching/CacheMap.h>
+#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/observers/Observer.h>
+#include <ArmarXCore/observers/variant/DatafieldRef.h>
#include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
#include <RobotAPI/interface/objectpose/ObjectPoseObserver.h>
+#include <RobotAPI/interface/observers/KinematicUnitObserverInterface.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotStateComponentPlugin.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
@@ -72,7 +75,6 @@ namespace armarx
, virtual public armarx::RobotStateComponentPluginUser
, virtual public armarx::LightweightRemoteGuiComponentPluginUser
, virtual public armarx::ArVizComponentPluginUser
-
{
public:
using RobotState = armarx::RobotStateComponentPluginUser;
@@ -114,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();
@@ -122,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:
@@ -151,12 +159,12 @@ 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;
@@ -171,6 +179,35 @@ namespace armarx
/// Caches results of attempts to retrieve the OOBB from ArmarXObjects.
simox::caching::CacheMap<ObjectID, std::optional<simox::OrientedBoxf>> oobbCache;
+
+ 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.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);
+ };
+ RobotHeadMovement robotHead;
+ std::mutex robotHeadMutex;
+
struct Visu
{
bool enabled = false;
@@ -192,12 +229,24 @@ namespace armarx
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/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.cpp b/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.cpp
index 60672d5da0b446160b6e0a075a52303a44d10c79..950edbf74b1bdc1516614b7dfc8ac5c9a9598845 100644
--- a/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.cpp
+++ b/source/RobotAPI/components/units/ObstacleAvoidingPlatformUnit/ObstacleAvoidingPlatformUnit.cpp
@@ -359,8 +359,8 @@ armarx::ObstacleAvoidingPlatformUnit::get_velocities()
ARMARX_CHECK(modulated_vel.allFinite()) << "Velocity contains non-finite values.";
ARMARX_CHECK(std::isfinite(target_rot_vel)) << "Rotation velocity is non-finite.";
- ARMARX_DEBUG << "Target velocity: " << target_vel.transpose() << "; " << target_rot_vel;
- ARMARX_DEBUG << "Modulated velocity: " << modulated_vel.transpose();
+ ARMARX_DEBUG << "Target velocity: " << target_vel.transpose() << "; norm: " << target_vel.norm() << "; " << target_rot_vel;
+ ARMARX_DEBUG << "Modulated velocity: " << modulated_vel.transpose() << modulated_vel.norm();
const auto r = Eigen::Rotation2D(m_control_data.agent_ori).toRotationMatrix().inverse();
@@ -802,14 +802,16 @@ armarx::ObstacleAvoidingPlatformUnit::createPropertyDefinitions()
// Settings.
def->optional(m_control_data.adaptive_max_vel_exp, "adaptive_max_vel_exponent",
- "Adaptive max vel exponent. 0 = disable.");
+ "Adaptive max vel exponent. This throttles the max_vel adaptively "
+ "depending on the angle between target velocity and modulated velocity. "
+ "0 = disable.");
// Control parameters.
def->optional(m_control_data.kp, "control.pos.kp");
def->optional(m_rot_pid_controller.Kp, "control.rot.kp");
def->optional(m_rot_pid_controller.Ki, "control.rot.ki");
def->optional(m_rot_pid_controller.Kd, "control.rot.kd");
- def->optional(m_control_loop.cycle_time, "control.pose.cycle_time");
+ def->optional(m_control_loop.cycle_time, "control.pose.cycle_time", "Control loop cycle time.");
// Obstacle avoidance parameter.
def->optional(m_control_data.agent_safety_margin, "doa.agent_safety_margin");
diff --git a/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidget.ui b/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidget.ui
index 85819912107bc3006fb94cec8a71428f57e0481f..fef69f4366e204f0a27fa3857e0221d7d7dba2bb 100644
--- a/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidget.ui
+++ b/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidget.ui
@@ -135,6 +135,13 @@
</property>
</spacer>
</item>
+ <item>
+ <widget class="QPushButton" name="pushButtonCopyCurrentPose">
+ <property name="text">
+ <string>copy current pose</string>
+ </property>
+ </widget>
+ </item>
<item>
<widget class="QLabel" name="labelParsingSuccess">
<property name="text">
@@ -524,6 +531,16 @@
</item>
</layout>
</item>
+ <item row="7" column="2" colspan="6">
+ <widget class="QCheckBox" name="checkBoxSkipWaypoints">
+ <property name="text">
+ <string>Skip waypoints</string>
+ </property>
+ <property name="checked">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
</layout>
</widget>
</item>
diff --git a/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidgetController.cpp b/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidgetController.cpp
index 7f245db4be10b20612849486bb388071c1504875..303aa4ecdeabc7030e74b432858cb9c063b7f5bc 100644
--- a/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidgetController.cpp
+++ b/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidgetController.cpp
@@ -23,6 +23,8 @@
#include <random>
#include <string>
+#include <QClipboard>
+
#include <SimoxUtility/json.h>
#include <ArmarXCore/util/CPPUtility/container.h>
@@ -38,13 +40,14 @@ namespace armarx
using T = CartesianWaypointControlGuiWidgetController;
std::lock_guard g{_allMutex};
_ui.setupUi(getWidget());
- connect(_ui.radioButtonWPJson, &QPushButton::clicked, this, &T::triggerParsing);
- connect(_ui.radioButton4f, &QPushButton::clicked, this, &T::triggerParsing);
- connect(_ui.textEditWPs, &QTextEdit::textChanged, this, &T::triggerParsing);
+ connect(_ui.radioButtonWPJson, &QPushButton::clicked, this, &T::triggerParsing);
+ connect(_ui.radioButton4f, &QPushButton::clicked, this, &T::triggerParsing);
+ connect(_ui.textEditWPs, &QTextEdit::textChanged, this, &T::triggerParsing);
- connect(_ui.pushButtonExecute, &QPushButton::clicked, this, &T::on_pushButtonExecute_clicked);
- connect(_ui.pushButtonZeroFT, &QPushButton::clicked, this, &T::on_pushButtonZeroFT_clicked);
- connect(_ui.pushButtonSendSettings, &QPushButton::clicked, this, &T::on_pushButtonSendSettings_clicked);
+ connect(_ui.pushButtonExecute, &QPushButton::clicked, this, &T::on_pushButtonExecute_clicked);
+ connect(_ui.pushButtonZeroFT, &QPushButton::clicked, this, &T::on_pushButtonZeroFT_clicked);
+ connect(_ui.pushButtonSendSettings, &QPushButton::clicked, this, &T::on_pushButtonSendSettings_clicked);
+ connect(_ui.pushButtonCopyCurrentPose, &QPushButton::clicked, this, &T::copyCurrentPose);
connectCreateAcivateDeactivateDelete(
_ui.pushButtonCreateController,
@@ -88,6 +91,28 @@ namespace armarx
}
}
+ void CartesianWaypointControlGuiWidgetController::copyCurrentPose()
+ {
+ std::lock_guard g{_allMutex};
+ if (!_robot)
+ {
+ return;
+ }
+ synchronizeLocalClone(_robot);
+ const auto rns = _robot->getRobotNodeSet(_ui.comboBoxChain->currentText().toStdString());
+ if (!rns)
+ {
+ return;
+ }
+ const auto tcp = rns->getTCP();
+ if (!tcp)
+ {
+ return;
+ }
+ const auto str = simox::json::eigen4f2posquatJson(tcp->getPoseInRootFrame());
+ QApplication::clipboard()->setText(QString::fromStdString(str));
+ }
+
void CartesianWaypointControlGuiWidgetController::on_pushButtonZeroFT_clicked()
{
std::lock_guard g{_allMutex};
@@ -148,26 +173,27 @@ namespace armarx
std::lock_guard g{_allMutex};
NJointCartesianWaypointControllerRuntimeConfig cfg;
- cfg.wpCfg.maxPositionAcceleration = static_cast<float>(_ui.doubleSpinBoxMaxAccPos->value());
- cfg.wpCfg.maxOrientationAcceleration = static_cast<float>(_ui.doubleSpinBoxMaxAccOri->value());
- cfg.wpCfg.maxNullspaceAcceleration = static_cast<float>(_ui.doubleSpinBoxMaxAccNull->value());
+ cfg.wpCfg.maxPositionAcceleration = static_cast<float>(_ui.doubleSpinBoxMaxAccPos->value());
+ cfg.wpCfg.maxOrientationAcceleration = static_cast<float>(_ui.doubleSpinBoxMaxAccOri->value());
+ cfg.wpCfg.maxNullspaceAcceleration = static_cast<float>(_ui.doubleSpinBoxMaxAccNull->value());
- cfg.wpCfg.kpJointLimitAvoidance = static_cast<float>(_ui.doubleSpinBoxLimitAvoidKP->value());
- cfg.wpCfg.jointLimitAvoidanceScale = static_cast<float>(_ui.doubleSpinBoxLimitAvoidScale->value());
+ cfg.wpCfg.kpJointLimitAvoidance = static_cast<float>(_ui.doubleSpinBoxLimitAvoidKP->value());
+ cfg.wpCfg.jointLimitAvoidanceScale = static_cast<float>(_ui.doubleSpinBoxLimitAvoidScale->value());
- cfg.wpCfg.thresholdOrientationNear = static_cast<float>(_ui.doubleSpinBoxOriNear->value());
- cfg.wpCfg.thresholdOrientationReached = static_cast<float>(_ui.doubleSpinBoxOriReached->value());
- cfg.wpCfg.thresholdPositionNear = static_cast<float>(_ui.doubleSpinBoxPosNear->value());
- cfg.wpCfg.thresholdPositionReached = static_cast<float>(_ui.doubleSpinBoxPosReached->value());
+ cfg.wpCfg.thresholdOrientationNear = static_cast<float>(_ui.doubleSpinBoxOriNear->value());
+ cfg.wpCfg.thresholdOrientationReached = static_cast<float>(_ui.doubleSpinBoxOriReached->value());
+ cfg.wpCfg.thresholdPositionNear = static_cast<float>(_ui.doubleSpinBoxPosNear->value());
+ cfg.wpCfg.thresholdPositionReached = static_cast<float>(_ui.doubleSpinBoxPosReached->value());
- cfg.wpCfg.maxOriVel = static_cast<float>(_ui.doubleSpinBoxOriMaxVel->value());
- cfg.wpCfg.maxPosVel = static_cast<float>(_ui.doubleSpinBoxPosMaxVel->value());
- cfg.wpCfg.kpOri = static_cast<float>(_ui.doubleSpinBoxOriKP->value());
- cfg.wpCfg.kpPos = static_cast<float>(_ui.doubleSpinBoxPosKP->value());
+ cfg.wpCfg.maxOriVel = static_cast<float>(_ui.doubleSpinBoxOriMaxVel->value());
+ cfg.wpCfg.maxPosVel = static_cast<float>(_ui.doubleSpinBoxPosMaxVel->value());
+ cfg.wpCfg.kpOri = static_cast<float>(_ui.doubleSpinBoxOriKP->value());
+ cfg.wpCfg.kpPos = static_cast<float>(_ui.doubleSpinBoxPosKP->value());
- cfg.forceThreshold = static_cast<float>(_ui.doubleSpinBoxFTLimit->value());
+ cfg.forceThreshold = static_cast<float>(_ui.doubleSpinBoxFTLimit->value());
cfg.optimizeNullspaceIfTargetWasReached = _ui.checkBoxKeepOptimizing->isChecked();
cfg.forceThresholdInRobotRootZ = _ui.checkBoxLimitinZ->isChecked();
+ cfg.skipToClosestWaypoint = _ui.checkBoxSkipWaypoints->isChecked();
return cfg;
}
diff --git a/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidgetController.h b/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidgetController.h
index f5358b49ea1dfbb097abd2d9432b403ee823f46c..01c59683c8dd3081e50ac58aa180d9c1a1d7ced0 100644
--- a/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidgetController.h
+++ b/source/RobotAPI/gui-plugins/CartesianWaypointControlGui/CartesianWaypointControlGuiWidgetController.h
@@ -76,6 +76,7 @@ namespace armarx
void on_pushButtonZeroFT_clicked();
void on_pushButtonSendSettings_clicked();
void createController() override;
+ void copyCurrentPose();
void triggerParsing();
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/ObstacleAvoidingPlatformUnitHelper.cpp b/source/RobotAPI/libraries/RobotStatechartHelpers/ObstacleAvoidingPlatformUnitHelper.cpp
index 27525c42d19079f8fbd3db620f03ad1710f09897..03ed255120ccf8a5084ea824f7d8e2d790c1c485 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/ObstacleAvoidingPlatformUnitHelper.cpp
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/ObstacleAvoidingPlatformUnitHelper.cpp
@@ -89,3 +89,10 @@ const
return target_dist < m_pos_reached_threshold and
std::fabs(target_angular_dist) < m_ori_reached_threshold;
}
+
+
+void
+armarx::ObstacleAvoidingPlatformUnitHelper::setMaxVelocities(float max_vel, float max_angular_vel)
+{
+ m_platform_unit->setMaxVelocities(max_vel, max_angular_vel);
+}
diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/ObstacleAvoidingPlatformUnitHelper.h b/source/RobotAPI/libraries/RobotStatechartHelpers/ObstacleAvoidingPlatformUnitHelper.h
index 96bd461e7f6194de38283839eaaff0d5ec135967..12d7100f997eeb818f6cf0aff989e42a92059df5 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/ObstacleAvoidingPlatformUnitHelper.h
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/ObstacleAvoidingPlatformUnitHelper.h
@@ -61,6 +61,9 @@ namespace armarx
isFinalTargetReached()
const;
+ void
+ setMaxVelocities(float max_vel, float max_angular_vel);
+
private:
struct target
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);