Feature/end effector trajectories

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,7 @@
 #include <RobotAPI/interface/units/GraspCandidateProviderInterface.h>
 #include <RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLogger.h>
 
+
 namespace armarx
 {
 
@@ -138,6 +141,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 +164,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 +206,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 +602,8 @@ namespace armarx
             traj->addKeypoint(
                 ::math::Helpers::TranslateAndRotatePose(kp->getTargetPose(), translation, rotation),
                 kp->handJointsTarget,
-                kp->dt);
+                kp->dt,
+                kp->shape);
         }
         return traj;
     }
@@ -566,12 +612,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 +667,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 +768,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