Merge branch 'master' of git.h2t.iar.kit.edu:sw/armarx/robot-api

source/RobotAPI/libraries/GraspingUtility/GraspCandidateReader.h

@@ -16,31 +16,31 @@ namespace armarx::armem
 
         void connect(bool use = true);
 
-        grasping::GraspCandidatePtr
+        ::armarx::grasping::GraspCandidatePtr
         queryGraspCandidateInstanceByID(armem::MemoryID const& id) const;
 
-        grasping::GraspCandidateDict
+        ::armarx::grasping::GraspCandidateDict
         queryGraspCandidateInstancesByID(std::vector<armem::MemoryID> const& ids) const;
 
-        grasping::BimanualGraspCandidatePtr
+        ::armarx::grasping::BimanualGraspCandidatePtr
         queryBimanualGraspCandidateInstanceByID(armem::MemoryID const& id) const;
 
-        grasping::GraspCandidateDict
+        ::armarx::grasping::GraspCandidateDict
         queryLatestGraspCandidateEntity(std::string const& provider,
                                         std::string const& entity) const;
 
-        std::map<std::string, grasping::BimanualGraspCandidatePtr>
+        std::map<std::string, ::armarx::grasping::BimanualGraspCandidatePtr>
         queryLatestBimanualGraspCandidateEntity(std::string const& provider,
                                                 std::string const& entity) const;
 
-        grasping::GraspCandidateDict
+        ::armarx::grasping::GraspCandidateDict
         queryLatestGraspCandidates(std::string const& provider = "") const;
 
-        grasping::GraspCandidateDict queryGraspCandidatesNewerThan(
+        ::armarx::grasping::GraspCandidateDict queryGraspCandidatesNewerThan(
             std::string const& provider = "",
             const armarx::DateTime& timestamp = armarx::DateTime::Now()) const;
 
-        std::map<std::string, grasping::BimanualGraspCandidatePtr>
+        std::map<std::string, ::armarx::grasping::BimanualGraspCandidatePtr>
         queryLatestBimanualGraspCandidates(std::string const& provider = "") const;
 
 
@@ -48,10 +48,10 @@ namespace armarx::armem
 
 
     private:
-        grasping::GraspCandidateDict
+        ::armarx::grasping::GraspCandidateDict
         getGraspCandidatesFromResultSet(armem::client::QueryResult const& qResult) const;
 
-        std::map<std::string, grasping::BimanualGraspCandidatePtr>
+        std::map<std::string, ::armarx::grasping::BimanualGraspCandidatePtr>
         getBimanualGraspCandidatesFromResultSet(armem::client::QueryResult const& qResult) const;
 
         armem::client::Reader memoryReader;

source/RobotAPI/libraries/GraspingUtility/GraspTrajectory.h

@@ -23,12 +23,15 @@
 
 #pragma once
 
-#include <Eigen/Core>
-#include <memory>
 #include <map>
+#include <memory>
+
+#include <Eigen/Core>
+
 #include <VirtualRobot/VirtualRobot.h>
-#include <RobotAPI/libraries/diffik/SimpleDiffIK.h>
+
 #include <RobotAPI/interface/units/GraspCandidateProviderInterface.h>
+#include <RobotAPI/libraries/diffik/SimpleDiffIK.h>
 
 namespace armarx
 {
@@ -48,20 +51,23 @@ namespace armarx
         {
         public:
             Eigen::Matrix4f tcpTarget;
-            Eigen::Vector3f handJointsTarget;
+            armarx::NameValueMap handJointsTarget;
             float dt;
             Eigen::Vector3f feedForwardPosVelocity;
             Eigen::Vector3f feedForwardOriVelocity;
-            Eigen::Vector3f feedForwardHandJointsVelocity;
-
-            Keypoint(const Eigen::Matrix4f& tcpTarget, const Eigen::Vector3f& handJointsTarget);
-            Keypoint(const Eigen::Matrix4f& tcpTarget, const Eigen::Vector3f& handJointsTarget, float dt,
-                     const Eigen::Vector3f& feedForwardPosVelocity, const Eigen::Vector3f& feedForwardOriVelocity
-                     , const Eigen::Vector3f& feedForwardHandJointsVelocity);
+            Eigen::VectorXf feedForwardHandJointsVelocity;
+
+            Keypoint(const Eigen::Matrix4f& tcpTarget, const armarx::NameValueMap& handJointsTarget);
+            Keypoint(const Eigen::Matrix4f& tcpTarget,
+                     const armarx::NameValueMap& handJointsTarget,
+                     float dt,
+                     const Eigen::Vector3f& feedForwardPosVelocity,
+                     const Eigen::Vector3f& feedForwardOriVelocity,
+                     const Eigen::VectorXf& feedForwardHandJointsVelocity);
             Eigen::Vector3f getTargetPosition() const;
             Eigen::Matrix3f getTargetOrientation() const;
             Eigen::Matrix4f getTargetPose() const;
-            void updateVelocities(const KeypointPtr& prev, float dt);
+            void updateVelocities(const KeypointPtr& prev, float deltat);
         };
 
         struct Length
@@ -73,17 +79,26 @@ namespace armarx
 
     public:
         GraspTrajectory() = default;
-        GraspTrajectory(const Eigen::Matrix4f& tcpStart, const Eigen::Vector3f& handJointsStart);
+        GraspTrajectory(const Eigen::Matrix4f& tcpStart,
+                        const armarx::NameValueMap& handJointsStart);
 
-        void addKeypoint(const Eigen::Matrix4f& tcpTarget, const Eigen::Vector3f& handJointsTarget, float dt);
+        void addKeypoint(const Eigen::Matrix4f& tcpTarget,
+                         const armarx::NameValueMap& handJointsTarget,
+                         float dt);
 
         size_t getKeypointCount() const;
 
-        void insertKeypoint(size_t index, const Eigen::Matrix4f& tcpTarget, const Eigen::Vector3f& handJointsTarget, float dt);
+        void insertKeypoint(size_t index,
+                            const Eigen::Matrix4f& tcpTarget,
+                            const armarx::NameValueMap& handJointsTarget,
+                            float dt);
 
         void removeKeypoint(size_t index);
 
-        void replaceKeypoint(size_t index, const Eigen::Matrix4f& tcpTarget, const Eigen::Vector3f& handJointsTarget, float dt);
+        void replaceKeypoint(size_t index,
+                             const Eigen::Matrix4f& tcpTarget,
+                             const armarx::NameValueMap& handJointsTarget,
+                             float dt);
 
         void setKeypointDt(size_t index, float dt);
 
@@ -103,7 +118,7 @@ namespace armarx
         std::vector<Eigen::Vector3f> getAllKeypointPositions();
         std::vector<Eigen::Matrix3f> getAllKeypointOrientations();
 
-        Eigen::Vector3f GetHandValues(float t);
+        armarx::NameValueMap GetHandValues(float t);
 
         Eigen::Vector3f GetPositionDerivative(float t);
 
@@ -118,18 +133,21 @@ namespace armarx
         Length calculateLength() const;
 
 
-        GraspTrajectoryPtr getTranslatedAndRotated(const Eigen::Vector3f& translation, const Eigen::Matrix3f& rotation);
+        GraspTrajectoryPtr getTranslatedAndRotated(const Eigen::Vector3f& translation,
+                                                   const Eigen::Matrix3f& rotation);
         GraspTrajectoryPtr getTransformed(const Eigen::Matrix4f& transform);
         GraspTrajectoryPtr getTransformedToOtherHand();
 
         GraspTrajectoryPtr getClone();
 
         GraspTrajectoryPtr
-        getTransformedToGraspPose(const Eigen::Matrix4f& target, const Eigen::Vector3f& handForward = Eigen::Vector3f::UnitZ());
-
-        SimpleDiffIK::Reachability calculateReachability(VirtualRobot::RobotNodeSetPtr rns, VirtualRobot::RobotNodePtr tcp = VirtualRobot::RobotNodePtr(), SimpleDiffIK::Parameters params = SimpleDiffIK::Parameters());
-
+        getTransformedToGraspPose(const Eigen::Matrix4f& target,
+                                  const Eigen::Vector3f& handForward = Eigen::Vector3f::UnitZ());
 
+        SimpleDiffIK::Reachability
+        calculateReachability(VirtualRobot::RobotNodeSetPtr rns,
+                              VirtualRobot::RobotNodePtr tcp = VirtualRobot::RobotNodePtr(),
+                              SimpleDiffIK::Parameters params = SimpleDiffIK::Parameters());
 
 
         void writeToFile(const std::string& filename);
@@ -139,13 +157,10 @@ namespace armarx
         static GraspTrajectoryPtr ReadFromString(const std::string& xml);
 
     private:
-
         void updateKeypointMap();
 
     private:
         std::vector<KeypointPtr> keypoints;
         std::map<float, size_t> keypointMap;
-
-
     };
-}
+} // namespace armarx