From 4084bc720704bb60bc9aa4f06533aa969e283087 Mon Sep 17 00:00:00 2001
From: Patrick Dormanns <patrick.dormanns@student.kit.edu>
Date: Tue, 14 Nov 2023 21:23:13 +0100
Subject: [PATCH] implemented RelaxHand-skill
---
.../control_skill_provider/Component.cpp | 12 +++++--
source/armarx/control/skills/CMakeLists.txt | 4 +++
source/armarx/control/skills/skills.h | 24 ++++++++++++-
.../skills/skills/MoveJointsToPosition.h | 2 +-
.../skills/skills/MoveJointsWithVelocity.cpp | 2 +-
.../skills/skills/MoveJointsWithVelocity.h | 4 +--
.../control/skills/skills/RelaxHand.cpp | 20 +++++++++++
.../armarx/control/skills/skills/RelaxHand.h | 34 +++++++++++++++++++
.../control/skills/skills/ShapeHand.cpp | 19 ++---------
.../armarx/control/skills/skills/ShapeHand.h | 14 +++-----
.../control/skills/skills/aron/HandParam.xml | 11 ++++++
.../skills/skills/aron/RelaxHandParams.xml | 20 +++++++++++
.../skills/skills/aron/ShapeHandParams.xml | 10 +++---
13 files changed, 137 insertions(+), 39 deletions(-)
create mode 100644 source/armarx/control/skills/skills/RelaxHand.cpp
create mode 100644 source/armarx/control/skills/skills/RelaxHand.h
create mode 100644 source/armarx/control/skills/skills/aron/HandParam.xml
create mode 100644 source/armarx/control/skills/skills/aron/RelaxHandParams.xml
diff --git a/source/armarx/control/components/control_skill_provider/Component.cpp b/source/armarx/control/components/control_skill_provider/Component.cpp
index 3a557d17..b6b21e48 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/RelaxHand.h>
#include <armarx/control/skills/skills/ShapeHand.h>
namespace armarx::control::components::control_skill_provider
@@ -42,13 +43,13 @@ namespace armarx::control::components::control_skill_provider
addSkillFactory<armarx::control::skills::skills::MoveJointsToPosition>(skillRemote);
}
{
- armarx::control::skills::Remote skillRemote{remote.kinematicUnit};
+ armarx::control::skills::KinematicUnitRemote skillRemote{remote.kinematicUnit};
addSkillFactory<armarx::control::skills::skills::MoveJointsWithVelocity>(
skillRemote);
}
{
- armarx::control::skills::skills::ShapeHand::Remote skillRemote{
- remote.leftHandUnit, remote.rightHandUnit};
+ armarx::control::skills::HandUnitRemote skillRemote{remote.leftHandUnit,
+ remote.rightHandUnit};
addSkillFactory<armarx::control::skills::skills::ShapeHand>(skillRemote);
}
{
@@ -57,6 +58,11 @@ namespace armarx::control::components::control_skill_provider
{
addSkillFactory<armarx::control::skills::skills::CloseHand>();
}
+ {
+ armarx::control::skills::HandUnitRemote skillRemote{remote.leftHandUnit,
+ remote.rightHandUnit};
+ addSkillFactory<armarx::control::skills::skills::RelaxHand>(skillRemote);
+ }
}
}
diff --git a/source/armarx/control/skills/CMakeLists.txt b/source/armarx/control/skills/CMakeLists.txt
index 73ac05a6..e277fbd4 100644
--- a/source/armarx/control/skills/CMakeLists.txt
+++ b/source/armarx/control/skills/CMakeLists.txt
@@ -2,7 +2,9 @@ armarx_add_aron_library(skills_aron
ARON_FILES
skills/aron/MoveJointsToPositionParams.xml
skills/aron/MoveJointsWithVelocityParams.xml
+ skills/aron/HandParam.xml
skills/aron/ShapeHandParams.xml
+ skills/aron/RelaxHandParams.xml
)
armarx_add_library(skills
@@ -11,12 +13,14 @@ armarx_add_library(skills
skills/MoveJointsToPosition.cpp
skills/MoveJointsWithVelocity.cpp
skills/ShapeHand.cpp
+ skills/RelaxHand.cpp
HEADERS
skills.h
skills/MoveJointsToPosition.h
skills/MoveJointsWithVelocity.h
skills/ShapeHand.h
+ skills/RelaxHand.h
DEPENDENCIES_PUBLIC
diff --git a/source/armarx/control/skills/skills.h b/source/armarx/control/skills/skills.h
index 172fd68a..51652e9d 100644
--- a/source/armarx/control/skills/skills.h
+++ b/source/armarx/control/skills/skills.h
@@ -2,13 +2,35 @@
#include <string>
+#include <RobotAPI/interface/units/HandUnitInterface.h>
#include <RobotAPI/interface/units/KinematicUnitInterface.h>
+#include <armarx/control/skills/skills/aron/ShapeHandParams.aron.generated.h>
+
namespace armarx::control::skills
{
- struct Remote
+ struct KinematicUnitRemote
{
::armarx::KinematicUnitInterfacePrx kinematicUnit;
};
+ struct HandUnitRemote
+ {
+ ::armarx::HandUnitInterfacePrx& leftHandUnit;
+ ::armarx::HandUnitInterfacePrx& rightHandUnit;
+
+ ::armarx::HandUnitInterfacePrx&
+ getHandUnit(const skills::Hand hand)
+ {
+ if (hand == skills::Hand::Left)
+ {
+ return leftHandUnit;
+ }
+ else // hand == Hand::Right
+ {
+ return rightHandUnit;
+ }
+ }
+ };
+
} // namespace armarx::control::skills
diff --git a/source/armarx/control/skills/skills/MoveJointsToPosition.h b/source/armarx/control/skills/skills/MoveJointsToPosition.h
index 498545d5..fc8c0ae2 100644
--- a/source/armarx/control/skills/skills/MoveJointsToPosition.h
+++ b/source/armarx/control/skills/skills/MoveJointsToPosition.h
@@ -25,7 +25,7 @@ namespace armarx::control::skills::skills
{
public:
- struct Remote : public armarx::control::skills::Remote
+ struct Remote : public armarx::control::skills::KinematicUnitRemote
{
::armarx::armem::robot_state::VirtualRobotReader virtualRobotReader;
std::string robotName;
diff --git a/source/armarx/control/skills/skills/MoveJointsWithVelocity.cpp b/source/armarx/control/skills/skills/MoveJointsWithVelocity.cpp
index dbf8ec4e..287a7ad6 100644
--- a/source/armarx/control/skills/skills/MoveJointsWithVelocity.cpp
+++ b/source/armarx/control/skills/skills/MoveJointsWithVelocity.cpp
@@ -5,7 +5,7 @@
namespace armarx::control::skills::skills
{
- MoveJointsWithVelocity::MoveJointsWithVelocity(const Remote& r) :
+ MoveJointsWithVelocity::MoveJointsWithVelocity(const KinematicUnitRemote& r) :
::armarx::skills::SimpleSpecializedSkill<ParamType>(GetSkillDescription()), remote(r)
{
}
diff --git a/source/armarx/control/skills/skills/MoveJointsWithVelocity.h b/source/armarx/control/skills/skills/MoveJointsWithVelocity.h
index 5f201e60..7810cfd6 100644
--- a/source/armarx/control/skills/skills/MoveJointsWithVelocity.h
+++ b/source/armarx/control/skills/skills/MoveJointsWithVelocity.h
@@ -35,7 +35,7 @@ namespace armarx::control::skills::skills
.parametersType = ParamType::ToAronType()};
}
- MoveJointsWithVelocity(const Remote&);
+ MoveJointsWithVelocity(const KinematicUnitRemote&);
private:
@@ -53,7 +53,7 @@ namespace armarx::control::skills::skills
const armarx::core::time::Duration& dur);
private:
- Remote remote;
+ KinematicUnitRemote remote;
};
} // namespace armarx::control::skills::skills
diff --git a/source/armarx/control/skills/skills/RelaxHand.cpp b/source/armarx/control/skills/skills/RelaxHand.cpp
new file mode 100644
index 00000000..3c2a121c
--- /dev/null
+++ b/source/armarx/control/skills/skills/RelaxHand.cpp
@@ -0,0 +1,20 @@
+#include "RelaxHand.h"
+
+namespace armarx::control::skills::skills
+{
+ RelaxHand::RelaxHand(const HandUnitRemote& r) :
+ ::armarx::skills::SimpleSpecializedSkill<RelaxHandParams>(GetSkillDescription()), remote(r)
+ {
+ }
+
+ armarx::skills::Skill::MainResult
+ RelaxHand::main(const SpecializedMainInput& in)
+ {
+ HandUnitInterfacePrx& handUnit = remote.getHandUnit(in.parameters.hand);
+
+ ARMARX_INFO << "Setting shape of hand '" << handUnit->getHandName() << "' to 'Relax'.";
+ handUnit->setShape("Relax");
+ return MakeSucceededResult();
+ }
+
+} // namespace armarx::control::skills::skills
diff --git a/source/armarx/control/skills/skills/RelaxHand.h b/source/armarx/control/skills/skills/RelaxHand.h
new file mode 100644
index 00000000..8ef56879
--- /dev/null
+++ b/source/armarx/control/skills/skills/RelaxHand.h
@@ -0,0 +1,34 @@
+#pragma once
+
+#include <RobotAPI/libraries/skills/provider/SimpleSpecializedSkill.h>
+
+#include <armarx/control/skills/skills/aron/RelaxHandParams.aron.generated.h>
+
+#include "../skills.h"
+
+namespace armarx::control::skills::skills
+{
+
+ class RelaxHand : public ::armarx::skills::SimpleSpecializedSkill<RelaxHandParams>
+ {
+
+ public:
+ static ::armarx::skills::SkillDescription
+ GetSkillDescription()
+ {
+ return ::armarx::skills::SkillDescription{.skillId = {.skillName = "RelaxHand"},
+ .description = "Relaxes hand joints",
+ .timeout = ::armarx::Duration::Seconds(1),
+ .parametersType = ParamType::ToAronType()};
+ }
+
+ RelaxHand(const HandUnitRemote&);
+
+ private:
+ ::armarx::skills::Skill::MainResult main(const SpecializedMainInput& in) override;
+
+ private:
+ HandUnitRemote remote;
+ };
+
+} // namespace armarx::control::skills::skills
diff --git a/source/armarx/control/skills/skills/ShapeHand.cpp b/source/armarx/control/skills/skills/ShapeHand.cpp
index 8396458e..a42bffa8 100644
--- a/source/armarx/control/skills/skills/ShapeHand.cpp
+++ b/source/armarx/control/skills/skills/ShapeHand.cpp
@@ -4,7 +4,7 @@
namespace armarx::control::skills::skills
{
- ShapeHand::ShapeHand(const Remote& r) :
+ ShapeHand::ShapeHand(const HandUnitRemote& r) :
::armarx::skills::SpecializedSkill<ShapeHandParams>(GetSkillDescription()), remote(r)
{
}
@@ -28,7 +28,7 @@ namespace armarx::control::skills::skills
// set shape/joinAngles
- auto& handUnit = getHandUnit(getParameters().hand);
+ auto& handUnit = remote.getHandUnit(getParameters().hand);
if (getParameters().shapeName)
{
@@ -80,7 +80,7 @@ namespace armarx::control::skills::skills
armarx::skills::Skill::MainResult
ShapeHand::waitUntilFinished(const NameValueMap& targetAngles)
{
- auto& handUnit = getHandUnit(getParameters().hand);
+ auto& handUnit = remote.getHandUnit(getParameters().hand);
const auto movementTimeout = getParameters().waitUntilFinished->movementTimeout;
ARMARX_INFO << "Waiting until target position is reached"
@@ -162,19 +162,6 @@ namespace armarx::control::skills::skills
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)
{
diff --git a/source/armarx/control/skills/skills/ShapeHand.h b/source/armarx/control/skills/skills/ShapeHand.h
index 9e6b0daa..7310115e 100644
--- a/source/armarx/control/skills/skills/ShapeHand.h
+++ b/source/armarx/control/skills/skills/ShapeHand.h
@@ -1,10 +1,11 @@
#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>
+#include "../skills.h"
+
namespace armarx::control::skills::skills
{
@@ -23,21 +24,14 @@ namespace armarx::control::skills::skills
.parametersType = ParamType::ToAronType()};
}
- struct Remote
- {
- ::armarx::HandUnitInterfacePrx& leftHandUnit;
- ::armarx::HandUnitInterfacePrx& rightHandUnit;
- };
-
- ShapeHand(const Remote&);
+ ShapeHand(const HandUnitRemote&);
private:
::armarx::skills::Skill::MainResult main() override;
Skill::MainResult waitUntilFinished(const NameValueMap& targetAngles);
private:
- Remote remote;
- ::armarx::HandUnitInterfacePrx& getHandUnit(const Hand);
+ HandUnitRemote remote;
float getAccuracy(const std::string& jointName);
};
diff --git a/source/armarx/control/skills/skills/aron/HandParam.xml b/source/armarx/control/skills/skills/aron/HandParam.xml
new file mode 100644
index 00000000..f0cb63ff
--- /dev/null
+++ b/source/armarx/control/skills/skills/aron/HandParam.xml
@@ -0,0 +1,11 @@
+<?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>
+
+ </GenerateTypes>
+</AronTypeDefinition>
diff --git a/source/armarx/control/skills/skills/aron/RelaxHandParams.xml b/source/armarx/control/skills/skills/aron/RelaxHandParams.xml
new file mode 100644
index 00000000..27a257ae
--- /dev/null
+++ b/source/armarx/control/skills/skills/aron/RelaxHandParams.xml
@@ -0,0 +1,20 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<AronTypeDefinition>
+
+ <AronIncludes>
+ <Include include="armarx/control/skills/skills/aron/HandParam.xml" />
+ </AronIncludes>
+
+ <GenerateTypes>
+
+ <!-- 'RelaxHand'-skill parameters -->
+ <Object name="::armarx::control::skills::skills::RelaxHandParams">
+
+ <ObjectChild key="hand">
+ <::armarx::control::skills::skills::Hand />
+ </ObjectChild>
+
+ </Object>
+
+ </GenerateTypes>
+</AronTypeDefinition>
diff --git a/source/armarx/control/skills/skills/aron/ShapeHandParams.xml b/source/armarx/control/skills/skills/aron/ShapeHandParams.xml
index 063cf8df..cd468a1a 100644
--- a/source/armarx/control/skills/skills/aron/ShapeHandParams.xml
+++ b/source/armarx/control/skills/skills/aron/ShapeHandParams.xml
@@ -1,11 +1,11 @@
<?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>
+ <AronIncludes>
+ <Include include="armarx/control/skills/skills/aron/HandParam.xml" />
+ </AronIncludes>
+
+ <GenerateTypes>
<!-- Parameters to define when the skill should terminate (if waiting) -->
<Object name="::armarx::control::skills::skills::WaitUntilFinishedParams">
--
GitLab