From 83474b9c16787dbed7ce06c5169f3cc7fa4759fa Mon Sep 17 00:00:00 2001
From: Jianfeng Gao <jianfeng.gao@kit.edu>
Date: Mon, 4 Dec 2023 14:00:06 +0100
Subject: [PATCH] update default pose for retrieve hand
---
.../retrieve_hand/skills/RetrieveHand.cpp | 68 +++++++++----------
1 file changed, 32 insertions(+), 36 deletions(-)
diff --git a/source/armarx/control/retrieve_hand/skills/RetrieveHand.cpp b/source/armarx/control/retrieve_hand/skills/RetrieveHand.cpp
index a440b9b0..fd563b07 100644
--- a/source/armarx/control/retrieve_hand/skills/RetrieveHand.cpp
+++ b/source/armarx/control/retrieve_hand/skills/RetrieveHand.cpp
@@ -2,8 +2,8 @@
#include <ArmarXCore/core/logging/Logging.h>
-#include <armarx/control/retrieve_hand/constants/constants.h>
#include <armarx/control/deprecated_njoint_mp_controller/task_space/ControllerInterface.h>
+#include <armarx/control/retrieve_hand/constants/constants.h>
namespace armarx::control::retrieve_hand::skills
{
@@ -13,34 +13,33 @@ namespace armarx::control::retrieve_hand::skills
{
ParamType defaultParams;
defaultParams.targetPose.header.agent = "Armar7";
- defaultParams.targetPose.position = Eigen::Matrix<float, 3, 1> {-203.5f, 269.00f, 1130.0f};
- defaultParams.targetPose.orientation = Eigen::Quaternion<float> {
- 0.741636, -0.6635227, 0.05756, -0.079999};
+ defaultParams.targetPose.header.frame = "root";
+ defaultParams.targetPose.position = Eigen::Matrix<float, 3, 1>{-234.0f, 284.06f, 1103.0f};
+ defaultParams.targetPose.orientation =
+ Eigen::Quaternion<float>{0.7640368, -0.63349, 0.06744281, -0.10187};
// ARMARX_INFO << VAROUT(defaultParams.targetPose);
// ARMARX_INFO << VAROUT(defaultParams.targetPose.position) << VAROUT(defaultParams.targetPose.orientation);
// ARMARX_INFO << VAROUT(defaultParams.targetPose.header.frame) << VAROUT(defaultParams.targetPose.header.agent);
// defaultParams.exampleStringParam = "exampleDefaultValue";
- auto skillId = armarx::skills::SkillID{.skillName = armarx::control::retrieve_hand::constants::SKILL_NAME};
+ auto skillId = armarx::skills::SkillID{
+ .skillName = armarx::control::retrieve_hand::constants::SKILL_NAME};
- return ::armarx::skills::SkillDescription{
- .skillId = skillId,
- .description = "TODO: Description of skill RetrieveHand.",
- .rootProfileDefaults = defaultParams.toAron(),
- .timeout = ::armarx::Duration::MilliSeconds(1000),
- .parametersType = ParamType::ToAronType()};
+ return ::armarx::skills::SkillDescription{.skillId = skillId,
+ .description =
+ "TODO: Description of skill RetrieveHand.",
+ .rootProfileDefaults = defaultParams.toAron(),
+ .timeout = ::armarx::Duration::MilliSeconds(1000),
+ .parametersType = ParamType::ToAronType()};
}
- RetrieveHand::RetrieveHand(const Remote& r) :
- Base(GetSkillDescription()), remote(r)
+ RetrieveHand::RetrieveHand(const Remote& r) : Base(GetSkillDescription()), remote(r)
{
}
-
- RetrieveHand::RetrieveHand() :
- Base(GetSkillDescription())
+ RetrieveHand::RetrieveHand() : Base(GetSkillDescription())
{
}
@@ -57,26 +56,23 @@ namespace armarx::control::retrieve_hand::skills
RetrieveHand::main(const SpecializedMainInput& in)
{
// Enter main code of the skill here.
- core::RetrieveHand::Remote rem{
- .memoryNameSystem = remote.memoryNameSystem,
- .robotUnit = remote.robotUnit};
-
- armarx::FramedPose targetPose {
- in.parameters.targetPose.position,
- in.parameters.targetPose.orientation,
- in.parameters.targetPose.header.frame,
- in.parameters.targetPose.header.agent
- };
+ core::RetrieveHand::Remote rem{.memoryNameSystem = remote.memoryNameSystem,
+ .robotUnit = remote.robotUnit};
+
+ armarx::FramedPose targetPose{in.parameters.targetPose.position,
+ in.parameters.targetPose.orientation,
+ in.parameters.targetPose.header.frame,
+ in.parameters.targetPose.header.agent};
core::RetrieveHand::Properties props{
- .robotName = in.parameters.robotName,
- .robotNodeSet = in.parameters.robotNodeSet,
- .timeDurationInSec = in.parameters.timeDurationInSec,
- .targetPose = targetPose,
- .kpLinear = in.parameters.kpLinear,
- .kpAngular = in.parameters.kpAngular,
- .kdLinear = in.parameters.kdLinear,
- .kdAngular = in.parameters.kdAngular,
- .fileNames = in.parameters.fileNames,
+ .robotName = in.parameters.robotName,
+ .robotNodeSet = in.parameters.robotNodeSet,
+ .timeDurationInSec = in.parameters.timeDurationInSec,
+ .targetPose = targetPose,
+ .kpLinear = in.parameters.kpLinear,
+ .kpAngular = in.parameters.kpAngular,
+ .kdLinear = in.parameters.kdLinear,
+ .kdAngular = in.parameters.kdAngular,
+ .fileNames = in.parameters.fileNames,
};
@@ -98,4 +94,4 @@ namespace armarx::control::retrieve_hand::skills
}
*/
-}
+} // namespace armarx::control::retrieve_hand::skills
--
GitLab