From b87155a248dcc033b15badc6e379236b5bd418d2 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Fri, 23 Feb 2024 14:16:52 +0100
Subject: [PATCH] improving grasp trajectory / fixing bug regarding shape
---
.../GraspingUtility/GraspTrajectory.cpp | 88 ++++++++++++++++---
.../GraspingUtility/GraspTrajectory.h | 5 ++
2 files changed, 79 insertions(+), 14 deletions(-)
diff --git a/source/RobotAPI/libraries/GraspingUtility/GraspTrajectory.cpp b/source/RobotAPI/libraries/GraspingUtility/GraspTrajectory.cpp
index 7ab4f089a..1b4935111 100644
--- a/source/RobotAPI/libraries/GraspingUtility/GraspTrajectory.cpp
+++ b/source/RobotAPI/libraries/GraspingUtility/GraspTrajectory.cpp
@@ -34,9 +34,11 @@
#include "ArmarXCore/core/logging/Logging.h"
#include <ArmarXCore/core/exceptions/Exception.h>
+#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/rapidxml/wrapper/RapidXmlReader.h>
#include <ArmarXCore/core/rapidxml/wrapper/RapidXmlWriter.h>
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
+#include <ArmarXCore/core/time/Duration.h>
#include <ArmarXGui/libraries/StructuralJson/JPathNavigator.h>
#include <ArmarXGui/libraries/StructuralJson/StructuralJsonParser.h>
@@ -44,6 +46,8 @@
#include <RobotAPI/interface/units/GraspCandidateProviderInterface.h>
#include <RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLogger.h>
+#include <eigen3/Eigen/src/Core/util/Constants.h>
+
namespace armarx
{
@@ -138,6 +142,8 @@ namespace armarx
GraspTrajectoryPtr
GraspTrajectory::ReadFromFile(const std::string& filename)
{
+ ARMARX_IMPORTANT << filename;
+
ARMARX_TRACE;
const auto ext = std::filesystem::path(filename).extension();
if (ext == ".xml")
@@ -159,15 +165,37 @@ namespace armarx
inline void
from_json(const nlohmann::json& j, GraspTrajectoryKeypoint& kp)
{
- const std::optional<std::string> shape =
- j.contains("shape") ? std::optional<std::string>(j.at("shape").get<std::string>())
- : std::nullopt;
+ ARMARX_TRACE;
+ std::optional<std::string> shape;
- const std::map<std::string, float> targetHandValues =
- j.contains("HandValues") ? j.at("HandValues").get<std::map<std::string, float>>()
- : std::map<std::string, float>{};
+ if (j.contains("shape"))
+ {
+ if (not j.at("shape").is_null())
+ {
+ shape = j.at("shape");
+ }
+ }
+
+ ARMARX_TRACE;
+ std::optional<std::map<std::string, float>> targetHandValues;
- kp = GraspTrajectoryKeypoint(j.at("Pose"), targetHandValues, shape);
+ if (j.contains("fingerValues"))
+ {
+ if (not j.at("fingerValues").is_null())
+ {
+ targetHandValues = j.at("fingerValues");
+ }
+ }
+
+ // VERY hacky!
+ const Eigen::Matrix<float, 4, 4, Eigen::RowMajor> aronTf = j.at("pose");
+ const Eigen::Matrix4f tf = aronTf.transpose();
+
+ ARMARX_IMPORTANT << tf;
+
+ ARMARX_TRACE;
+ kp = GraspTrajectoryKeypoint(
+ tf, targetHandValues.value_or(std::map<std::string, float>{}), shape);
}
GraspTrajectoryPtr
@@ -179,9 +207,27 @@ namespace armarx
const nlohmann::json j = nlohmann::json::parse(ifs);
std::vector<GraspTrajectoryKeypoint> traj;
- traj = j;
+ traj = j.at("keypoints");
- return std::make_shared<GraspTrajectory>(traj);
+ const armarx::Duration duration =
+ armarx::Duration::MicroSeconds(j.at("duration").at("microSeconds").get<std::int64_t>());
+
+ // at the moment, we assume that all keypoints are sampled equidistant
+
+ ARMARX_CHECK_NOT_EMPTY(traj);
+ const float dtSeconds = duration.toSecondsDouble() / (traj.size() - 1);
+
+ for (auto& kp : traj)
+ {
+ kp.dt = dtSeconds;
+ }
+
+ const std::string frame = j.at("frame").at("frame");
+
+ auto graspTrajectory = std::make_shared<GraspTrajectory>(traj);
+ graspTrajectory->setFrame(frame);
+
+ return graspTrajectory;
}
GraspTrajectoryPtr
@@ -557,7 +603,8 @@ namespace armarx
traj->addKeypoint(
::math::Helpers::TranslateAndRotatePose(kp->getTargetPose(), translation, rotation),
kp->handJointsTarget,
- kp->dt);
+ kp->dt,
+ kp->shape);
}
return traj;
}
@@ -566,12 +613,13 @@ namespace armarx
GraspTrajectory::getTransformed(const Eigen::Matrix4f& transform) const
{
ARMARX_TRACE;
- GraspTrajectoryPtr traj(
- new GraspTrajectory(transform * getStartPose(), getKeypoint(0)->handJointsTarget));
+ GraspTrajectoryPtr traj(new GraspTrajectory(
+ transform * getStartPose(), getKeypoint(0)->handJointsTarget, getKeypoint(0)->shape));
for (size_t i = 1; i < keypoints.size(); i++)
{
const KeypointPtr& kp = keypoints.at(i);
- traj->addKeypoint(transform * kp->getTargetPose(), kp->handJointsTarget, kp->dt);
+ traj->addKeypoint(
+ transform * kp->getTargetPose(), kp->handJointsTarget, kp->dt, kp->shape);
}
return traj;
}
@@ -620,7 +668,7 @@ namespace armarx
const GraspTrajectory::KeypointPtr& kp = getKeypoint(i);
Eigen::Matrix4f target_pose = kp->getTargetPose();
target_pose.block<3, 3>(0, 0) = flip_yz * target_pose.block<3, 3>(0, 0) * flip_yz;
- output_trajectory->addKeypoint(target_pose, kp->handJointsTarget, kp->dt);
+ output_trajectory->addKeypoint(target_pose, kp->handJointsTarget, kp->dt, kp->shape);
}
return output_trajectory;
}
@@ -721,4 +769,16 @@ namespace armarx
feedForwardOriVelocity(0, 0, 0)
{
}
+
+ void
+ GraspTrajectory::setFrame(const std::string& frame)
+ {
+ frame_ = frame;
+ }
+
+ const std::optional<std::string>&
+ GraspTrajectory::getFrame() const
+ {
+ return frame_;
+ }
} // namespace armarx
diff --git a/source/RobotAPI/libraries/GraspingUtility/GraspTrajectory.h b/source/RobotAPI/libraries/GraspingUtility/GraspTrajectory.h
index 718642c2a..15786c619 100644
--- a/source/RobotAPI/libraries/GraspingUtility/GraspTrajectory.h
+++ b/source/RobotAPI/libraries/GraspingUtility/GraspTrajectory.h
@@ -175,11 +175,16 @@ namespace armarx
static GraspTrajectoryPtr ReadFromJSON(const std::string& filename);
static GraspTrajectoryPtr ReadFromString(const std::string& xml);
+ void setFrame(const std::string& frame);
+ const std::optional<std::string>& getFrame() const;
+
private:
void updateKeypointMap();
private:
std::vector<KeypointPtr> keypoints;
std::map<float, size_t> keypointMap;
+
+ std::optional<std::string> frame_;
};
} // namespace armarx
--
GitLab