diff --git a/source/armarx/control/retrieve_hand/aron/RetrieveHandParams.xml b/source/armarx/control/retrieve_hand/aron/RetrieveHandParams.xml index 148dafe44f7b8d49bc72a4cb549fcf3d702d0d98..30c32edc8a452d4e90d576af0bdd6eb6ca00b8a5 100644 --- a/source/armarx/control/retrieve_hand/aron/RetrieveHandParams.xml +++ b/source/armarx/control/retrieve_hand/aron/RetrieveHandParams.xml @@ -1,41 +1,41 @@ <?xml version="1.0" encoding="UTF-8" ?> <AronTypeDefinition> -<!-- <AronIncludes> - <Include include="<armarx/control/common/mp/aron/MPConfig.xml>" autoinclude="true" /> - </AronIncludes>--> + <AronIncludes> + <Include include="RobotAPI/libraries/aron/common/aron/framed.xml" /> + </AronIncludes> <GenerateTypes> <Object name="::armarx::control::retrieve_hand::skills::RetrieveHandParams"> <ObjectChild key="robotName"> - <String /> + <String default="Armar7"/> </ObjectChild> <ObjectChild key="targetPose"> - <Matrix4f /> + <armarx::arondto::FramedPositionAndOrientation /> </ObjectChild> <ObjectChild key="robotNodeSet"> - <String /> + <String default="LeftArm"/> </ObjectChild> <ObjectChild key="timeDurationInSec"> - <Double /> + <Double default="1.0" /> </ObjectChild> <ObjectChild key="kpLinear"> - <Float /> + <Float default="10.0" /> </ObjectChild> <ObjectChild key="kpAngular"> - <Float /> + <Float default="8.0" /> </ObjectChild> <ObjectChild key="kdLinear"> - <Float /> + <Float default="1.0" /> </ObjectChild> <ObjectChild key="kdAngular"> - <Float /> + <Float default="1.0" /> </ObjectChild> <ObjectChild key="fileNames"> diff --git a/source/armarx/control/retrieve_hand/core/RetrieveHand.cpp b/source/armarx/control/retrieve_hand/core/RetrieveHand.cpp index 274a70401aaa03ab1da4f7c4126836df5c4c1894..f6225294260f34cc4f76023e9f3146b36b759840 100644 --- a/source/armarx/control/retrieve_hand/core/RetrieveHand.cpp +++ b/source/armarx/control/retrieve_hand/core/RetrieveHand.cpp @@ -97,11 +97,15 @@ namespace armarx::control::retrieve_hand::core robotReader.connect(remote.memoryNameSystem); auto now = armarx::core::time::DateTime::Now(); + // auto r = robotReader.getSynchronizedRobot(properties.robotName, now); + // ARMARX_CHECK(r.get()); std::vector<double> goals; + // Eigen::Matrix4f targetPoseInRoot = properties.targetPose.toRootFrame(r)->toEigen(); + Eigen::Matrix4f targetPoseInRoot = properties.targetPose.toEigen(); // Eigen::Matrix4f currentPose = r->getRobotNodeSet(properties.robotNodeSet)->getTCP()->getPoseInRootFrame(); - goals = armarx::control::common::mat4ToDVec(properties.targetPose); - ARMARX_INFO << "Running VMP with goal " << VAROUT(properties.targetPose) << ", vec: " << armarx::control::common::dVecToString(goals); + goals = armarx::control::common::mat4ToDVec(targetPoseInRoot); + ARMARX_INFO << "Running VMP with goal \n" << VAROUT(targetPoseInRoot) << ", vec: \n" << armarx::control::common::dVecToString(goals); // if (in.isViaPoseListSet() && in.isViaPoseCanValSet() && in.getViaPoseList().size() == in.getViaPoseCanVal().size()) // { diff --git a/source/armarx/control/retrieve_hand/core/RetrieveHand.h b/source/armarx/control/retrieve_hand/core/RetrieveHand.h index a3da0e31fbe78c2e3716b547698bb6900eb2df87..a39f4a7ceece9a1e381f267c5d4a57770cd563f9 100644 --- a/source/armarx/control/retrieve_hand/core/RetrieveHand.h +++ b/source/armarx/control/retrieve_hand/core/RetrieveHand.h @@ -58,7 +58,8 @@ namespace armarx::control::retrieve_hand::core std::string robotName; std::string robotNodeSet; double timeDurationInSec; - Eigen::Matrix4f targetPose; + // Eigen::Matrix4f targetPoseInRoot; + armarx::FramedPose targetPose; float kpLinear = 10.0f; float kpAngular = 10.0f; float kdLinear = 1.0f; diff --git a/source/armarx/control/retrieve_hand/skills/RetrieveHand.cpp b/source/armarx/control/retrieve_hand/skills/RetrieveHand.cpp index 2aeed21478dab22bdf159787467afb340f10295c..a440b9b0d2e37d208eda2faee8b3b4268bc7766e 100644 --- a/source/armarx/control/retrieve_hand/skills/RetrieveHand.cpp +++ b/source/armarx/control/retrieve_hand/skills/RetrieveHand.cpp @@ -1,5 +1,7 @@ #include "RetrieveHand.h" +#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> @@ -10,6 +12,14 @@ namespace armarx::control::retrieve_hand::skills RetrieveHand::GetSkillDescription() { 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}; + // 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}; @@ -51,11 +61,17 @@ namespace armarx::control::retrieve_hand::skills .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 = in.parameters.targetPose, + .targetPose = targetPose, .kpLinear = in.parameters.kpLinear, .kpAngular = in.parameters.kpAngular, .kdLinear = in.parameters.kdLinear,