Move Armar6GraspTrajectory to RobotAPI as GraspTrajectory and

deprecate old GraspTrajectory in diffik

source/RobotAPI/libraries/GraspingUtility/CMakeLists.txt

@@ -19,6 +19,7 @@ armarx_add_library(
              GraspCandidateWriter.cpp
              GraspCandidateReader.cpp
              GraspCandidateVisu.cpp
+             GraspTrajectory.cpp
 
     HEADERS  box_to_grasp_candidates.h
              box_to_grasp_candidates.ipp
@@ -30,6 +31,7 @@ armarx_add_library(
              GraspCandidateWriter.h
              GraspCandidateReader.h
              GraspCandidateVisu.h
+             GraspTrajectory.h
 )
 armarx_enable_aron_file_generation_for_target(
     TARGET_NAME

source/RobotAPI/libraries/GraspingUtility/GraspTrajectory.cpp

+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author     Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#include <VirtualRobot/math/Helpers.h>
+
+#include <ArmarXCore/core/exceptions/Exception.h>
+#include <ArmarXCore/core/rapidxml/wrapper/RapidXmlReader.h>
+#include <ArmarXCore/core/rapidxml/wrapper/RapidXmlWriter.h>
+#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
+
+#include <ArmarXGui/libraries/StructuralJson/JPathNavigator.h>
+#include <ArmarXGui/libraries/StructuralJson/StructuralJsonParser.h>
+
+#include <RobotAPI/interface/units/GraspCandidateProviderInterface.h>
+#include <RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLogger.h>
+
+#include "GraspTrajectory.h"
+
+namespace armarx
+{
+
+    void
+    GraspTrajectory::writeToFile(const std::string& filename)
+    {
+        RapidXmlWriter writer;
+        RapidXmlWriterNode root = writer.createRootNode("GraspTrajectory");
+        for (const KeypointPtr& keypoint : keypoints)
+        {
+            SimpleJsonLoggerEntry e;
+            e.Add("dt", keypoint->dt);
+            e.AddAsArr("Pose", keypoint->tcpTarget);
+            e.AddAsArr("HandValues", keypoint->handJointsTarget);
+            root.append_string_node("Keypoint", e.obj->toJsonString(0, "", true));
+        }
+        writer.saveToFile(filename, true);
+    }
+
+    GraspTrajectoryPtr
+    GraspTrajectory::ReadFromReader(const RapidXmlReaderPtr& reader)
+    {
+        RapidXmlReaderNode root = reader->getRoot("GraspTrajectory");
+        GraspTrajectoryPtr traj;
+        for (const RapidXmlReaderNode& kpNode : root.nodes("Keypoint"))
+        {
+            StructuralJsonParser p(kpNode.value());
+            p.parse();
+            JPathNavigator nav(p.parsedJson);
+            float dt = nav.selectSingleNode("dt").asFloat();
+
+            Eigen::Matrix4f pose;
+            std::vector<JPathNavigator> rows = nav.select("Pose/*");
+            for (int i = 0; i < 4; i++)
+            {
+                std::vector<JPathNavigator> cells = rows.at(i).select("*");
+                for (int j = 0; j < 4; j++)
+                {
+                    pose(i, j) = cells.at(j).asFloat();
+                }
+            }
+
+            Eigen::Vector3f handValues;
+            std::vector<JPathNavigator> cells = nav.select("HandValues/*");
+            for (int j = 0; j < 3; j++)
+            {
+                handValues(j) = cells.at(j).asFloat();
+            }
+
+            if (!traj)
+            {
+                traj = GraspTrajectoryPtr(new GraspTrajectory(pose, handValues));
+            }
+            else
+            {
+                traj->addKeypoint(pose, handValues, dt);
+            }
+        }
+        return traj;
+    }
+
+    GraspTrajectoryPtr
+    GraspTrajectory::ReadFromFile(const std::string& filename)
+    {
+        return ReadFromReader(RapidXmlReader::FromFile(filename));
+    }
+
+    GraspTrajectoryPtr
+    GraspTrajectory::ReadFromString(const std::string& xml)
+    {
+        return ReadFromReader(RapidXmlReader::FromXmlString(xml));
+    }
+
+    GraspTrajectory::GraspTrajectory(const Eigen::Matrix4f &tcpStart,
+                                                 const Eigen::Vector3f &handJointsStart) {
+        KeypointPtr keypoint(new Keypoint(tcpStart, handJointsStart));
+        keypointMap[0] = keypoints.size();
+        keypoints.push_back(keypoint);
+    }
+
+    void
+    GraspTrajectory::addKeypoint(const Eigen::Matrix4f &tcpTarget, const Eigen::Vector3f &handJointsTarget,
+                                            float dt) {
+        KeypointPtr prev = lastKeypoint();
+        KeypointPtr keypoint(new Keypoint(tcpTarget, handJointsTarget));
+        keypoint->updateVelocities(prev, dt);
+        float t = getDuration() + dt;
+        keypointMap[t] = keypoints.size();
+        keypoints.push_back(keypoint);
+    }
+
+    size_t
+    GraspTrajectory::getKeypointCount() const {
+        return keypoints.size();
+    }
+
+    void
+    GraspTrajectory::insertKeypoint(size_t index, const Eigen::Matrix4f &tcpTarget,
+                                               const Eigen::Vector3f &handJointsTarget, float dt) {
+        if (index <= 0 || index > keypoints.size())
+        {
+            throw LocalException("Index out of range" + std::to_string(index));
+        }
+        KeypointPtr prev = keypoints.at(index - 1);
+        KeypointPtr keypoint(new Keypoint(tcpTarget, handJointsTarget));
+        keypoint->updateVelocities(prev, dt);
+        if (index < keypoints.size())
+        {
+            KeypointPtr next = keypoints.at(index);
+            next->updateVelocities(keypoint, next->dt);
+        }
+        keypoints.insert(keypoints.begin() + index, keypoint);
+        updateKeypointMap();
+    }
+
+    void
+    GraspTrajectory::removeKeypoint(size_t index) {
+        if (index <= 0 || index >= keypoints.size())
+        {
+            throw LocalException("Index out of range" + std::to_string(index));
+        }
+        keypoints.erase(keypoints.begin() + index);
+        if (index < keypoints.size())
+        {
+            KeypointPtr prev = keypoints.at(index - 1);
+            KeypointPtr next = keypoints.at(index);
+            next->updateVelocities(prev, next->dt);
+        }
+        updateKeypointMap();
+    }
+
+    void
+    GraspTrajectory::replaceKeypoint(size_t index, const Eigen::Matrix4f &tcpTarget,
+                                                const Eigen::Vector3f &handJointsTarget, float dt) {
+        if (index <= 0 || index >= keypoints.size())
+        {
+            throw LocalException("Index out of range" + std::to_string(index));
+        }
+        KeypointPtr prev = keypoints.at(index - 1);
+        KeypointPtr keypoint(new Keypoint(tcpTarget, handJointsTarget));
+        keypoint->updateVelocities(prev, dt);
+        keypoints.at(index) = keypoint;
+        updateKeypointMap();
+    }
+
+    void
+    GraspTrajectory::setKeypointDt(size_t index, float dt) {
+        if (index <= 0 || index >= keypoints.size())
+        {
+            throw LocalException("Index out of range" + std::to_string(index));
+        }
+        KeypointPtr prev = keypoints.at(index - 1);
+        KeypointPtr& keypoint = keypoints.at(index);
+        keypoint->updateVelocities(prev, dt);
+        updateKeypointMap();
+    }
+
+    GraspTrajectory::KeypointPtr &
+    GraspTrajectory::lastKeypoint() {
+        return keypoints.at(keypoints.size() - 1);
+    }
+
+    GraspTrajectory::KeypointPtr &
+    GraspTrajectory::getKeypoint(int i) {
+        return keypoints.at(i);
+    }
+
+    Eigen::Matrix4f
+    GraspTrajectory::getStartPose() {
+        return getKeypoint(0)->getTargetPose();
+    }
+
+    void
+    GraspTrajectory::getIndex(float t, int &i1, int &i2, float &f) {
+        if (t <= 0)
+        {
+            i1 = 0;
+            i2 = 0;
+            f = 0;
+            return;
+        }
+        std::map<float, size_t>::const_iterator it, prev;
+        it = keypointMap.upper_bound(t);
+        if (it == keypointMap.end())
+        {
+            i1 = keypoints.size() - 1;
+            i2 = keypoints.size() - 1;
+            f = 0;
+            return;
+        }
+        prev = std::prev(it);
+        i1 = prev->second;
+        i2 = it->second;
+        f = ::math::Helpers::ILerp(prev->first, it->first, t);
+    }
+
+    Eigen::Vector3f
+    GraspTrajectory::GetPosition(float t) {
+        int i1, i2;
+        float f;
+        getIndex(t, i1, i2, f);
+        return ::math::Helpers::Lerp(getKeypoint(i1)->getTargetPosition(), getKeypoint(i2)->getTargetPosition(), f);
+    }
+
+    Eigen::Matrix3f
+    GraspTrajectory::GetOrientation(float t) {
+        int i1, i2;
+        float f;
+        getIndex(t, i1, i2, f);
+        Eigen::Vector3f oriVel = GetOrientationDerivative(t);
+        if (oriVel.squaredNorm() == 0)
+        {
+            return getKeypoint(i1)->getTargetOrientation();
+        }
+        Eigen::AngleAxisf aa(oriVel.norm(), oriVel.normalized());
+        aa.angle() = aa.angle() * f * getKeypoint(i2)->dt;
+        return aa.toRotationMatrix() * getKeypoint(i1)->getTargetOrientation();
+    }
+
+    Eigen::Matrix4f
+    GraspTrajectory::GetPose(float t) {
+        return ::math::Helpers::CreatePose(GetPosition(t), GetOrientation(t));
+    }
+
+    std::vector<Eigen::Matrix4f>
+    GraspTrajectory::getAllKeypointPoses() {
+        std::vector<Eigen::Matrix4f> res;
+        for (const KeypointPtr& keypoint : keypoints)
+        {
+            res.emplace_back(keypoint->getTargetPose());
+        }
+        return res;
+    }
+
+    std::vector<Eigen::Vector3f>
+    GraspTrajectory::getAllKeypointPositions() {
+        std::vector<Eigen::Vector3f> res;
+        for (const KeypointPtr& keypoint : keypoints)
+        {
+            res.emplace_back(keypoint->getTargetPosition());
+        }
+        return res;
+    }
+
+    std::vector<Eigen::Matrix3f>
+    GraspTrajectory::getAllKeypointOrientations() {
+        std::vector<Eigen::Matrix3f> res;
+        for (const KeypointPtr& keypoint : keypoints)
+        {
+            res.emplace_back(keypoint->getTargetOrientation());
+        }
+        return res;
+    }
+
+    Eigen::Vector3f
+    GraspTrajectory::GetHandValues(float t) {
+        int i1, i2;
+        float f;
+        getIndex(t, i1, i2, f);
+        return ::math::Helpers::Lerp(getKeypoint(i1)->handJointsTarget, getKeypoint(i2)->handJointsTarget, f);
+    }
+
+    Eigen::Vector3f
+    GraspTrajectory::GetPositionDerivative(float t) {
+        int i1, i2;
+        float f;
+        getIndex(t, i1, i2, f);
+        return getKeypoint(i2)->feedForwardPosVelocity;
+    }
+
+    Eigen::Vector3f
+    GraspTrajectory::GetOrientationDerivative(float t) {
+        int i1, i2;
+        float f;
+        getIndex(t, i1, i2, f);
+        return getKeypoint(i2)->feedForwardOriVelocity;
+    }
+
+    Eigen::Matrix<float, 6, 1>
+    GraspTrajectory::GetTcpDerivative(float t) {
+        Eigen::Matrix<float, 6, 1> ffVel;
+        ffVel.block<3, 1>(0, 0) = GetPositionDerivative(t);
+        ffVel.block<3, 1>(3, 0) = GetOrientationDerivative(t);
+        return ffVel;
+    }
+
+    Eigen::Vector3f
+    GraspTrajectory::GetHandJointsDerivative(float t) {
+        int i1, i2;
+        float f;
+        getIndex(t, i1, i2, f);
+        return getKeypoint(i2)->feedForwardHandJointsVelocity;
+    }
+
+    float
+    GraspTrajectory::getDuration() const {
+        return keypointMap.rbegin()->first;
+    }
+
+    GraspTrajectory::Length
+    GraspTrajectory::calculateLength() const {
+        Length l;
+        for (size_t i = 1; i < keypoints.size(); i++)
+        {
+            KeypointPtr k0 = keypoints.at(i - 1);
+            KeypointPtr k1 = keypoints.at(i);
+
+            Eigen::Vector3f pos0 = ::math::Helpers::GetPosition(k0->tcpTarget);
+            Eigen::Matrix3f ori0 = ::math::Helpers::GetOrientation(k0->tcpTarget);
+
+            Eigen::Vector3f pos1 = ::math::Helpers::GetPosition(k1->tcpTarget);
+            Eigen::Matrix3f ori1 = ::math::Helpers::GetOrientation(k1->tcpTarget);
+
+            l.pos += (pos1 - pos0).norm();
+            l.ori += fabs(::math::Helpers::GetAngleAxisFromTo(ori0, ori1).angle());
+        }
+        return l;
+    }
+
+    GraspTrajectoryPtr
+    GraspTrajectory::getTranslatedAndRotated(const Eigen::Vector3f &translation, const Eigen::Matrix3f &rotation) {
+        GraspTrajectoryPtr traj(new GraspTrajectory(::math::Helpers::TranslateAndRotatePose(getKeypoint(0)->getTargetPose(), translation, rotation), getKeypoint(0)->handJointsTarget));
+        for (size_t i = 1; i < keypoints.size(); i++)
+        {
+            KeypointPtr& kp = keypoints.at(i);
+            traj->addKeypoint(::math::Helpers::TranslateAndRotatePose(kp->getTargetPose(), translation, rotation), kp->handJointsTarget, kp->dt);
+        }
+        return traj;
+    }
+
+    GraspTrajectoryPtr
+    GraspTrajectory::getTransformed(const Eigen::Matrix4f &transform) {
+        GraspTrajectoryPtr traj(new GraspTrajectory(transform * getStartPose(), getKeypoint(0)->handJointsTarget));
+        for (size_t i = 1; i < keypoints.size(); i++)
+        {
+            KeypointPtr& kp = keypoints.at(i);
+            traj->addKeypoint(transform * kp->getTargetPose(), kp->handJointsTarget, kp->dt);
+        }
+        return traj;
+    }
+
+    GraspTrajectoryPtr
+    GraspTrajectory::getClone() {
+        return getTransformed(Eigen::Matrix4f::Identity());
+    }
+
+    GraspTrajectoryPtr
+    GraspTrajectory::getTransformedToGraspPose(const Eigen::Matrix4f &target, const Eigen::Vector3f &handForward) {
+        Eigen::Matrix4f startPose = getStartPose();
+        Eigen::Vector3f targetHandForward = ::math::Helpers::TransformDirection(target, handForward);
+        Eigen::Vector3f trajHandForward = ::math::Helpers::TransformDirection(startPose, handForward);
+        Eigen::Vector3f up(0, 0, 1);
+
+        float angle = ::math::Helpers::Angle(targetHandForward, trajHandForward, up);
+        Eigen::AngleAxisf aa(angle, up);
+
+        Eigen::Matrix4f transform = ::math::Helpers::CreateTranslationRotationTranslationPose(-::math::Helpers::GetPosition(startPose), aa.toRotationMatrix(), ::math::Helpers::GetPosition(target));
+        return getTransformed(transform);
+    }
+
+    GraspTrajectoryPtr
+    GraspTrajectory::getTransformedToOtherHand() {
+        Eigen::Matrix3f flip_yz = Eigen::Matrix3f::Identity();
+        flip_yz(0, 0) *= -1.0;
+        Eigen::Matrix4f start_pose = getStartPose();
+        start_pose.block<3, 3>(0, 0) = flip_yz * start_pose.block<3, 3>(0, 0) * flip_yz;
+        GraspTrajectoryPtr output_trajectory(
+                new GraspTrajectory(start_pose, getKeypoint(0)->handJointsTarget));
+        for (size_t i = 1; i < getKeypointCount(); i++)
+        {
+            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);
+        }
+        return output_trajectory;
+    }
+
+    SimpleDiffIK::Reachability
+    GraspTrajectory::calculateReachability(VirtualRobot::RobotNodeSetPtr rns, VirtualRobot::RobotNodePtr tcp,
+                                                 SimpleDiffIK::Parameters params) {
+        return SimpleDiffIK::CalculateReachability(getAllKeypointPoses(), Eigen::VectorXf::Zero(rns->getSize()), rns, tcp, params);
+    }
+
+    void
+    GraspTrajectory::updateKeypointMap() {
+        keypointMap.clear();
+        float t = 0;
+        for (size_t i = 0; i < keypoints.size(); i++)
+        {
+            t += getKeypoint(i)->dt;
+            keypointMap[t] = i;
+        }
+    }
+
+
+
+    void
+    GraspTrajectory::Keypoint::updateVelocities(const GraspTrajectory::KeypointPtr &prev, float dt) {
+        Eigen::Vector3f pos0 = ::math::Helpers::GetPosition(prev->tcpTarget);
+        Eigen::Matrix3f ori0 = ::math::Helpers::GetOrientation(prev->tcpTarget);
+        Eigen::Vector3f hnd0 = prev->handJointsTarget;
+
+        Eigen::Vector3f pos1 = ::math::Helpers::GetPosition(tcpTarget);
+        Eigen::Matrix3f ori1 = ::math::Helpers::GetOrientation(tcpTarget);
+        Eigen::Vector3f hnd1 = handJointsTarget;
+
+        Eigen::Vector3f dpos = pos1 - pos0;
+        Eigen::Vector3f dori = ::math::Helpers::GetRotationVector(ori0, ori1);
+        Eigen::Vector3f dhnd = hnd1 - hnd0;
+
+        this->dt = dt;
+        feedForwardPosVelocity = dpos / dt;
+        feedForwardOriVelocity = dori / dt;
+        feedForwardHandJointsVelocity = dhnd / dt;
+    }
+
+    Eigen::Vector3f
+    GraspTrajectory::Keypoint::getTargetPosition() const {
+        return ::math::Helpers::GetPosition(tcpTarget);
+    }
+
+    Eigen::Matrix3f
+    GraspTrajectory::Keypoint::getTargetOrientation() const {
+        return ::math::Helpers::GetOrientation(tcpTarget);
+    }
+
+    Eigen::Matrix4f
+    GraspTrajectory::Keypoint::getTargetPose() const {
+        return tcpTarget;
+    }
+
+    GraspTrajectory::Keypoint::Keypoint(const Eigen::Matrix4f &tcpTarget, const Eigen::Vector3f &handJointsTarget,
+                                              float dt, const Eigen::Vector3f &feedForwardPosVelocity,
+                                              const Eigen::Vector3f &feedForwardOriVelocity,
+                                              const Eigen::Vector3f &feedForwardHandJointsVelocity)
+            : tcpTarget(tcpTarget), handJointsTarget(handJointsTarget), dt(dt),
+              feedForwardPosVelocity(feedForwardPosVelocity), feedForwardOriVelocity(feedForwardOriVelocity),
+              feedForwardHandJointsVelocity(feedForwardHandJointsVelocity)
+    { }
+
+    GraspTrajectory::Keypoint::Keypoint(const Eigen::Matrix4f &tcpTarget, const Eigen::Vector3f &handJointsTarget)
+            : tcpTarget(tcpTarget), handJointsTarget(handJointsTarget), dt(0),
+              feedForwardPosVelocity(0, 0, 0), feedForwardOriVelocity(0, 0, 0), feedForwardHandJointsVelocity(0, 0, 0)
+    { }
+}

source/RobotAPI/libraries/GraspingUtility/GraspTrajectory.h

+/*
+ * This file is part of ArmarX.
+ *
+ * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T),
+ * Karlsruhe Institute of Technology (KIT), all rights reserved.
+ *
+ * ArmarX is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * ArmarX is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * @author     Simon Ottenhaus (simon dot ottenhaus at kit dot edu)
+ * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
+ *             GNU General Public License
+ */
+
+#pragma once
+
+#include <Eigen/Core>
+#include <memory>
+#include <map>
+#include <VirtualRobot/VirtualRobot.h>
+#include <RobotAPI/libraries/diffik/SimpleDiffIK.h>
+#include <RobotAPI/interface/units/GraspCandidateProviderInterface.h>
+
+namespace armarx
+{
+    class RapidXmlReader;
+    using RapidXmlReaderPtr = std::shared_ptr<RapidXmlReader>;
+
+    class GraspTrajectory;
+    typedef std::shared_ptr<GraspTrajectory> GraspTrajectoryPtr;
+
+    class GraspTrajectory
+    {
+    public:
+        class Keypoint;
+        typedef std::shared_ptr<Keypoint> KeypointPtr;
+
+        class Keypoint
+        {
+        public:
+            Eigen::Matrix4f tcpTarget;
+            Eigen::Vector3f 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::Vector3f getTargetPosition() const;
+            Eigen::Matrix3f getTargetOrientation() const;
+            Eigen::Matrix4f getTargetPose() const;
+            void updateVelocities(const KeypointPtr& prev, float dt);
+        };
+
+        struct Length
+        {
+            float pos = 0;
+            float ori = 0;
+        };
+
+
+    public:
+        GraspTrajectory() = default;
+        GraspTrajectory(const Eigen::Matrix4f& tcpStart, const Eigen::Vector3f& handJointsStart);
+
+        void addKeypoint(const Eigen::Matrix4f& tcpTarget, const Eigen::Vector3f& handJointsTarget, float dt);
+
+        size_t getKeypointCount() const;
+
+        void insertKeypoint(size_t index, const Eigen::Matrix4f& tcpTarget, const Eigen::Vector3f& handJointsTarget, float dt);
+
+        void removeKeypoint(size_t index);
+
+        void replaceKeypoint(size_t index, const Eigen::Matrix4f& tcpTarget, const Eigen::Vector3f& handJointsTarget, float dt);
+
+        void setKeypointDt(size_t index, float dt);
+
+        KeypointPtr& lastKeypoint();
+        KeypointPtr& getKeypoint(int i);
+        Eigen::Matrix4f getStartPose();
+
+        void getIndex(float t, int& i1, int& i2, float& f);
+
+        Eigen::Vector3f GetPosition(float t);
+
+        Eigen::Matrix3f GetOrientation(float t);
+
+        Eigen::Matrix4f GetPose(float t);
+
+        std::vector<Eigen::Matrix4f> getAllKeypointPoses();
+        std::vector<Eigen::Vector3f> getAllKeypointPositions();
+        std::vector<Eigen::Matrix3f> getAllKeypointOrientations();
+
+        Eigen::Vector3f GetHandValues(float t);
+
+        Eigen::Vector3f GetPositionDerivative(float t);
+
+        Eigen::Vector3f GetOrientationDerivative(float t);
+
+        Eigen::Matrix<float, 6, 1> GetTcpDerivative(float t);
+
+        Eigen::Vector3f GetHandJointsDerivative(float t);
+
+        float getDuration() const;
+
+        Length calculateLength() const;
+
+
+        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());
+
+
+
+
+        void writeToFile(const std::string& filename);
+
+        static GraspTrajectoryPtr ReadFromReader(const RapidXmlReaderPtr& reader);
+        static GraspTrajectoryPtr ReadFromFile(const std::string& filename);
+        static GraspTrajectoryPtr ReadFromString(const std::string& xml);
+
+    private:
+
+        void updateKeypointMap();
+
+    private:
+        std::vector<KeypointPtr> keypoints;
+        std::map<float, size_t> keypointMap;
+
+
+    };
+}

source/RobotAPI/libraries/diffik/CMakeLists.txt

@@ -13,16 +13,16 @@ set(LIBS
 set(LIB_FILES
     NaturalDiffIK.cpp
     SimpleDiffIK.cpp
-    RobotPlacement.cpp
-    GraspTrajectory.cpp
+#    RobotPlacement.cpp
+#    GraspTrajectory.cpp
     CompositeDiffIK.cpp
 )
 set(LIB_HEADERS
     NaturalDiffIK.h
     SimpleDiffIK.h
     DiffIKProvider.h
-    RobotPlacement.h
-    GraspTrajectory.h
+#    RobotPlacement.h
+#    GraspTrajectory.h
     CompositeDiffIK.h
 )