diff --git a/source/armarx/control/components/control_skill_provider/Component.cpp b/source/armarx/control/components/control_skill_provider/Component.cpp
index d944e193fb02bd283f7e6cee4024eac305acd801..3a557d17bcb5e1c18be7bc23f4e3e6c01e649fb7 100644
--- a/source/armarx/control/components/control_skill_provider/Component.cpp
+++ b/source/armarx/control/components/control_skill_provider/Component.cpp
@@ -4,6 +4,7 @@
#include <armarx/control/skills/skills/MoveJointsToPosition.h>
#include <armarx/control/skills/skills/MoveJointsWithVelocity.h>
+#include <armarx/control/skills/skills/ShapeHand.h>
namespace armarx::control::components::control_skill_provider
{
@@ -15,6 +16,8 @@ namespace armarx::control::components::control_skill_provider
new ::armarx::ComponentPropertyDefinitions(getConfigIdentifier());
def->component(remote.kinematicUnit);
+ def->component(remote.leftHandUnit, "LeftHandUnit", "LeftHandUnit");
+ def->component(remote.rightHandUnit, "RightHandUnit", "RightHandUnit");
def->required(properties.robotName, "RobotName", "Name of the robot.");
@@ -43,6 +46,17 @@ namespace armarx::control::components::control_skill_provider
addSkillFactory<armarx::control::skills::skills::MoveJointsWithVelocity>(
skillRemote);
}
+ {
+ armarx::control::skills::skills::ShapeHand::Remote skillRemote{
+ remote.leftHandUnit, remote.rightHandUnit};
+ addSkillFactory<armarx::control::skills::skills::ShapeHand>(skillRemote);
+ }
+ {
+ addSkillFactory<armarx::control::skills::skills::OpenHand>();
+ }
+ {
+ addSkillFactory<armarx::control::skills::skills::CloseHand>();
+ }
}
}
diff --git a/source/armarx/control/components/control_skill_provider/Component.h b/source/armarx/control/components/control_skill_provider/Component.h
index ad99599195fdb44b2401f0bd98e801de881a020c..61c32aefa06df513594bc3d3b243c6c03ca2e4a1 100644
--- a/source/armarx/control/components/control_skill_provider/Component.h
+++ b/source/armarx/control/components/control_skill_provider/Component.h
@@ -2,6 +2,7 @@
#include <ArmarXCore/core/Component.h>
+#include <RobotAPI/interface/units/HandUnitInterface.h>
#include <RobotAPI/interface/units/KinematicUnitInterface.h>
#include <RobotAPI/libraries/armem/client/plugins/PluginUser.h>
#include <RobotAPI/libraries/armem/client/plugins/ReaderWriterPlugin.h>
@@ -48,6 +49,8 @@ namespace armarx::control::components::control_skill_provider
struct Remote
{
::armarx::KinematicUnitInterfacePrx kinematicUnit;
+ ::armarx::HandUnitInterfacePrx leftHandUnit;
+ ::armarx::HandUnitInterfacePrx rightHandUnit;
} remote;
struct Properties
diff --git a/source/armarx/control/skills/CMakeLists.txt b/source/armarx/control/skills/CMakeLists.txt
index 30a2865035f108052a2e68efae6d459c3d4cba70..73ac05a62262d857cf4e2bada333deb7ca485b69 100644
--- a/source/armarx/control/skills/CMakeLists.txt
+++ b/source/armarx/control/skills/CMakeLists.txt
@@ -2,6 +2,7 @@ armarx_add_aron_library(skills_aron
ARON_FILES
skills/aron/MoveJointsToPositionParams.xml
skills/aron/MoveJointsWithVelocityParams.xml
+ skills/aron/ShapeHandParams.xml
)
armarx_add_library(skills
@@ -9,11 +10,13 @@ armarx_add_library(skills
skills.cpp
skills/MoveJointsToPosition.cpp
skills/MoveJointsWithVelocity.cpp
+ skills/ShapeHand.cpp
HEADERS
skills.h
skills/MoveJointsToPosition.h
skills/MoveJointsWithVelocity.h
+ skills/ShapeHand.h
DEPENDENCIES_PUBLIC
@@ -24,6 +27,7 @@ armarx_add_library(skills
# RobotAPI
RobotAPISkills
armem_robot_state
+ RobotAPIUnits
RobotAPISimpleTrajectory
# armarx_control
diff --git a/source/armarx/control/skills/skills/ShapeHand.cpp b/source/armarx/control/skills/skills/ShapeHand.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8396458e42a20a973918eff28e77a7707c5df7d9
--- /dev/null
+++ b/source/armarx/control/skills/skills/ShapeHand.cpp
@@ -0,0 +1,228 @@
+#include "ShapeHand.h"
+
+#include <ArmarXCore/observers/variant/SingleTypeVariantList.h>
+
+namespace armarx::control::skills::skills
+{
+ ShapeHand::ShapeHand(const Remote& r) :
+ ::armarx::skills::SpecializedSkill<ShapeHandParams>(GetSkillDescription()), remote(r)
+ {
+ }
+
+ ::armarx::skills::Skill::MainResult
+ ShapeHand::main()
+ {
+ // verify exactly one of the parameters 'shapeName' and 'jointAngles' is set
+ if ((not getParameters().shapeName) and (not getParameters().jointAngles))
+ {
+ ARMARX_ERROR << "Exactly one of the parameters 'shapeName' and 'jointAngles' must be "
+ "set. However, neither has been set.";
+ return MakeFailedResult();
+ }
+ else if (getParameters().shapeName and getParameters().jointAngles)
+ {
+ ARMARX_ERROR << "Exactly one of the parameters 'shapeName' and 'jointAngles' must be "
+ "set. However, both have been set.";
+ return MakeFailedResult();
+ }
+
+ // set shape/joinAngles
+
+ auto& handUnit = getHandUnit(getParameters().hand);
+
+ if (getParameters().shapeName)
+ {
+ const auto& shape = getParameters().shapeName.value();
+
+ // check that the shape is defined
+ const auto defShapes =
+ ::armarx::SingleTypeVariantListPtr::dynamicCast(handUnit->getShapeNames())
+ ->toStdVector<std::string>();
+
+ if (not std::any_of(defShapes.begin(),
+ defShapes.end(),
+ [&shape](const auto& defShape) { return shape == defShape; }))
+ {
+ ARMARX_ERROR << "Shape with name '" << shape
+ << "' not defined. Defined shapes: " << defShapes;
+ return MakeFailedResult();
+ }
+
+ // set shape
+ ARMARX_INFO << "Setting shape of hand '" << handUnit->getHandName() << "' to '" << shape
+ << "'.";
+
+ handUnit->setShape(shape);
+
+ if (getParameters().waitUntilFinished)
+ {
+ return waitUntilFinished(handUnit->getShapeJointValues(shape));
+ }
+ }
+
+ else // getParameters().jointAngles
+ {
+ const auto jointAngles = getParameters().jointAngles.value();
+
+ ARMARX_INFO << "Setting joint angles of hand '" << handUnit->getHandName()
+ << "' to: " << jointAngles;
+
+ handUnit->setJointAngles(jointAngles);
+
+ if (getParameters().waitUntilFinished)
+ {
+ return waitUntilFinished(jointAngles);
+ }
+ }
+ return MakeSucceededResult();
+ }
+
+ armarx::skills::Skill::MainResult
+ ShapeHand::waitUntilFinished(const NameValueMap& targetAngles)
+ {
+ auto& handUnit = getHandUnit(getParameters().hand);
+ const auto movementTimeout = getParameters().waitUntilFinished->movementTimeout;
+
+ ARMARX_INFO << "Waiting until target position is reached"
+ << (movementTimeout ? " or movement times out" : "");
+
+ struct
+ {
+ NameValueMap angles;
+ ::armarx::DateTime time;
+ } pastJointAngles{handUnit->getCurrentJointValues(), DateTime::Now()};
+
+ const Clock clock;
+ const auto sleepTime = Duration::MilliSeconds(20);
+
+ bool reached = false;
+ bool movementTimedOut = false;
+
+ while (not shouldSkillTerminate() and not reached and not movementTimedOut)
+ {
+ const auto currentAngles = handUnit->getCurrentJointValues();
+
+ // check if target reached
+ reached = true;
+ for (const auto& [jointName, targetAngle] : targetAngles)
+ {
+ if (std::abs(targetAngle - currentAngles.at(jointName)) > getAccuracy(jointName))
+ {
+ reached = false;
+ ARMARX_VERBOSE
+ << deactivateSpam(1) << "Delta for joint '" << jointName
+ << "' is: " << std::abs(targetAngle - currentAngles.at(jointName)) << " > "
+ << getAccuracy(jointName) << "(accuracy)";
+ break;
+ }
+ }
+
+ // check if movement timed out
+ if (movementTimeout and (DateTime::Now() - pastJointAngles.time).toSecondsDouble() >=
+ movementTimeout.value())
+ {
+ movementTimedOut = true;
+ for (const auto& [jointName, targetAngle] : targetAngles)
+ {
+ if (std::abs(currentAngles.at(jointName) -
+ pastJointAngles.angles.at(jointName)) > getAccuracy(jointName))
+ {
+ movementTimedOut = false;
+ ARMARX_VERBOSE
+ << deactivateSpam(1) << "Joint '" << jointName << "' still moving: "
+ << std::abs(currentAngles.at(jointName) -
+ pastJointAngles.angles.at(jointName))
+ << " > " << getAccuracy(jointName) << "(accuracy)"
+ << "in the last " << movementTimeout.value() << "s (movement timeout)";
+ break;
+ }
+ }
+ pastJointAngles = {currentAngles, DateTime::Now()};
+ }
+
+ clock.waitFor(sleepTime);
+ }
+
+ if (reached)
+ {
+ ARMARX_INFO << "Target position reached!";
+ }
+ else if (movementTimedOut)
+ {
+ ARMARX_INFO << "Movement timed out! Joints stopped moving. Not waiting anymore until "
+ "target postion is reached";
+ }
+ else if (shouldSkillTerminate())
+ {
+ ARMARX_IMPORTANT
+ << "Skill aborted. Not waiting anymore until target position is reached";
+ return MakeAbortedResult();
+ }
+
+ return MakeSucceededResult();
+ }
+
+ HandUnitInterfacePrx&
+ ShapeHand::getHandUnit(const Hand hand)
+ {
+ if (hand == ::armarx::control::skills::skills::Hand::Left)
+ {
+ return remote.leftHandUnit;
+ }
+ else
+ {
+ return remote.rightHandUnit;
+ }
+ }
+
+ float
+ ShapeHand::getAccuracy(const std::string& jointName)
+ {
+ const auto accuracy = getParameters().waitUntilFinished->accuracy;
+ const auto& accuracyOverride = getParameters().waitUntilFinished->accuracyOverride;
+
+ return accuracyOverride.count(jointName) > 0 ? accuracyOverride.at(jointName) : accuracy;
+ }
+
+ OpenHand::OpenHand() :
+ armarx::skills::SpecializedSkill<OpenCloseHandParams>(GetSkillDescription())
+ {
+ }
+
+ ::armarx::skills::Skill::MainResult
+ OpenHand::main()
+ {
+ // call ShapeHand-skill
+ ::armarx::skills::SkillID id;
+ id.skillName = "ShapeHand";
+ id.providerId = getSkillId().providerId;
+
+ ShapeHandParams params;
+ params.hand = getParameters().hand;
+ params.shapeName = "Open";
+ params.waitUntilFinished = getParameters().waitUntilFinished;
+
+ return {(*callSubskill(::armarx::skills::SkillProxy(manager, id), params.toAron())).status};
+ }
+
+ CloseHand::CloseHand() :
+ armarx::skills::SpecializedSkill<OpenCloseHandParams>(GetSkillDescription())
+ {
+ }
+
+ ::armarx::skills::Skill::MainResult
+ CloseHand::main()
+ {
+ // call ShapeHand-skill
+ ::armarx::skills::SkillID id;
+ id.skillName = "ShapeHand";
+ id.providerId = getSkillId().providerId;
+
+ ShapeHandParams params;
+ params.hand = getParameters().hand;
+ params.shapeName = "Close";
+ params.waitUntilFinished = getParameters().waitUntilFinished;
+
+ return {(*callSubskill(::armarx::skills::SkillProxy(manager, id), params.toAron())).status};
+ }
+} // namespace armarx::control::skills::skills
diff --git a/source/armarx/control/skills/skills/ShapeHand.h b/source/armarx/control/skills/skills/ShapeHand.h
new file mode 100644
index 0000000000000000000000000000000000000000..9e6b0daaf62a52e47fe010f85cd00fbb57270aed
--- /dev/null
+++ b/source/armarx/control/skills/skills/ShapeHand.h
@@ -0,0 +1,81 @@
+#pragma once
+
+#include <RobotAPI/interface/units/HandUnitInterface.h>
+#include <RobotAPI/libraries/skills/provider/SpecializedSkill.h>
+
+#include <armarx/control/skills/skills/aron/ShapeHandParams.aron.generated.h>
+
+namespace armarx::control::skills::skills
+{
+
+ class ShapeHand : public ::armarx::skills::SpecializedSkill<ShapeHandParams>
+ {
+
+ public:
+ static ::armarx::skills::SkillDescription
+ GetSkillDescription()
+ {
+ return ::armarx::skills::SkillDescription{
+ .skillId = {.skillName = "ShapeHand"},
+ .description =
+ "Shapes the hand according to a predefined shape or passed joint angles",
+ .timeout = ::armarx::Duration::Minutes(1),
+ .parametersType = ParamType::ToAronType()};
+ }
+
+ struct Remote
+ {
+ ::armarx::HandUnitInterfacePrx& leftHandUnit;
+ ::armarx::HandUnitInterfacePrx& rightHandUnit;
+ };
+
+ ShapeHand(const Remote&);
+
+ private:
+ ::armarx::skills::Skill::MainResult main() override;
+ Skill::MainResult waitUntilFinished(const NameValueMap& targetAngles);
+
+ private:
+ Remote remote;
+ ::armarx::HandUnitInterfacePrx& getHandUnit(const Hand);
+
+ float getAccuracy(const std::string& jointName);
+ };
+
+ class OpenHand : public ::armarx::skills::SpecializedSkill<OpenCloseHandParams>
+ {
+ public:
+ static ::armarx::skills::SkillDescription
+ GetSkillDescription()
+ {
+ return ::armarx::skills::SkillDescription{.skillId = {.skillName = "OpenHand"},
+ .description = "Opens the hand",
+ .timeout = ::armarx::Duration::Minutes(1),
+ .parametersType = ParamType::ToAronType()};
+ }
+
+ OpenHand();
+
+ private:
+ ::armarx::skills::Skill::MainResult main() override;
+ };
+
+ class CloseHand : public ::armarx::skills::SpecializedSkill<OpenCloseHandParams>
+ {
+ public:
+ static ::armarx::skills::SkillDescription
+ GetSkillDescription()
+ {
+ return ::armarx::skills::SkillDescription{.skillId = {.skillName = "CloseHand"},
+ .description = "Closes the hand",
+ .timeout = ::armarx::Duration::Minutes(1),
+ .parametersType = ParamType::ToAronType()};
+ }
+
+ CloseHand();
+
+ private:
+ ::armarx::skills::Skill::MainResult main() override;
+ };
+
+} // namespace armarx::control::skills::skills
diff --git a/source/armarx/control/skills/skills/aron/ShapeHandParams.xml b/source/armarx/control/skills/skills/aron/ShapeHandParams.xml
new file mode 100644
index 0000000000000000000000000000000000000000..063cf8dfe6899a108b9beb4ffa13a100dd2a9b5e
--- /dev/null
+++ b/source/armarx/control/skills/skills/aron/ShapeHandParams.xml
@@ -0,0 +1,83 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+ <GenerateTypes>
+
+ <IntEnum name="::armarx::control::skills::skills::Hand">
+ <EnumValue key="Left" value="0"/>
+ <EnumValue key="Right" value="1"/>
+ </IntEnum>
+
+ <!-- Parameters to define when the skill should terminate (if waiting) -->
+ <Object name="::armarx::control::skills::skills::WaitUntilFinishedParams">
+
+ <!-- Each joint must be within this accuracy around the target angle for the skill
+ to terminate -->
+ <ObjectChild key="accuracy">
+ <float />
+ </ObjectChild>
+
+ <!-- joint name -> accuracy -->
+ <ObjectChild key="accuracyOverride">
+ <Dict>
+ <float />
+ </Dict>
+ </ObjectChild>
+
+ <!-- If set the skill also terminates when no joint moves more than 'accuracy' in
+ 'movementTimeout' seconds (e.g. when joint is blocked) -->
+ <ObjectChild key="movementTimeout">
+ <float optional="true" />
+ </ObjectChild>
+
+ </Object>
+
+
+ <!-- 'ShapeHand'-skill parameters -->
+ <Object name="::armarx::control::skills::skills::ShapeHandParams">
+
+ <ObjectChild key="hand">
+ <::armarx::control::skills::skills::Hand />
+ </ObjectChild>
+
+ <!-- Must provide exactly one of the parameters 'shapeName' and 'jointAngles' -->
+
+ <!-- name of predefined hand shape -->
+ <ObjectChild key="shapeName">
+ <string optional="true" />
+ </ObjectChild>
+
+ <!-- joint name -> target value -->
+ <ObjectChild key="jointAngles">
+ <Dict optional="true">
+ <float />
+ </Dict>
+ </ObjectChild>
+
+ <!-- If set, the skill blocks until target angles are reached / angles don't change
+ anymore. Otherwise, the skill terminates instantly. When in simulation,
+ 'simulateMovement' must be set in order for this to work. -->
+ <ObjectChild key="waitUntilFinished">
+ <::armarx::control::skills::skills::WaitUntilFinishedParams optional="true" />
+ </ObjectChild>
+
+ </Object>
+
+
+ <!-- 'OpenHand'/'CloseHand'-skills parameters -->
+ <Object name="::armarx::control::skills::skills::OpenCloseHandParams">
+
+ <ObjectChild key="hand">
+ <::armarx::control::skills::skills::Hand />
+ </ObjectChild>
+
+ <!-- If set, the skill blocks until target angles are reached / angles don't change
+ anymore. Otherwise, the skill terminates instantly. When in simulation,
+ 'simulateMovement' must be set in order for this to work. -->
+ <ObjectChild key="waitUntilFinished">
+ <::armarx::control::skills::skills::WaitUntilFinishedParams optional="true" />
+ </ObjectChild>
+
+ </Object>
+
+ </GenerateTypes>
+</AronTypeDefinition>