diff --git a/source/armar6/skills/components/PickAndPlaceComponent/PickAndPlaceComponent.cpp b/source/armar6/skills/components/PickAndPlaceComponent/PickAndPlaceComponent.cpp
index 68d341064ae235e8c8f076ec755b7efa8c235878..c4bf3e452d76308b4a9a250a33409cc38d5d730c 100644
--- a/source/armar6/skills/components/PickAndPlaceComponent/PickAndPlaceComponent.cpp
+++ b/source/armar6/skills/components/PickAndPlaceComponent/PickAndPlaceComponent.cpp
@@ -21,42 +21,38 @@
*/
#include "PickAndPlaceComponent.h"
-#include "PickAndPlaceController.h"
-#include <armar6/skills/libraries/Armar6Tools/Armar6HandV2Helper.h>
-#include <armarx/manipulation/util/BimanualTransformationHelper.h>
+#include <SimoxUtility/math/pose/pose.h>
+#include <SimoxUtility/math/pose/transform.h>
+#include <VirtualRobot/math/Helpers.h>
+#include <VirtualRobot/math/Line.h>
+#include <VirtualRobot/math/LinearInterpolatedPose.h>
+#include <ArmarXCore/core/system/ArmarXDataPath.h>
+#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
+#include <ArmarXCore/core/time/Metronome.h>
+#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
+#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
+#include <RobotAPI/libraries/ArmarXObjects/aron_conversions/objpose.h>
+#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
+#include <RobotAPI/libraries/RobotStatechartHelpers/ForceTorqueHelper.h>
+#include <RobotAPI/libraries/RobotStatechartHelpers/KinematicUnitHelper.h>
+#include <RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.h>
+#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
+#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
+#include <RobotAPI/libraries/aron/common/aron_conversions/core.h>
+#include <armarx/manipulation/core/Affordance.h>
#include <armarx/manipulation/core/ExecutableAction.h>
#include <armarx/manipulation/core/ExecutedAction.h>
-#include <armarx/manipulation/core/Affordance.h>
#include <armarx/manipulation/core/FramedUncertainPose.h>
-#include <armarx/manipulation/core/aron_conversions.h>
#include <armarx/manipulation/core/aron/ExecutableAction.aron.generated.h>
#include <armarx/manipulation/core/aron/ExecutedAction.aron.generated.h>
+#include <armarx/manipulation/core/aron_conversions.h>
+#include <armarx/manipulation/util/BimanualTransformationHelper.h>
-#include <RobotAPI/libraries/RobotStatechartHelpers/ForceTorqueHelper.h>
-#include <RobotAPI/libraries/RobotStatechartHelpers/KinematicUnitHelper.h>
-#include <RobotAPI/libraries/RobotStatechartHelpers/PositionControllerHelper.h>
-#include <RobotAPI/libraries/ArmarXObjects/ObjectFinder.h>
-#include <RobotAPI/libraries/ArmarXObjects/ice_conversions.h>
-#include <RobotAPI/libraries/ArmarXObjects/aron_conversions/objpose.h>
-#include <RobotAPI/libraries/aron/common/aron_conversions/core.h>
-#include <RobotAPI/libraries/armem_objects/aron_conversions.h>
-#include <RobotAPI/libraries/armem_objects/aron/ObjectInstance.aron.generated.h>
-
-#include <ArmarXCore/core/time/Metronome.h>
-#include <ArmarXCore/core/system/ArmarXDataPath.h>
-#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
-#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
-
-
-#include <VirtualRobot/math/Helpers.h>
-#include <VirtualRobot/math/Line.h>
-#include <VirtualRobot/math/LinearInterpolatedPose.h>
-
-#include <SimoxUtility/math/pose/pose.h>
-#include <SimoxUtility/math/pose/transform.h>
+#include <armar6/skills/libraries/Armar6Tools/Armar6HandV2Helper.h>
+#include "PickAndPlaceController.h"
namespace armarx
{
@@ -77,18 +73,22 @@ namespace armarx
return location;
}
-
PropertyDefinitionsPtr
PickAndPlaceComponent::createPropertyDefinitions()
{
ARMARX_TRACE;
- armarx::PropertyDefinitionsPtr properties(new armarx::ComponentPropertyDefinitions(getConfigIdentifier()));
+ armarx::PropertyDefinitionsPtr properties(
+ new armarx::ComponentPropertyDefinitions(getConfigIdentifier()));
- properties->defineOptionalProperty<std::string>("DebugObserverName", "DebugObserver",
- "Name of the topic the DebugObserver listens to.");
- properties->defineOptionalProperty<std::string>("ConfigPath", "armar6_skills/pick-and-place/PickAndPlaceComponent.json",
- "Path to the config file");
+ properties->defineOptionalProperty<std::string>(
+ "DebugObserverName",
+ "DebugObserver",
+ "Name of the topic the DebugObserver listens to.");
+ properties->defineOptionalProperty<std::string>(
+ "ConfigPath",
+ "armar6_skills/pick-and-place/PickAndPlaceComponent.json",
+ "Path to the config file");
//more modern variant to use components
@@ -100,26 +100,22 @@ namespace armarx
return properties;
}
-
- PickAndPlaceComponent::PickAndPlaceComponent(): graspWriter(memoryNameSystem()), graspReader(memoryNameSystem())
+ PickAndPlaceComponent::PickAndPlaceComponent()
{
}
-
std::string
PickAndPlaceComponent::getDefaultName() const
{
return GetDefaultName();
}
-
std::string
PickAndPlaceComponent::GetDefaultName()
{
return "PickAndPlaceComponent";
}
-
void
PickAndPlaceComponent::onInitComponent()
{
@@ -129,8 +125,10 @@ namespace armarx
std::string packageName = "armar6_skills";
armarx::CMakePackageFinder finder(packageName);
std::string dataDir = finder.getDataDir() + "/" + packageName;
- data.controllerInfo.graspTrajectories["RightTopOpen"] = Armar6GraspTrajectory::ReadFromFile(dataDir + "/motions/grasps/RightTopOpen.xml");
- data.controllerInfo.graspTrajectories["RightSideOpen"] = Armar6GraspTrajectory::ReadFromFile(dataDir + "/motions/grasps/RightSideOpen.xml");
+ data.controllerInfo.graspTrajectories["RightTopOpen"] =
+ Armar6GraspTrajectory::ReadFromFile(dataDir + "/motions/grasps/RightTopOpen.xml");
+ data.controllerInfo.graspTrajectories["RightSideOpen"] =
+ Armar6GraspTrajectory::ReadFromFile(dataDir + "/motions/grasps/RightSideOpen.xml");
}
offeringTopicFromProperty("DebugObserverName");
@@ -157,13 +155,13 @@ namespace armarx
<< ", for right: " << config.Prepose.Right.Waypoints.size();
}
-
- void PickAndPlaceComponent::onConnectComponent()
+ void
+ PickAndPlaceComponent::onConnectComponent()
{
ARMARX_TRACE;
- graspWriter.connect();
- graspReader.connect();
+ graspWriter.connect(memoryNameSystem());
+ graspReader.connect(memoryNameSystem());
getTopicFromProperty(debugObserver, "DebugObserverName");
@@ -186,10 +184,12 @@ namespace armarx
data.robot = getRobot("gui-robot");
}
data.robot->setPropagatingJointValuesEnabled(!config.General.IsSimulation);
- robotNameHelper = RobotNameHelper::Create(getRobotStateComponent()->getRobotInfo(), nullptr);
+ robotNameHelper =
+ RobotNameHelper::Create(getRobotStateComponent()->getRobotInfo(), nullptr);
- gui.simoxFileCache = simox::caching::CacheMap<armarx::ObjectID, PackageFileLocation>(getLocation);
+ gui.simoxFileCache =
+ simox::caching::CacheMap<armarx::ObjectID, PackageFileLocation>(getLocation);
saveObjectPoses();
@@ -197,10 +197,7 @@ namespace armarx
createRemoteGuiTab();
RemoteGui_startRunningTask();
- controlTask = new SimpleRunningTask<>([this]()
- {
- this->runControlLoop();
- });
+ controlTask = new SimpleRunningTask<>([this]() { this->runControlLoop(); });
data.boxGraspCandidatePlanner = std::make_unique<BoxGraspCandidatePlanner>();
data.boxGraspCandidatePlanner->setArviz(arviz);
@@ -223,8 +220,8 @@ namespace armarx
controlTask->start();
}
-
- void PickAndPlaceComponent::onDisconnectComponent()
+ void
+ PickAndPlaceComponent::onDisconnectComponent()
{
ARMARX_TRACE;
@@ -233,14 +230,14 @@ namespace armarx
controlTask = nullptr;
}
-
- void PickAndPlaceComponent::onExitComponent()
+ void
+ PickAndPlaceComponent::onExitComponent()
{
ARMARX_TRACE;
}
-
- void PickAndPlaceComponent::createRemoteGuiTab()
+ void
+ PickAndPlaceComponent::createRemoteGuiTab()
{
ARMARX_TRACE;
@@ -264,7 +261,7 @@ namespace armarx
if (0 <= gui.objectIndex and gui.objectIndex < static_cast<int>(options.size()))
{
- if (! data.objectPosesDynamic.empty())
+ if (!data.objectPosesDynamic.empty())
{
auto* objectPose = data.objectPosesDynamic.at(gui.objectIndex);
gui.graspCandidatesObject = *objectPose;
@@ -288,12 +285,13 @@ namespace armarx
std::unique_lock lock(data.controlMutex);
ARMARX_INFO << "Querying for entity " << gui.graspCandidatesObject.objectID.str();
candidates = graspReader.queryLatestGraspCandidateEntity(
- getName(), gui.graspCandidatesObject.objectID.str());
+ getName(), gui.graspCandidatesObject.objectID.str());
}
- catch (armem::error::QueryFailed &e)
+ catch (armem::error::QueryFailed& e)
{
- ARMARX_WARNING << "No Entries for entity " << gui.graspCandidatesObject.objectID.str()
- << " in Memory " << graspMemoryName << ".";
+ ARMARX_WARNING << "No Entries for entity "
+ << gui.graspCandidatesObject.objectID.str() << " in Memory "
+ << graspMemoryName << ".";
}
gui.graspCandidates.clear();
std::vector<std::string> options;
@@ -305,8 +303,8 @@ namespace armarx
bool isReachable = grasp->reachabilityInfo->reachable;
std::stringstream key;
- key << gui.graspCandidatesObject.objectID.str() << " " << i
- << " (" << (isReachable ? std::string("reachable") : std::string("unreachable")) << ")";
+ key << gui.graspCandidatesObject.objectID.str() << " " << i << " ("
+ << (isReachable ? std::string("reachable") : std::string("unreachable")) << ")";
gui.graspCandidates[key.str()] = id;
++i;
@@ -344,10 +342,10 @@ namespace armarx
std::vector<std::string> options;
options.reserve(static_cast<int>(Action::End) - static_cast<int>(Action::Begin));
- for (Action action = Action::Begin; action != Action::End; action = static_cast<Action>(static_cast<int>(action) + 1))
+ for (Action action = Action::Begin; action != Action::End;
+ action = static_cast<Action>(static_cast<int>(action) + 1))
{
options.push_back(action_names.to_name(action));
-
}
gui.tab.selectAction.setOptions(options);
gui.tab.selectAction.setIndex(static_cast<int>(gui.action));
@@ -357,7 +355,8 @@ namespace armarx
row += 1;
}
{
- gui.tab.selectSide.setOptions({side_names.to_name(Side::Left), side_names.to_name(Side::Right)});
+ gui.tab.selectSide.setOptions(
+ {side_names.to_name(Side::Left), side_names.to_name(Side::Right)});
gui.tab.selectSide.setIndex(static_cast<int>(gui.side));
root.add(Label("Side:"), Pos{row, 0});
@@ -404,8 +403,8 @@ namespace armarx
RemoteGui_createTab(getName(), root, &gui.tab);
}
-
- void PickAndPlaceComponent::RemoteGui_update()
+ void
+ PickAndPlaceComponent::RemoteGui_update()
{
ARMARX_TRACE;
@@ -431,8 +430,9 @@ namespace armarx
{
ARMARX_INFO << "Selected new object index: " << newObjectIndex;
std::unique_lock lock(data.objectPoseMutex);
- if (0 <= newObjectIndex and newObjectIndex < static_cast<int>(data.objectPosesDynamic.size())
- and not data.objectPosesDynamic.empty())
+ if (0 <= newObjectIndex and
+ newObjectIndex < static_cast<int>(data.objectPosesDynamic.size()) and
+ not data.objectPosesDynamic.empty())
{
auto* objectPose = data.objectPosesDynamic.at(newObjectIndex);
ARMARX_INFO << "Selected new object: " << objectPose->objectID.str();
@@ -473,7 +473,7 @@ namespace armarx
{
std::unique_lock lock(data.controlMutex);
data.autoUpdateGraspsAllObjects = gui.tab.autoUpdateGraspsAllObjects.getValue();
- if(data.autoUpdateGraspsAllObjects)
+ if (data.autoUpdateGraspsAllObjects)
{
for (auto* objectPose : data.objectPosesDynamic)
{
@@ -497,7 +497,6 @@ namespace armarx
saveObjectPoses();
data.objectsUpdated = false;
}
-
}
if (gui.tab.calculateGraspCandidates.wasClicked())
@@ -507,10 +506,12 @@ namespace armarx
data.controllerInfo.action = gui.action;
data.controllerInfo.side = gui.side;
- if (0 <= gui.objectIndex and gui.objectIndex < static_cast<int>(data.objectPosesDynamic.size()))
+ if (0 <= gui.objectIndex and
+ gui.objectIndex < static_cast<int>(data.objectPosesDynamic.size()))
{
data.controllerInfo.objectPose = *data.objectPosesDynamic.at(gui.objectIndex);
- getGraspCandidatesInPluginFormat(data, data.controllerInfo.side, data.controllerInfo.objectPose);
+ getGraspCandidatesInPluginFormat(
+ data, data.controllerInfo.side, data.controllerInfo.objectPose);
gui.recreateTabNextLoop = true;
}
else
@@ -526,7 +527,8 @@ namespace armarx
std::unique_lock lock_poses(data.objectPoseMutex);
data.controllerInfo.action = gui.action;
data.controllerInfo.side = gui.side;
- if (0 <= gui.objectIndex and gui.objectIndex < static_cast<int>(data.objectPosesDynamic.size()))
+ if (0 <= gui.objectIndex and
+ gui.objectIndex < static_cast<int>(data.objectPosesDynamic.size()))
{
data.controllerInfo.objectPose = *data.objectPosesDynamic.at(gui.objectIndex);
}
@@ -534,14 +536,15 @@ namespace armarx
{
try
{
- data.controllerInfo.grasp = graspReader.queryGraspCandidateInstanceByID(armem::MemoryID::fromString(gui.graspID));
+ data.controllerInfo.grasp = graspReader.queryGraspCandidateInstanceByID(
+ armem::MemoryID::fromString(gui.graspID));
}
- catch (armem::error::QueryFailed &e)
+ catch (armem::error::QueryFailed& e)
{
- ARMARX_WARNING << "No grasp candidate with ID " << gui.graspID << " in memory " << graspMemoryName << ".";
+ ARMARX_WARNING << "No grasp candidate with ID " << gui.graspID << " in memory "
+ << graspMemoryName << ".";
data.controllerInfo.grasp = nullptr;
}
-
}
else
{
@@ -564,10 +567,12 @@ namespace armarx
std::unique_lock lock_poses(data.objectPoseMutex);
data.controllerInfo.action = gui.action;
data.controllerInfo.side = gui.side;
- if (0 <= gui.objectIndex and gui.objectIndex < static_cast<int>(data.objectPosesDynamic.size()))
+ if (0 <= gui.objectIndex and
+ gui.objectIndex < static_cast<int>(data.objectPosesDynamic.size()))
{
data.controllerInfo.objectPose = *data.objectPosesDynamic.at(gui.objectIndex);
- getGraspCandidatesInPluginFormat(data, data.controllerInfo.side, data.controllerInfo.objectPose);
+ getGraspCandidatesInPluginFormat(
+ data, data.controllerInfo.side, data.controllerInfo.objectPose);
}
}
{
@@ -593,19 +598,22 @@ namespace armarx
}
}
-
- grasping::CalculateGraspCandidatesOutput PickAndPlaceComponent::calculateGraspCandidates(
- const grasping::CalculateGraspCandidatesInput& input, const Ice::Current&)
+ grasping::CalculateGraspCandidatesOutput
+ PickAndPlaceComponent::calculateGraspCandidates(
+ const grasping::CalculateGraspCandidatesInput& input,
+ const Ice::Current&)
{
ARMARX_TRACE;
grasping::CalculateGraspCandidatesOutput output;
- const std::set<std::string> sides { side_names.to_name(Side::Left), side_names.to_name(Side::Right) };
+ const std::set<std::string> sides{side_names.to_name(Side::Left),
+ side_names.to_name(Side::Right)};
if (sides.count(input.side) == 0)
{
ARMARX_ERROR << "Invalid value for side: '" << input.side << "'. "
- << "Expected one of:\n" << std::vector<std::string>(sides.begin(), sides.end());
+ << "Expected one of:\n"
+ << std::vector<std::string>(sides.begin(), sides.end());
return output;
}
@@ -617,11 +625,13 @@ namespace armarx
try
{
- output.candidates = graspReader.queryLatestGraspCandidateEntity(getName(), pose.objectID.str());
+ output.candidates =
+ graspReader.queryLatestGraspCandidateEntity(getName(), pose.objectID.str());
}
- catch (armem::error::QueryFailed &e)
+ catch (armem::error::QueryFailed& e)
{
- ARMARX_WARNING << "No Entries for entity " << pose.objectID.str() << " in Memory " << graspMemoryName << ".";
+ ARMARX_WARNING << "No Entries for entity " << pose.objectID.str() << " in Memory "
+ << graspMemoryName << ".";
return output;
}
}
@@ -629,10 +639,9 @@ namespace armarx
return output;
}
-
grasping::ExecutePreposeOutput
- PickAndPlaceComponent::executePrepose(
- const grasping::ExecutePreposeInput& input, const Ice::Current&)
+ PickAndPlaceComponent::executePrepose(const grasping::ExecutePreposeInput& input,
+ const Ice::Current&)
{
ARMARX_TRACE;
@@ -645,11 +654,13 @@ namespace armarx
control.action = Action::Prepose;
- const std::set<std::string> sides { side_names.to_name(Side::Left), side_names.to_name(Side::Right) };
+ const std::set<std::string> sides{side_names.to_name(Side::Left),
+ side_names.to_name(Side::Right)};
if (sides.count(input.side) == 0)
{
ARMARX_ERROR << "Invalid value for side: '" << input.side << "'. "
- << "Expected one of:\n" << std::vector<std::string>(sides.begin(), sides.end());
+ << "Expected one of:\n"
+ << std::vector<std::string>(sides.begin(), sides.end());
return output;
}
control.side = side_names.from_name(input.side);
@@ -666,10 +677,8 @@ namespace armarx
return output;
}
-
grasping::ExecutePushOutput
- PickAndPlaceComponent::executePush(
- const grasping::ExecutePushInput& input, const Ice::Current&)
+ PickAndPlaceComponent::executePush(const grasping::ExecutePushInput& input, const Ice::Current&)
{
ARMARX_TRACE;
@@ -682,11 +691,13 @@ namespace armarx
control.action = Action::Push;
- const std::set<std::string> sides { side_names.to_name(Side::Left), side_names.to_name(Side::Right) };
+ const std::set<std::string> sides{side_names.to_name(Side::Left),
+ side_names.to_name(Side::Right)};
if (sides.count(input.side) == 0)
{
ARMARX_ERROR << "Invalid value for side: '" << input.side << "'. "
- << "Expected one of:\n" << std::vector<std::string>(sides.begin(), sides.end());
+ << "Expected one of:\n"
+ << std::vector<std::string>(sides.begin(), sides.end());
return output;
}
control.side = side_names.from_name(input.side);
@@ -702,9 +713,8 @@ namespace armarx
return output;
}
-
- grasping::ExecutePickOutput PickAndPlaceComponent::executePick(
- const grasping::ExecutePickInput& input, const Ice::Current&)
+ grasping::ExecutePickOutput
+ PickAndPlaceComponent::executePick(const grasping::ExecutePickInput& input, const Ice::Current&)
{
ARMARX_TRACE;
@@ -718,15 +728,17 @@ namespace armarx
control.action = Action::Pick;
try
{
- control.grasp = graspReader.queryGraspCandidateInstanceByID(armem::MemoryID::fromString(input.candidateID));
+ control.grasp = graspReader.queryGraspCandidateInstanceByID(
+ armem::MemoryID::fromString(input.candidateID));
}
- catch (armem::error::QueryFailed &e)
+ catch (armem::error::QueryFailed& e)
{
- ARMARX_WARNING << "No grasp candidate with ID " << input.candidateID << " in memory " << graspMemoryName << ".";
+ ARMARX_WARNING << "No grasp candidate with ID " << input.candidateID
+ << " in memory " << graspMemoryName << ".";
return output;
}
- if (input.liftVector) // Else leave default.
+ if (input.liftVector) // Else leave default.
{
control.liftVector = Vector3Ptr::dynamicCast(input.liftVector)->toEigen();
}
@@ -748,10 +760,9 @@ namespace armarx
return output;
}
-
grasping::ExecutePlaceOutput
- PickAndPlaceComponent::executePlace(
- const grasping::ExecutePlaceInput& input, const Ice::Current&)
+ PickAndPlaceComponent::executePlace(const grasping::ExecutePlaceInput& input,
+ const Ice::Current&)
{
ARMARX_TRACE;
@@ -764,18 +775,21 @@ namespace armarx
control.action = Action::Place;
- const std::set<std::string> sides { side_names.to_name(Side::Left), side_names.to_name(Side::Right) };
+ const std::set<std::string> sides{side_names.to_name(Side::Left),
+ side_names.to_name(Side::Right)};
if (sides.count(input.side) == 0)
{
ARMARX_ERROR << "Invalid value for side: '" << input.side << "'. "
- << "Expected one of:\n" << std::vector<std::string>(sides.begin(), sides.end());
+ << "Expected one of:\n"
+ << std::vector<std::string>(sides.begin(), sides.end());
return output;
}
control.side = side_names.from_name(input.side);
- if (input.placePositionGlobal) // Else leave default.
+ if (input.placePositionGlobal) // Else leave default.
{
- control.placePositionGlobal = Vector3Ptr::dynamicCast(input.placePositionGlobal)->toEigen();
+ control.placePositionGlobal =
+ Vector3Ptr::dynamicCast(input.placePositionGlobal)->toEigen();
}
ARMARX_INFO << "Executing place action";
@@ -789,10 +803,9 @@ namespace armarx
return output;
}
-
grasping::ExecuteRetreatOutput
- PickAndPlaceComponent::executeRetreat(
- const grasping::ExecuteRetreatInput& input, const Ice::Current&)
+ PickAndPlaceComponent::executeRetreat(const grasping::ExecuteRetreatInput& input,
+ const Ice::Current&)
{
ARMARX_TRACE;
@@ -805,11 +818,13 @@ namespace armarx
control.action = Action::Retreat;
- const std::set<std::string> sides { side_names.to_name(Side::Left), side_names.to_name(Side::Right) };
+ const std::set<std::string> sides{side_names.to_name(Side::Left),
+ side_names.to_name(Side::Right)};
if (sides.count(input.side) == 0)
{
ARMARX_ERROR << "Invalid value for side: '" << input.side << "'. "
- << "Expected one of:\n" << std::vector<std::string>(sides.begin(), sides.end());
+ << "Expected one of:\n"
+ << std::vector<std::string>(sides.begin(), sides.end());
return output;
}
control.side = side_names.from_name(input.side);
@@ -825,10 +840,9 @@ namespace armarx
return output;
}
-
grasping::ExecutePreposeReverseOutput
- PickAndPlaceComponent::executePreposeReverse(
- const grasping::ExecutePreposeReverseInput& input, const Ice::Current&)
+ PickAndPlaceComponent::executePreposeReverse(const grasping::ExecutePreposeReverseInput& input,
+ const Ice::Current&)
{
ARMARX_TRACE;
@@ -841,11 +855,13 @@ namespace armarx
control.action = Action::PreposeReverse;
- const std::set<std::string> sides { side_names.to_name(Side::Left), side_names.to_name(Side::Right) };
+ const std::set<std::string> sides{side_names.to_name(Side::Left),
+ side_names.to_name(Side::Right)};
if (sides.count(input.side) == 0)
{
ARMARX_ERROR << "Invalid value for side: '" << input.side << "'. "
- << "Expected one of:\n" << std::vector<std::string>(sides.begin(), sides.end());
+ << "Expected one of:\n"
+ << std::vector<std::string>(sides.begin(), sides.end());
return output;
}
control.side = side_names.from_name(input.side);
@@ -861,10 +877,9 @@ namespace armarx
return output;
}
-
grasping::LocalizeObjectInHandOutput
- PickAndPlaceComponent::localizeObjectInHand(
- const grasping::LocalizeObjectInHandInput& input, const Ice::Current&)
+ PickAndPlaceComponent::localizeObjectInHand(const grasping::LocalizeObjectInHandInput& input,
+ const Ice::Current&)
{
ARMARX_TRACE;
@@ -953,9 +968,9 @@ namespace armarx
for (const objpose::ObjectPose& p : acceptedObjectPoses)
{
layer.add(viz::Object(std::to_string(i))
- .pose(p.objectPoseGlobal)
- .fileByObjectFinder(objectID)
- .overrideColor(simox::Color::blue(255, 128)));
+ .pose(p.objectPoseGlobal)
+ .fileByObjectFinder(objectID)
+ .overrideColor(simox::Color::blue(255, 128)));
++i;
}
arviz.commit(layer);
@@ -972,7 +987,9 @@ namespace armarx
return output;
}
- aron::data::dto::DictPtr PickAndPlaceComponent::executeAction(const aron::data::dto::DictPtr &executableAction, const Ice::Current&)
+ aron::data::dto::DictPtr
+ PickAndPlaceComponent::executeAction(const aron::data::dto::DictPtr& executableAction,
+ const Ice::Current&)
{
ARMARX_TRACE;
armarx::manipulation::core::arondto::ExecutableAction dto;
@@ -981,7 +998,8 @@ namespace armarx
manipulation::core::ExecutableAction action = armarx::manipulation::core::fromAron(dto);
armarx::manipulation::core::arondto::ExecutedAction exDto;
- std::string actionType = action.unimanual.at(0).hypothesis->affordance.get()->getAffordanceID();
+ std::string actionType =
+ action.unimanual.at(0).hypothesis->affordance.get()->getAffordanceID();
if (actionType == "Prepose")
{
@@ -999,7 +1017,7 @@ namespace armarx
// executes prepose and pick currently
// fill grasp candidate with necessary info and forward to controller
// cannot call ice method executePick here because that one uses armem MemoryID
- grasping::GraspCandidatePtr candidate = new grasping::GraspCandidate();
+ grasping::GraspCandidatePtr candidate = new grasping::GraspCandidate();
auto hypothesis = action.unimanual.at(0).hypothesis;
@@ -1016,14 +1034,13 @@ namespace armarx
{
std::unique_lock lock(data.objectPoseMutex);
- for (objpose::ObjectPose & object : data.objectPoses)
+ for (objpose::ObjectPose& object : data.objectPoses)
{
std::string name = object.objectID.className();
- if(name.find("apple") != name.npos )
+ if (name.find("apple") != name.npos)
{
objectPose = object;
break;
-
}
}
}
@@ -1087,7 +1104,8 @@ namespace armarx
//executes place, retreat and preposeReverse currently
grasping::ExecutePlaceInput input;
input.side = action.unimanual.at(0).handedness->toString();
- input.placePositionGlobal = action.unimanual.at(0).hypothesis->framedPose.get()->pose->position;
+ input.placePositionGlobal =
+ action.unimanual.at(0).hypothesis->framedPose.get()->pose->position;
manipulation::core::ExecutedAction exAction(action, armem::Time::Now());
bool executed = executePlace(input).executed;
@@ -1135,17 +1153,17 @@ namespace armarx
return exDto.toAronDTO();
}
- void PickAndPlaceComponent::recover(const Ice::Current &)
+ void
+ PickAndPlaceComponent::recover(const Ice::Current&)
{
-
}
- void PickAndPlaceComponent::abort(const Ice::Current &)
+ void
+ PickAndPlaceComponent::abort(const Ice::Current&)
{
controller->stopExecution();
}
-
void
PickAndPlaceComponent::saveObjectPoses()
{
@@ -1164,7 +1182,6 @@ namespace armarx
gui.recreateTabNextLoop = true;
}
-
void
PickAndPlaceComponent::visualizeGraspCandidates()
{
@@ -1179,12 +1196,13 @@ namespace armarx
{
std::unique_lock lock(data.controlMutex);
ARMARX_INFO << "Querying for entity " << gui.graspCandidatesObject.objectID.str();
- candidates = graspReader.queryLatestGraspCandidateEntity(getName(), gui.graspCandidatesObject.objectID.str());
+ candidates = graspReader.queryLatestGraspCandidateEntity(
+ getName(), gui.graspCandidatesObject.objectID.str());
}
- catch (armem::error::QueryFailed &e)
+ catch (armem::error::QueryFailed& e)
{
ARMARX_WARNING << "No Entries for entity " << gui.graspCandidatesObject.objectID.str()
- << " in Memory " << graspMemoryName << ".";
+ << " in Memory " << graspMemoryName << ".";
return;
}
@@ -1205,11 +1223,13 @@ namespace armarx
Eigen::Matrix4f tcp2handRoot = fromIce(candidate->tcpPoseInHandRoot).inverse();
Eigen::Matrix4f graspPose = PosePtr::dynamicCast(candidate->graspPose)->toEigen();
- std::string modelFile = "armar6_rt/robotmodel/Armar6-SH/Armar6-" + candidate->side + "Hand-v3.xml";
- viz::Robot hand = viz::Robot("Grasp_" + armem::MemoryID::fromString(id).getEntityInstanceID().str())
- .file("armar6_rt", modelFile)
- .pose(fromIce(candidate->robotPose) * graspPose * tcp2handRoot)
- .overrideColor(color);
+ std::string modelFile =
+ "armar6_rt/robotmodel/Armar6-SH/Armar6-" + candidate->side + "Hand-v3.xml";
+ viz::Robot hand =
+ viz::Robot("Grasp_" + armem::MemoryID::fromString(id).getEntityInstanceID().str())
+ .file("armar6_rt", modelFile)
+ .pose(fromIce(candidate->robotPose) * graspPose * tcp2handRoot)
+ .overrideColor(color);
if (isReachable)
{
@@ -1223,7 +1243,6 @@ namespace armarx
arviz.commit({graspsLayer, graspsNonReachableLayer});
}
-
void
PickAndPlaceComponent::visualizePlacePosition()
{
@@ -1245,7 +1264,8 @@ namespace armarx
Eigen::Matrix4f startPose = tcp->getPoseInRootFrame();
// Make an intermediate pose which only moves in x,y not z
- Eigen::Vector3f placePositionRobot = (robotPoseGlobal.inverse() * placePosition.homogeneous()).head<3>();
+ Eigen::Vector3f placePositionRobot =
+ (robotPoseGlobal.inverse() * placePosition.homogeneous()).head<3>();
Eigen::Matrix4f intermediatePose = startPose;
intermediatePose(0, 3) = placePositionRobot(0);
intermediatePose(1, 3) = placePositionRobot(1);
@@ -1258,24 +1278,20 @@ namespace armarx
placeWaypoints.push_back(intermediatePose);
placeWaypoints.push_back(placePose);
- SimpleDiffIK::Reachability reachability = SimpleDiffIK::CalculateReachability(placeWaypoints,
- Eigen::VectorXf::Zero(rns->getSize()),
- rns, tcp);
+ SimpleDiffIK::Reachability reachability = SimpleDiffIK::CalculateReachability(
+ placeWaypoints, Eigen::VectorXf::Zero(rns->getSize()), rns, tcp);
// TODO: Make color red if not reachable!
viz::Color color = reachability.reachable ? viz::Color::green() : viz::Color::red();
viz::Layer placeLayer = arviz.layer("Place");
- viz::Sphere vizPlace = viz::Sphere("PlacePosition")
- .position(placePosition)
- .color(color)
- .radius(20.0f);
+ viz::Sphere vizPlace =
+ viz::Sphere("PlacePosition").position(placePosition).color(color).radius(20.0f);
placeLayer.add(vizPlace);
arviz.commit(placeLayer);
}
-
void
PickAndPlaceComponent::visualizeLocalizePosition()
{
@@ -1283,13 +1299,15 @@ namespace armarx
std::unique_lock lock(data.controlMutex);
synchronizeLocalClone(data.robot);
- RobotNameHelper::RobotArm arm = robotNameHelper->getRobotArm(side_names.to_name(gui.side), data.robot);
+ RobotNameHelper::RobotArm arm =
+ robotNameHelper->getRobotArm(side_names.to_name(gui.side), data.robot);
VirtualRobot::RobotNodePtr tcp = arm.getTCP();
Eigen::Matrix4f startPose = tcp->getPoseInRootFrame();
- (void) startPose;
+ (void)startPose;
- const LocalizeSingleConfig& localizeConfig = (gui.side == Side::Left) ? config.Localize.Left : config.Localize.Right;
+ const LocalizeSingleConfig& localizeConfig =
+ (gui.side == Side::Left) ? config.Localize.Left : config.Localize.Right;
FramedPose localizeTcpPoseFramed = localizeConfig.TcpPose;
Eigen::Matrix4f localizeTcpPoseGlobal = localizeTcpPoseFramed.toGlobalEigen(data.robot);
@@ -1300,7 +1318,6 @@ namespace armarx
arviz.commit(localizeLayer);
}
-
void
PickAndPlaceComponent::visualizeObjectPoses()
{
@@ -1316,7 +1333,8 @@ namespace armarx
{
Eigen::Matrix4f pose = object.objectPoseGlobal;
- if (object.attachment && !object.attachment->agentName.empty() && !object.attachment->frameName.empty())
+ if (object.attachment && !object.attachment->agentName.empty() &&
+ !object.attachment->frameName.empty())
{
auto attachedToNode = data.robot->getRobotNode(object.attachment->frameName);
@@ -1325,17 +1343,19 @@ namespace armarx
PackageFileLocation location = gui.simoxFileCache.get(object.objectID);
viz::Object viz = viz::Object(object.objectID.className())
- .file(location.package, location.relativePath)
- .pose(pose);
+ .file(location.package, location.relativePath)
+ .pose(pose);
objectLayer.add(viz);
}
- std::map<std::string, float> jointValues = data.robot->getConfig()->getRobotNodeJointValueMap();
- viz::Robot robot_viz = viz::Robot("Armar6")
- .file("armar6_rt", "armar6_rt/robotmodel/Armar6-SH/Armar6-SH.xml")
- .pose(data.robot->getGlobalPose())
- .joints(jointValues);
+ std::map<std::string, float> jointValues =
+ data.robot->getConfig()->getRobotNodeJointValueMap();
+ viz::Robot robot_viz =
+ viz::Robot("Armar6")
+ .file("armar6_rt", "armar6_rt/robotmodel/Armar6-SH/Armar6-SH.xml")
+ .pose(data.robot->getGlobalPose())
+ .joints(jointValues);
robotLayer.add(robot_viz);
}
@@ -1343,39 +1363,42 @@ namespace armarx
arviz.commit({objectLayer, robotLayer});
}
-
void
PickAndPlaceComponent::runControlLoop()
{
ARMARX_TRACE;
//synchronizeLocalClone(robot);
- memoryNameSystem().subscribe(armem::MemoryID("Object", "Instance"),
- [&](const std::vector<armem::MemoryID>& snapshotIDs)
- {
- std::unique_lock lock(data.controlMutex);
- if (data.autoUpdateGraspsAllObjects)
+ memoryNameSystem().subscribe(
+ armem::MemoryID("Object", "Instance"),
+ [&](const std::vector<armem::MemoryID>& snapshotIDs)
{
- data.objectsUpdated = true;
-
- const armem::client::QueryResult result = objectReader.queryMemoryIDs(snapshotIDs);
- if (result.success)
+ std::unique_lock lock(data.controlMutex);
+ if (data.autoUpdateGraspsAllObjects)
{
- Side side = data.controllerInfo.side;
+ data.objectsUpdated = true;
- result.memory.forEachInstance([this, &side](const armem::wm::EntityInstance& instance)
+ const armem::client::QueryResult result =
+ objectReader.queryMemoryIDs(snapshotIDs);
+ if (result.success)
{
- const objpose::ObjectPose objectPose =
- aron::fromAron<objpose::ObjectPose>(
- armem::arondto::ObjectInstance::FromAron(instance.data()).pose);
- if (not objectPose.isStatic)
- {
- getGraspCandidatesInPluginFormat(data, side, objectPose);
- }
- });
- }
- }
- });
+ Side side = data.controllerInfo.side;
+
+ result.memory.forEachInstance(
+ [this, &side](const armem::wm::EntityInstance& instance)
+ {
+ const objpose::ObjectPose objectPose =
+ aron::fromAron<objpose::ObjectPose>(
+ armem::arondto::ObjectInstance::FromAron(instance.data())
+ .pose);
+ if (not objectPose.isStatic)
+ {
+ getGraspCandidatesInPluginFormat(data, side, objectPose);
+ }
+ });
+ }
+ }
+ });
Metronome metronome(Duration::MilliSeconds(20));
@@ -1406,8 +1429,12 @@ namespace armarx
case Action::Pick:
{
PickAndPlaceController::ExecutePickOutput output =
- controller->executePick(control.side, control.grasp, control.liftVector,
- control.graspTrajectories, data.objectPoses, data.objectPoseMutex);
+ controller->executePick(control.side,
+ control.grasp,
+ control.liftVector,
+ control.graspTrajectories,
+ data.objectPoses,
+ data.objectPoseMutex);
{
std::unique_lock lock(data.controlMutex);
data.controllerOutput = {};
@@ -1417,16 +1444,25 @@ namespace armarx
break;
case Action::Place:
{
- controller->executePlace(control.side, control.placePositionGlobal, data.objectPoses,
+ controller->executePlace(control.side,
+ control.placePositionGlobal,
+ data.objectPoses,
data.objectPoseMutex);
}
break;
case Action::LocalizeObjectInHand:
{
- objpose::ObjectPoseSeq acceptedObjectPoses = controller->executeLocalizeInHand(
- control.side, control.objectID, data.objectPoses, data.objectPoseMutex,
- control.initialWaitMS, control.maxDistanceFromTcp, control.minPoseUpdates,
- control.moveHead, control.pollFrequency, control.timeoutMS);
+ objpose::ObjectPoseSeq acceptedObjectPoses =
+ controller->executeLocalizeInHand(control.side,
+ control.objectID,
+ data.objectPoses,
+ data.objectPoseMutex,
+ control.initialWaitMS,
+ control.maxDistanceFromTcp,
+ control.minPoseUpdates,
+ control.moveHead,
+ control.pollFrequency,
+ control.timeoutMS);
{
std::unique_lock lock(data.controlMutex);
data.controllerInfo.acceptedObjectPoses = acceptedObjectPoses;
@@ -1455,29 +1491,35 @@ namespace armarx
}
}
-
void
- PickAndPlaceComponent::getGraspCandidatesInPluginFormat(
- DataControl& data, const Side& side, const objpose::ObjectPose& objectPose)
+ PickAndPlaceComponent::getGraspCandidatesInPluginFormat(DataControl& data,
+ const Side& side,
+ const objpose::ObjectPose& objectPose)
{
//data.controlMutex needs to be locked before invoking this method
ARMARX_TRACE;
ARMARX_CHECK_GREATER(data.controllerInfo.graspTrajectories.size(), 0)
- << "Expected grasp trajectories to not be empty.";
+ << "Expected grasp trajectories to not be empty.";
synchronizeLocalClone(data.robot);
- grasping::GraspCandidateSeq candidates = data.boxGraspCandidatePlanner->calculateGraspCandidates(
- side, objectPose, data.robot, robotNameHelper, data.controllerInfo.graspTrajectories);
+ grasping::GraspCandidateSeq candidates =
+ data.boxGraspCandidatePlanner->calculateGraspCandidates(
+ side,
+ objectPose,
+ data.robot,
+ robotNameHelper,
+ data.controllerInfo.graspTrajectories);
graspWriter.commitGraspCandidateSeq(candidates, armem::Time::Now(), getName());
-
}
- ARMARX_REGISTER_COMPONENT_EXECUTABLE(PickAndPlaceComponent, PickAndPlaceComponent::GetDefaultName());
+ ARMARX_REGISTER_COMPONENT_EXECUTABLE(PickAndPlaceComponent,
+ PickAndPlaceComponent::GetDefaultName());
- void DataControl::setObjectPoses(const objpose::ObjectPoseSeq& _objectPoses)
+ void
+ DataControl::setObjectPoses(const objpose::ObjectPoseSeq& _objectPoses)
{
this->objectPoses = _objectPoses;
@@ -1491,4 +1533,4 @@ namespace armarx
}
}
-}
+} // namespace armarx
diff --git a/source/armar6/skills/components/skill_providers/hand_over/Component.cpp b/source/armar6/skills/components/skill_providers/hand_over/Component.cpp
index fb16d0f6d7ba417bc491dc4796670193f2f8924b..e761fb497f5d41e48b576cbefbb298ec9baea0be 100644
--- a/source/armar6/skills/components/skill_providers/hand_over/Component.cpp
+++ b/source/armar6/skills/components/skill_providers/hand_over/Component.cpp
@@ -125,8 +125,7 @@ namespace armar6::skills::components::skill_providers::hand_over
services.openPoseListener = remote.openPoseListener;
services.handUnits = remote.handUnits;
- ho::HandOver* skill = addSkill<ho::HandOver>();
- skill->connect(services);
+ addSkillFactory<ho::HandOver>(services);
}
}
@@ -152,7 +151,6 @@ namespace armar6::skills::components::skill_providers::hand_over
return Component::defaultName;
}
-
ARMARX_REGISTER_COMPONENT_EXECUTABLE(Component, Component::GetDefaultName());
} // namespace armar6::skills::components::skill_providers::hand_over
diff --git a/source/armar6/skills/components/skill_providers/introduction/Component.cpp b/source/armar6/skills/components/skill_providers/introduction/Component.cpp
index 9b0e9ef536e3a0bfae653761bf81475b8dd2776a..ab7777b1aaf500b304c1488f38d4ad40d52ac92b 100644
--- a/source/armar6/skills/components/skill_providers/introduction/Component.cpp
+++ b/source/armar6/skills/components/skill_providers/introduction/Component.cpp
@@ -98,20 +98,17 @@ namespace armar6::skills::components::skill_providers::introduction
skillRemote.speechObserver = remote.speechObserver;
skillRemote.textToSpeech = remote.textToSpeech;
- skills::IntroduceYourself* skill = addSkill<skills::IntroduceYourself>();
- skill->connectTo(skillRemote);
+ addSkillFactory<skills::IntroduceYourself>(skillRemote);
}
{
skills::NavigationPose::Remote skillRemote;
skillRemote.kinematicUnit = remote.kinematicUnit;
- skills::NavigationPose* skill = addSkill<skills::NavigationPose>();
- skill->connectTo(skillRemote);
+ addSkillFactory<skills::NavigationPose>(skillRemote);
}
{
- skills::IntroduceYourselfAtLocation* skill [[maybe_unused]] =
- addSkill<skills::IntroduceYourselfAtLocation>();
+ addSkillFactory<skills::IntroduceYourselfAtLocation>();
// skill->connectTo(...);
}
}
diff --git a/source/armar6/skills/hand_over/skills/HandOver.cpp b/source/armar6/skills/hand_over/skills/HandOver.cpp
index f68dd6ac978800e896cbd36d37115bd58c66eb40..9f48d8bc69a457aa264a961491d1eed3af217384 100644
--- a/source/armar6/skills/hand_over/skills/HandOver.cpp
+++ b/source/armar6/skills/hand_over/skills/HandOver.cpp
@@ -52,14 +52,13 @@ namespace armar6::skills::hand_over
default_params.handName = "Hand_R_EEF";
return armarx::skills::SkillDescription{
- .skillName = "HandOverObjectToHuman",
+ .skillId = {.skillName = "HandOverObjectToHuman"},
.description = "Move hand to human and open the hand when a FT spike is detected. "
"This skill does remove an attachment of an object to the hand from the "
"object memory if present.",
- .robots = {},
+ .rootProfileDefaults = default_params.toAron(),
.timeout = armarx::core::time::Duration::Minutes(5),
- .acceptedType = ParamType::ToAronType(),
- .defaultParams = default_params.toAron()};
+ .parametersType = ParamType::ToAronType()};
}
bool
@@ -222,14 +221,27 @@ namespace armar6::skills::hand_over
return std::make_pair(timestamp, latestData);
}
- HandOver::HandOver() : SkillBase(GetSkillDescription())
+ HandOver::HandOver(const Services& services) : SkillBase(GetSkillDescription())
{
+ this->services = services;
+ ARMARX_CHECK(services.robotStateComponent);
+ ARMARX_CHECK(services.robotUnit);
+ ARMARX_CHECK(services.forceTorqueUnit);
+ ARMARX_CHECK(services.headIKUnit);
+ ARMARX_CHECK(services.openPoseEstimation);
+ ARMARX_CHECK(services.objectPoseStorage);
+ ARMARX_CHECK(services.openPoseListener);
+ for (const auto& [k, v] : services.handUnits)
+ {
+ ARMARX_CHECK(v);
+ }
+ ARMARX_CHECK_EQUAL(services.handUnits.size(), 2);
}
HandOver::SkillBase::MainResult
HandOver::main(const SpecializedMainInput& in)
{
- auto& params = in.params;
+ auto& params = in.parameters;
//////////////////////////////////////////////////////////////////////////////////////////////////////
// ReachOutToHumanPose
@@ -420,7 +432,7 @@ namespace armar6::skills::hand_over
FramedPositionPtr lastIKTarget;
bool forceThresholdReached = false;
- while (!this->checkWhetherSkillShouldStopASAP()) // stop run function if returning true
+ while (!this->shouldSkillTerminate()) // stop run function if returning true
{
viz::Layer vizLayer = arviz.layer("HandOverState");
@@ -829,7 +841,7 @@ namespace armar6::skills::hand_over
// uncomment this if you need a continous run function. Make sure to use sleep or use blocking wait to reduce cpu load.
bool targetReached = false;
- while (!checkWhetherSkillShouldStopASAP()) // stop run function if returning true
+ while (!shouldSkillTerminate()) // stop run function if returning true
{
RemoteRobot::synchronizeLocalClone(robot, getRobotStateComponent());
auto targetPose =
@@ -866,22 +878,4 @@ namespace armar6::skills::hand_over
return {.status = armarx::skills::TerminatedSkillStatus::Succeeded};
}
- void
- HandOver::connect(const Services& services)
- {
- this->services = services;
- ARMARX_CHECK(services.robotStateComponent);
- ARMARX_CHECK(services.robotUnit);
- ARMARX_CHECK(services.forceTorqueUnit);
- ARMARX_CHECK(services.headIKUnit);
- ARMARX_CHECK(services.openPoseEstimation);
- ARMARX_CHECK(services.objectPoseStorage);
- ARMARX_CHECK(services.openPoseListener);
- for (const auto& [k, v] : services.handUnits)
- {
- ARMARX_CHECK(v);
- }
- ARMARX_CHECK_EQUAL(services.handUnits.size(), 2);
- }
-
} // namespace armar6::skills::hand_over
diff --git a/source/armar6/skills/hand_over/skills/HandOver.h b/source/armar6/skills/hand_over/skills/HandOver.h
index d2e92708b9372c0fc466cceb7482f68175dfb09e..7b460bcba55c58766e774542330e18c85f747c45 100644
--- a/source/armar6/skills/hand_over/skills/HandOver.h
+++ b/source/armar6/skills/hand_over/skills/HandOver.h
@@ -24,6 +24,7 @@
// Base Class
#include <RobotAPI/libraries/skills/provider/PeriodicSpecializedSkill.h>
+#include <RobotAPI/libraries/skills/provider/SimpleSpecializedSkill.h>
// Remote
#include <RobotAPI/components/ArViz/Client/Client.h>
@@ -60,10 +61,10 @@ namespace armar6::skills::hand_over
IceUtil::Time timestamp;
};
- class HandOver : public armarx::skills::SpecializedSkill<arondto::HandOverParams>
+ class HandOver : public armarx::skills::SimpleSpecializedSkill<arondto::HandOverParams>
{
public:
- using SkillBase = armarx::skills::SpecializedSkill<arondto::HandOverParams>;
+ using SkillBase = armarx::skills::SimpleSpecializedSkill<arondto::HandOverParams>;
// SkillDescription creator
static armarx::skills::SkillDescription GetSkillDescription();
@@ -88,13 +89,11 @@ namespace armar6::skills::hand_over
};
public:
- HandOver();
+ HandOver(const Services& services);
/// Do fancy stuff to hand over an object to the human
SkillBase::MainResult main(const SpecializedMainInput& in) override;
- void connect(const Services& services);
-
// Local private parameters
private:
Services services;
diff --git a/source/armar6/skills/introduction/skills/IntroduceYourself.cpp b/source/armar6/skills/introduction/skills/IntroduceYourself.cpp
index 824ccaacab34e64047c731ffe2d24e42c2667bfd..4b280dede38776ea8309fc29f3c32b8f9d3b7b25 100644
--- a/source/armar6/skills/introduction/skills/IntroduceYourself.cpp
+++ b/source/armar6/skills/introduction/skills/IntroduceYourself.cpp
@@ -16,11 +16,9 @@
#include <ArmarXCore/core/time/TimeUtil.h>
#include <ArmarXCore/observers/variant/StringValueMap.h>
#include <ArmarXCore/observers/variant/Variant.h>
-
#include <RobotAPI/libraries/SimpleTrajectory/Trajectory.h>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
-
namespace armar6::skills::introduction::skills
{
class InterruptedException : public std::exception
@@ -29,7 +27,6 @@ namespace armar6::skills::introduction::skills
~InterruptedException() override = default;
};
-
::armarx::skills::SkillDescription
IntroduceYourself::GetSkillDescription()
{
@@ -366,20 +363,19 @@ namespace armar6::skills::introduction::skills
};
return ::armarx::skills::SkillDescription{
- .skillName = "IntroduceYourself",
+ .skillId = {.skillName = "IntroduceYourself"},
.description = "TODO: Description of skill IntroduceYourself.",
- .robots = {},
+ .rootProfileDefaults = defaults.toAron(),
.timeout = ::armarx::Duration::Minutes(5),
- .acceptedType = ParamType::ToAronType(),
- .defaultParams = defaults.toAron()};
+ .parametersType = ParamType::ToAronType()};
}
-
- IntroduceYourself::IntroduceYourself() :
- ::armarx::skills::SpecializedSkill<ParamType>(GetSkillDescription())
+ IntroduceYourself::IntroduceYourself(const Remote& remote) :
+ ::armarx::skills::SimpleSpecializedSkill<ParamType>(GetSkillDescription())
{
- }
+ this->remote = remote;
+ }
/*
bool
@@ -392,7 +388,7 @@ namespace armar6::skills::introduction::skills
::armarx::skills::Skill::InitResult
IntroduceYourself::init(const SpecializedInitInput& in)
{
- auto displayText = in.params.DisplayText;
+ auto displayText = in.parameters.DisplayText;
if (displayText.size() == 2)
{
getMessageDisplay()->setMessage(displayText[0], displayText[1]);
@@ -405,7 +401,6 @@ namespace armar6::skills::introduction::skills
return InitResult{.status = ::armarx::skills::TerminatedSkillStatus::Succeeded};
}
-
::armarx::skills::Skill::MainResult
IntroduceYourself::main(const SpecializedMainInput& in)
{
@@ -413,7 +408,7 @@ namespace armar6::skills::introduction::skills
::armarx::skills::TerminatedSkillStatus::Succeeded;
try
{
- performPresentation(in.params);
+ performPresentation(in.parameters);
}
catch (const InterruptedException&)
{
@@ -422,7 +417,7 @@ namespace armar6::skills::introduction::skills
}
ARMARX_IMPORTANT << "Switching to position control and stopping platform.";
- switchControlMode(armarx::ePositionControl, in.params);
+ switchControlMode(armarx::ePositionControl, in.parameters);
getPlatformUnit()->move(0, 0, 0);
getRobotUnit()->switchNJointControllerSetup({});
@@ -440,7 +435,6 @@ namespace armar6::skills::introduction::skills
return ExitResult{.status = ::armarx::skills::TerminatedSkillStatus::Succeeded};
}
-
void
IntroduceYourself::switchControlMode(armarx::ControlMode mode, const ParamType& params)
{
@@ -458,7 +452,6 @@ namespace armar6::skills::introduction::skills
getKinematicUnit()->switchControlMode(modeMap);
}
-
void
IntroduceYourself::setJointAngles(const NameValueMap& values,
VirtualRobot::RobotPtr localRobot,
@@ -485,7 +478,6 @@ namespace armar6::skills::introduction::skills
}
}
-
void
IntroduceYourself::setJointAnglesSimulation(const NameValueMap& values,
VirtualRobot::RobotPtr _localRobot,
@@ -576,7 +568,6 @@ namespace armar6::skills::introduction::skills
getTextToSpeech()->reportText(text);
}
-
void
IntroduceYourself::waitForSpeechFinished()
{
@@ -663,7 +654,6 @@ namespace armar6::skills::introduction::skills
}
}
-
void
IntroduceYourself::performPresentation(const ParamType& params)
{
diff --git a/source/armar6/skills/introduction/skills/IntroduceYourself.h b/source/armar6/skills/introduction/skills/IntroduceYourself.h
index fd38135fa19b2906c0d6a6d577b2452c50cae81d..ff40b50bf79a6c1d380431703daf8c338c846c9e 100644
--- a/source/armar6/skills/introduction/skills/IntroduceYourself.h
+++ b/source/armar6/skills/introduction/skills/IntroduceYourself.h
@@ -2,21 +2,18 @@
#include <VirtualRobot/VirtualRobot.h>
-#include <ArmarXCore/interface/observers/ObserverInterface.h>
#include <ArmarXCore/core/services/tasks/TaskUtil.h>
-
+#include <ArmarXCore/interface/observers/ObserverInterface.h>
#include <ArmarXGui/interface/MessageDisplayInterface.h>
-
#include <RobotAPI/interface/core/RobotState.h>
#include <RobotAPI/interface/speech/SpeechInterface.h>
#include <RobotAPI/interface/units/KinematicUnitInterface.h>
#include <RobotAPI/interface/units/PlatformUnitInterface.h>
#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
-#include <RobotAPI/libraries/skills/provider/SpecializedSkill.h>
+#include <RobotAPI/libraries/skills/provider/SimpleSpecializedSkill.h>
#include <armar6/skills/introduction/skills/aron/IntroduceYourselfParams.aron.generated.h>
-
namespace armar6::skills::introduction::skills
{
@@ -27,7 +24,8 @@ namespace armar6::skills::introduction::skills
*
* Detailed description of class introduction.
*/
- class IntroduceYourself : public ::armarx::skills::SpecializedSkill<IntroduceYourselfParams>
+ class IntroduceYourself :
+ public ::armarx::skills::SimpleSpecializedSkill<IntroduceYourselfParams>
{
public:
static ::armarx::skills::SkillDescription GetSkillDescription();
@@ -45,15 +43,10 @@ namespace armar6::skills::introduction::skills
};
public:
- IntroduceYourself();
-
- void connectTo(const Remote& remote)
- {
- this->remote = remote;
- }
+ IntroduceYourself(const Remote& remote);
private:
- // SpecializedSkill interface
+ // SimpleSpecializedSkill interface
// Enable each function you want to override.
// bool isAvailable(const SpecializedInitInput&) const override;
::armarx::skills::Skill::InitResult init(const SpecializedInitInput&) override;
@@ -62,80 +55,85 @@ namespace armar6::skills::introduction::skills
private:
-
using NameValueMap = std::map<std::string, float>;
- void switchControlMode(
- armarx::ControlMode mode,
- const ParamType& params);
- void setJointAngles(
- const NameValueMap& values,
- VirtualRobot::RobotPtr localRobot,
- const ParamType& params,
- const std::string& name = "");
- void setJointAnglesSimulation(
- const NameValueMap& values,
- VirtualRobot::RobotPtr localRobot,
- const ParamType& params);
+ void switchControlMode(armarx::ControlMode mode, const ParamType& params);
+ void setJointAngles(const NameValueMap& values,
+ VirtualRobot::RobotPtr localRobot,
+ const ParamType& params,
+ const std::string& name = "");
+ void setJointAnglesSimulation(const NameValueMap& values,
+ VirtualRobot::RobotPtr localRobot,
+ const ParamType& params);
void speak(const std::string& text);
void waitForSpeechFinished();
- void waitForMoveFinish(
- const NameValueMap& values,
- VirtualRobot::RobotPtr localRobot,
- const ParamType& params,
- const std::string& name = "");
-
- void performPresentation(
- const ParamType& params);
-
+ void waitForMoveFinish(const NameValueMap& values,
+ VirtualRobot::RobotPtr localRobot,
+ const ParamType& params,
+ const std::string& name = "");
+ void performPresentation(const ParamType& params);
- const armarx::KinematicUnitInterfacePrx& getKinematicUnit() const
+ const armarx::KinematicUnitInterfacePrx&
+ getKinematicUnit() const
{
return remote.kinematicUnit;
}
- const armarx::RobotStateComponentInterfacePrx& getRobotStateComponent() const
+
+ const armarx::RobotStateComponentInterfacePrx&
+ getRobotStateComponent() const
{
return remote.robotStateComponent;
}
- const armarx::TextListenerInterfacePrx& getTextToSpeech() const
+
+ const armarx::TextListenerInterfacePrx&
+ getTextToSpeech() const
{
return remote.textToSpeech;
}
- const armarx::ObserverInterfacePrx& getSpeechObserver() const
+
+ const armarx::ObserverInterfacePrx&
+ getSpeechObserver() const
{
return remote.speechObserver;
}
- const armarx::DebugObserverInterfacePrx& getDebugObserver() const
+
+ const armarx::DebugObserverInterfacePrx&
+ getDebugObserver() const
{
return remote.debugObserver;
}
- const armarx::PlatformUnitInterfacePrx& getPlatformUnit() const
+
+ const armarx::PlatformUnitInterfacePrx&
+ getPlatformUnit() const
{
return remote.platformUnit;
}
- const armarx::RobotUnitInterfacePrx& getRobotUnit() const
+
+ const armarx::RobotUnitInterfacePrx&
+ getRobotUnit() const
{
return remote.robotUnit;
}
- const armarx::MessageDisplayInterfacePrx& getMessageDisplay() const
+
+ const armarx::MessageDisplayInterfacePrx&
+ getMessageDisplay() const
{
return remote.messageDisplay;
}
- bool isRunningTaskStopped() const
+
+ bool
+ isRunningTaskStopped() const
{
- return this->checkWhetherSkillShouldStopASAP();
+ return this->shouldSkillTerminate();
}
private:
-
-
Remote remote;
armarx::SimpleRunningTask<>::pointer_type setJointAnglesSimulationTask = nullptr;
-
};
-}
+} // namespace armar6::skills::introduction::skills
diff --git a/source/armar6/skills/introduction/skills/IntroduceYourselfAtLocation.cpp b/source/armar6/skills/introduction/skills/IntroduceYourselfAtLocation.cpp
index 0ce33282dfbed746fa9b535a57719b6a82f09933..544be20a0a9dbee49cc5e942d904eb9adba835d1 100644
--- a/source/armar6/skills/introduction/skills/IntroduceYourselfAtLocation.cpp
+++ b/source/armar6/skills/introduction/skills/IntroduceYourselfAtLocation.cpp
@@ -1,10 +1,10 @@
#include "IntroduceYourselfAtLocation.h"
-#include <RobotAPI/libraries/skills/provider/SkillProxy.h>
+#include <RobotAPI/libraries/skills/core/SkillProxy.h>
+#include <armarx/navigation/skills/constants.h>
#include "IntroduceYourself.h"
#include "NavigationPose.h"
-#include <armarx/navigation/skills/constants.h>
namespace armar6::skills::introduction::skills
{
@@ -14,29 +14,26 @@ namespace armar6::skills::introduction::skills
{
ParamType defaultParams;
IntroduceYourselfParams defaultIYP;
- defaultIYP.fromAron(IntroduceYourself::GetSkillDescription().defaultParams);
+ defaultIYP.fromAron(IntroduceYourself::GetSkillDescription().rootProfileDefaults);
NavigationPoseParams defaultNP;
- defaultNP.fromAron(NavigationPose::GetSkillDescription().defaultParams);
+ defaultNP.fromAron(NavigationPose::GetSkillDescription().rootProfileDefaults);
defaultParams.introduceYourselfParams = defaultIYP;
defaultParams.navigateToLocationParams.location = "R003/sh_center";
defaultParams.navigationPose = defaultNP;
return ::armarx::skills::SkillDescription{
- .skillName = "IntroduceYourselfAtLocation",
+ .skillId = {.skillName = "IntroduceYourselfAtLocation"},
.description = "TODO: Description of skill IntroduceYourselfAtLocation.",
- .robots = {},
+ .rootProfileDefaults = defaultParams.toAron(),
.timeout = ::armarx::Duration::Minutes(10),
- .acceptedType = ParamType::ToAronType(),
- .defaultParams = defaultParams.toAron()};
+ .parametersType = ParamType::ToAronType()};
}
-
IntroduceYourselfAtLocation::IntroduceYourselfAtLocation() :
- ::armarx::skills::SpecializedSkill<ParamType>(GetSkillDescription())
+ ::armarx::skills::SimpleSpecializedSkill<ParamType>(GetSkillDescription())
{
}
-
/*
bool
IntroduceYourselfAtLocation::isAvailable(const SpecializedInitInput&) const
@@ -59,10 +56,12 @@ namespace armar6::skills::introduction::skills
{
// Enter main code of the skill here.
const std::string IYSkillProviderName = "introduction_skill_provider";
+ const armarx::skills::ProviderID pid = {.providerName = "introduction_skill_provider"};
// Navigation Pose
- armarx::skills::SkillProxy navigationPose(manager, {IYSkillProviderName, "NavigationPose"});
- if (const auto result = navigationPose.executeFullSkill(getSkillId().toString(),
- in.params.navigationPose.toAron());
+ armarx::skills::SkillProxy navigationPose(manager,
+ armarx::skills::SkillID{pid, "NavigationPose"});
+ if (const auto result = navigationPose.executeSkill(getSkillId().toString(),
+ in.parameters.navigationPose.toAron());
result.status != armarx::skills::TerminatedSkillStatus::Succeeded)
{
return MainResult{
@@ -73,11 +72,14 @@ namespace armar6::skills::introduction::skills
// Navigate to landmark
namespace nav_const = armarx::navigation::skills::constants;
+
+ const armarx::skills::ProviderID nav_pid = {.providerName =
+ nav_const::NavigationSkillProviderName};
+
armarx::skills::SkillProxy navigateToLocation(
- manager,
- {nav_const::NavigationSkillProviderName, nav_const::skill_names::NavigateToLocation});
- if (const auto result = navigateToLocation.executeFullSkill(
- getSkillId().toString(), in.params.navigateToLocationParams.toAron());
+ manager, armarx::skills::SkillID{nav_pid, nav_const::skill_names::NavigateToLocation});
+ if (const auto result = navigateToLocation.executeSkill(
+ getSkillId().toString(), in.parameters.navigateToLocationParams.toAron());
result.status != armarx::skills::TerminatedSkillStatus::Succeeded)
{
return MainResult{
@@ -87,10 +89,10 @@ namespace armar6::skills::introduction::skills
}
// Actual presentation
- armarx::skills::SkillProxy introduceYourself(manager,
- {IYSkillProviderName, "IntroduceYourself"});
- if (const auto result = introduceYourself.executeFullSkill(
- getSkillId().toString(), in.params.introduceYourselfParams.toAron());
+ armarx::skills::SkillProxy introduceYourself(
+ manager, armarx::skills::SkillID{pid, "IntroduceYourself"});
+ if (const auto result = introduceYourself.executeSkill(
+ getSkillId().toString(), in.parameters.introduceYourselfParams.toAron());
result.status != armarx::skills::TerminatedSkillStatus::Succeeded)
{
return MainResult{
@@ -100,8 +102,8 @@ namespace armar6::skills::introduction::skills
}
// Navigation pose
- if (const auto result = navigationPose.executeFullSkill(getSkillId().toString(),
- in.params.navigationPose.toAron());
+ if (const auto result = navigationPose.executeSkill(getSkillId().toString(),
+ in.parameters.navigationPose.toAron());
result.status != armarx::skills::TerminatedSkillStatus::Succeeded)
{
return MainResult{
diff --git a/source/armar6/skills/introduction/skills/IntroduceYourselfAtLocation.h b/source/armar6/skills/introduction/skills/IntroduceYourselfAtLocation.h
index 1726b8854eda9d28df6fff50fcb33dc83efaf295..07d27f97ea19ee7d66ed96af39014982f6cd8105 100644
--- a/source/armar6/skills/introduction/skills/IntroduceYourselfAtLocation.h
+++ b/source/armar6/skills/introduction/skills/IntroduceYourselfAtLocation.h
@@ -1,6 +1,6 @@
#pragma once
-#include <RobotAPI/libraries/skills/provider/SpecializedSkill.h>
+#include <RobotAPI/libraries/skills/provider/SimpleSpecializedSkill.h>
#include <armar6/skills/introduction/skills/aron/IntroduceYourselfAtLocationParams.aron.generated.h>
@@ -15,7 +15,7 @@ namespace armar6::skills::introduction::skills
*
* Detailed description of class introduction.
*/
- class IntroduceYourselfAtLocation : public ::armarx::skills::SpecializedSkill<IntroduceYourselfAtLocationParams>
+ class IntroduceYourselfAtLocation : public ::armarx::skills::SimpleSpecializedSkill<IntroduceYourselfAtLocationParams>
{
public:
static ::armarx::skills::SkillDescription GetSkillDescription();
@@ -24,7 +24,7 @@ namespace armar6::skills::introduction::skills
IntroduceYourselfAtLocation();
private:
- // SpecializedSkill interface
+ // SimpleSpecializedSkill interface
// Enable each function you want to override.
// bool isAvailable(const SpecializedInitInput&) const override;
// ::armarx::skills::Skill::InitResult init(const SpecializedInitInput&) override;
diff --git a/source/armar6/skills/introduction/skills/NavigationPose.cpp b/source/armar6/skills/introduction/skills/NavigationPose.cpp
index f5ca1212361134de683cdd1a145d09899ef3c750..a5316ad2134f589f5609e0164da96aa350da4b2e 100644
--- a/source/armar6/skills/introduction/skills/NavigationPose.cpp
+++ b/source/armar6/skills/introduction/skills/NavigationPose.cpp
@@ -27,9 +27,12 @@
namespace armar6::skills::introduction::skills
{
- NavigationPose::NavigationPose() : SpecializedSkill(GetSkillDescription())
+ NavigationPose::NavigationPose(const Remote& remote) :
+ SimpleSpecializedSkill(GetSkillDescription())
{
+ this->remote_ = remote;
}
+
armarx::skills::SkillDescription
NavigationPose::GetSkillDescription()
{
@@ -38,17 +41,18 @@ namespace armar6::skills::introduction::skills
defaultParams.jointPositionToleranceOverride["TorsoJoint"] = 0.5;
defaultParams.jointMaxSpeed = 0.3;
- return armarx::skills::SkillDescription{.skillName = "NavigationPose",
+ return armarx::skills::SkillDescription{.skillId = {.skillName = "NavigationPose"},
.description = "Assume Navigation Pose.",
- .robots = {},
+ .rootProfileDefaults = defaultParams.toAron(),
.timeout = ::armarx::Duration::Seconds(30),
- .acceptedType = ParamType::ToAronType(),
- .defaultParams = defaultParams.toAron()};
+ .parametersType = ParamType::ToAronType()};
;
}
+
armarx::skills::Skill::MainResult
NavigationPose::main(
- const armarx::skills::SpecializedSkill<NavigationPoseParams>::SpecializedMainInput& in)
+ const armarx::skills::SimpleSpecializedSkill<NavigationPoseParams>::SpecializedMainInput&
+ in)
{
armarx::NameControlModeMap controlModes;
@@ -75,7 +79,7 @@ namespace armar6::skills::introduction::skills
armarx::NameValueMap targetJointVelocities;
armarx::NameValueMap nullJointVelocities;
- if (this->checkWhetherSkillShouldStopASAP())
+ if (this->shouldSkillTerminate())
{
return MainResult{
.status = ::armarx::skills::TerminatedSkillStatus::Aborted,
@@ -85,7 +89,7 @@ namespace armar6::skills::introduction::skills
for (const auto& [name, value] : targetJointAngles)
{
controlModes.emplace(name, armarx::ePositionControl);
- targetJointVelocities.emplace(name, in.params.jointMaxSpeed);
+ targetJointVelocities.emplace(name, in.parameters.jointMaxSpeed);
nullJointVelocities.emplace(name, 0);
}
@@ -93,13 +97,13 @@ namespace armar6::skills::introduction::skills
remote_.kinematicUnit->setJointVelocities(targetJointVelocities);
remote_.kinematicUnit->setJointAngles(targetJointAngles);
- const auto& toleranceOverrides = in.params.jointPositionToleranceOverride;
+ const auto& toleranceOverrides = in.parameters.jointPositionToleranceOverride;
armarx::Metronome metronome(armarx::Frequency::Hertz(10));
bool hasReachedTarget = false;
while (not hasReachedTarget)
{
- if (this->checkWhetherSkillShouldStopASAP())
+ if (this->shouldSkillTerminate())
{
remote_.kinematicUnit->setJointVelocities(targetJointVelocities);
return MainResult{
@@ -114,7 +118,7 @@ namespace armar6::skills::introduction::skills
{
float current = jointAngles.at(name);
- float tolerance = in.params.jointPositionTolerance;
+ float tolerance = in.parameters.jointPositionTolerance;
if (auto it = toleranceOverrides.find(name); it != toleranceOverrides.end())
{
tolerance = it->second;
diff --git a/source/armar6/skills/introduction/skills/NavigationPose.h b/source/armar6/skills/introduction/skills/NavigationPose.h
index bb8eb7f0cc5d0aa72e9cead41d6b5c17edc51240..7025cd4fba74976f111236133631c2d7a2a0f625 100644
--- a/source/armar6/skills/introduction/skills/NavigationPose.h
+++ b/source/armar6/skills/introduction/skills/NavigationPose.h
@@ -23,28 +23,25 @@
#pragma once
#include <RobotAPI/interface/units/KinematicUnitInterface.h>
-#include <RobotAPI/libraries/skills/provider/SpecializedSkill.h>
+#include <RobotAPI/libraries/skills/provider/SimpleSpecializedSkill.h>
#include <armar6/skills/introduction/skills/aron/NavigationPose.aron.generated.h>
namespace armar6::skills::introduction::skills
{
- class NavigationPose : public armarx::skills::SpecializedSkill<NavigationPoseParams>
+ class NavigationPose : public armarx::skills::SimpleSpecializedSkill<NavigationPoseParams>
{
public:
- NavigationPose();
-
- static armarx::skills::SkillDescription GetSkillDescription();
struct Remote
{
armarx::KinematicUnitInterfacePrx kinematicUnit;
};
- void
- connectTo(const Remote& remote)
- {
- remote_ = remote;
- }
+
+ NavigationPose(const Remote& remote);
+
+
+ static armarx::skills::SkillDescription GetSkillDescription();
private:
MainResult main(const SpecializedMainInput& in) override;
diff --git a/source/armar6/skills/statecharts/Armar6WipingDemoGroup/ObserveWiping.cpp b/source/armar6/skills/statecharts/Armar6WipingDemoGroup/ObserveWiping.cpp
index 4dc8b0f052703cb6952180fd89d5d110c8f04efb..ee44492ad772e8ae2f5a2bcadc90897611077456 100644
--- a/source/armar6/skills/statecharts/Armar6WipingDemoGroup/ObserveWiping.cpp
+++ b/source/armar6/skills/statecharts/Armar6WipingDemoGroup/ObserveWiping.cpp
@@ -107,7 +107,11 @@ ObserveWiping::run()
layerDemoArea.add(area);
- arviz.commit({layerDemoArea, layerWristSamples, layerTargetWipingTraj, layerAvgTraj, layerHandPositionTrajSegments});
+ arviz.commit({layerDemoArea,
+ layerWristSamples,
+ layerTargetWipingTraj,
+ layerAvgTraj,
+ layerHandPositionTrajSegments});
}
Armar6WipingDemoGroupStatechartContextExtension* c =
@@ -116,10 +120,10 @@ ObserveWiping::run()
armarx::armem::client::MemoryNameSystem mns = c->memoryNameSystem();
mns.setComponent(getContext<Armar6WipingDemoGroupStatechartContext>());
- armarx::armem::human::client::Reader humanPoseReader(mns);
+ armarx::armem::human::client::Reader humanPoseReader;
humanPoseReader.setProperties({"Human", "Pose"});
- humanPoseReader.connect();
+ humanPoseReader.connect(mns);
ARMARX_INFO << "Connected to memory";
@@ -192,9 +196,7 @@ ObserveWiping::run()
armarx::Duration epsilon = armarx::Duration::MicroSeconds(1);
armarx::Duration maxAge = now - timeOfLastReceivedPose - epsilon;
armarx::armem::human::client::Reader::Query query{
- in.getArMemProviderSegmentName(),
- now,
- maxAge};
+ in.getArMemProviderSegmentName(), now, maxAge};
armarx::armem::human::client::Reader::Result queryResult = humanPoseReader.query(query);
@@ -224,7 +226,7 @@ ObserveWiping::run()
handKeypoint = nearestHumanPose.keypoints.at(handKeypointName);
if (!isKeypointValid(handKeypoint.value()))
{
- ARMARX_DEBUG << "rejecting hand keypoint";
+ ARMARX_DEBUG << "rejecting hand keypoint";
handKeypoint = std::nullopt;
}
}
@@ -247,23 +249,23 @@ ObserveWiping::run()
auto rHandMedianFilteredValue = rHandFilter.getValue();
if (!rHandMedianFilteredValue)
{
- ARMARX_DEBUG << "The value of the Median filter is invalid, continue";
- cycle.waitForCycleDuration();
- continue;
+ ARMARX_DEBUG << "The value of the Median filter is invalid, continue";
+ cycle.waitForCycleDuration();
+ continue;
}
- Vector3Ptr filteredRHandPosition = VariantPtr::dynamicCast(rHandMedianFilteredValue)->get<Vector3>();
+ Vector3Ptr filteredRHandPosition =
+ VariantPtr::dynamicCast(rHandMedianFilteredValue)->get<Vector3>();
- if(!positionExpFilterInitialized)
+ if (!positionExpFilterInitialized)
{
- expFilteredPosition = filteredRHandPosition->toEigen();
- positionExpFilterInitialized = true;
+ expFilteredPosition = filteredRHandPosition->toEigen();
+ positionExpFilterInitialized = true;
}
else
{
- expFilteredPosition =
- (1 - positionExpFilterCoefficient) * expFilteredPosition +
- positionExpFilterCoefficient * filteredRHandPosition->toEigen();
+ expFilteredPosition = (1 - positionExpFilterCoefficient) * expFilteredPosition +
+ positionExpFilterCoefficient * filteredRHandPosition->toEigen();
}
FramedPositionPtr globalHandPosition =
@@ -315,11 +317,14 @@ ObserveWiping::run()
}
else
{
- ARMARX_INFO << "viz traj segment " << VAROUT(dataCounter) << " from " << VAROUT(prevGlobalHandPosition) << " to " << VAROUT(globalHandPosition->toEigen());
- layerHandPositionTrajSegments.add(viz::Cylinder("Hand Position Trajectory " + std::to_string(dataCounter))
- .fromTo(prevGlobalHandPosition, globalHandPosition->toEigen())
- .radius(5)
- .color(viz::Color::orange(255, 64)));
+ ARMARX_INFO << "viz traj segment " << VAROUT(dataCounter) << " from "
+ << VAROUT(prevGlobalHandPosition) << " to "
+ << VAROUT(globalHandPosition->toEigen());
+ layerHandPositionTrajSegments.add(
+ viz::Cylinder("Hand Position Trajectory " + std::to_string(dataCounter))
+ .fromTo(prevGlobalHandPosition, globalHandPosition->toEigen())
+ .radius(5)
+ .color(viz::Color::orange(255, 64)));
prevGlobalHandPosition = globalHandPosition->toEigen();
arviz.commit(layerHandPositionTrajSegments);
}
@@ -384,9 +389,9 @@ ObserveWiping::isInWipingDemoSpace(Eigen::Vector3f currentPos,
{
simox::AxisAlignedBoundingBox aabb(min, max);
bool result = aabb.is_inside(currentPos);
-// ARMARX_IMPORTANT << "is \t" << currentPos.transpose() << "\n between \t" << min.transpose()
-// << "\n and \t" << max.transpose() << "?"
-// << "\n\t " << result;
+ // ARMARX_IMPORTANT << "is \t" << currentPos.transpose() << "\n between \t" << min.transpose()
+ // << "\n and \t" << max.transpose() << "?"
+ // << "\n\t " << result;
return result;
}