GraspTrajectory.cpp

Go to the documentation of this file.

/*
 * 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 "GraspTrajectory.h"
 
#include <filesystem>
#include <fstream>
#include <memory>
#include <optional>
 
#include <SimoxUtility/json/eigen_conversion.h>
#include <SimoxUtility/json/json.hpp>
#include <VirtualRobot/RobotNodeSet.h>
#include <VirtualRobot/math/Helpers.h>
 
#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>
 
#include <RobotAPI/interface/units/GraspCandidateProviderInterface.h>
#include <RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLogger.h>
 
namespace armarx
{
 
    Eigen::VectorXf
    mapValuesToVector(const armarx::NameValueMap& map)
    {
        ARMARX_TRACE;
        Eigen::VectorXf vector(map.size());
        std::transform(
            map.begin(), map.end(), vector.data(), [](const auto& item) { return item.second; });
        return vector;
    }
 
    void
    GraspTrajectory::writeToFile(const std::string& filename)
    {
        ARMARX_TRACE;
        RapidXmlWriter writer;
        RapidXmlWriterNode root = writer.createRootNode("GraspTrajectory");
        for (const KeypointPtr& keypoint : keypoints)
        {
            SimpleJsonLoggerEntry e;
            e.Add("dt", keypoint->dt);
            e.AddAsArr("Pose", keypoint->tcpTarget);
            JsonObjectPtr const obj = std::make_shared<JsonObject>();
            for (const auto& [name, value] : keypoint->handJointsTarget)
            {
                obj->add(name, JsonValue::Create(value));
            }
            e.obj->add("HandValues", obj);
            root.append_string_node("Keypoint", e.obj->toJsonString(0, "", true));
        }
        writer.saveToFile(filename, true);
    }
 
    GraspTrajectoryPtr
    GraspTrajectory::ReadFromReader(const RapidXmlReaderPtr& reader)
    {
        ARMARX_TRACE;
        RapidXmlReaderNode const root = reader->getRoot();
 
        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();
                }
            }
 
            armarx::NameValueMap handValues;
            std::vector<JPathNavigator> cells = nav.select("HandValues/*");
            for (const auto& cell : cells)
            {
                handValues[cell.getKey()] = cell.asFloat();
            }
 
            // does not work at the moment
            const auto shapeNode = nav.selectSingleNode("shape");
            // ARMARX_IMPORTANT << VAROUT(shapeNode.isValid());
 
            std::optional<std::string> shape;
            if (shapeNode.isString())
            {
                shape = nav.selectSingleNode("shape").asString();
                // ARMARX_IMPORTANT << "shape: " << shape.value();
            }
 
 
            if (!traj)
            {
                traj = std::make_shared<GraspTrajectory>(pose, handValues, shape);
            }
            else
            {
                traj->addKeypoint(pose, handValues, dt, shape);
            }
        }
        return traj;
    }
 
    GraspTrajectoryPtr
    GraspTrajectory::ReadFromFile(const std::string& filename)
    {
        ARMARX_TRACE;
        const auto ext = std::filesystem::path(filename).extension();
        if (ext == ".xml")
        {
            return ReadFromReader(RapidXmlReader::FromFile(filename));
        }
        if (ext == ".json")
        {
            return ReadFromJSON(filename);
        }
 
        ARMARX_WARNING << "Unknown extension `" << ext << "`!";
        return nullptr;
    }
 
    // NLOHMANN_DEFINE_TYPE_NON_INTRUSIVE(GraspTrajectory::Keypoint,tcpTarget,handJointsTarget,shape,dt );
 
 
    inline void
    from_json(const nlohmann::json& j, GraspTrajectoryKeypoint& kp)
    {
        ARMARX_TRACE;
        std::optional<std::string> shape;
 
        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;
 
        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
    GraspTrajectory::ReadFromJSON(const std::string& filename)
    {
        ARMARX_CHECK(std::filesystem::exists(filename));
        ARMARX_INFO << "Reading from file `" << filename << "`";
 
        std::ifstream ifs(filename);
 
        const nlohmann::json j = nlohmann::json::parse(ifs);
 
        std::vector<GraspTrajectoryKeypoint> traj;
        traj = j.at("keypoints");
 
        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);
        ARMARX_VERBOSE << VAROUT(dtSeconds);
 
        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
    GraspTrajectory::ReadFromString(const std::string& xml)
    {
        ARMARX_TRACE;
        return ReadFromReader(RapidXmlReader::FromXmlString(xml));
    }
 
    GraspTrajectory::GraspTrajectory(const Eigen::Matrix4f& tcpStart,
                                     const armarx::NameValueMap& handJointsStart,
                                     const std::optional<std::string>& shape)
    {
        ARMARX_TRACE;
        KeypointPtr keypoint(new Keypoint(tcpStart, handJointsStart, shape));
        keypointMap[0] = keypoints.size();
        keypoints.push_back(keypoint);
    }
 
    GraspTrajectory::GraspTrajectory(const std::vector<Keypoint>& keypointz)
    {
        if (keypointz.empty())
        {
            return;
        }
 
        // insert first keypoint
        keypointMap[0] = keypoints.size();
        keypoints.push_back(std::make_shared<Keypoint>(keypointz.front()));
 
        // insert remaining keypoints
        for (std::size_t i = 1; i < keypointz.size(); i++)
        {
            const auto& kp = keypointz.at(i);
            addKeypoint(kp.tcpTarget, kp.handJointsTarget, kp.dt, kp.shape);
        }
    }
 
    void
    GraspTrajectory::addKeypoint(const Eigen::Matrix4f& tcpTarget,
                                 const armarx::NameValueMap& handJointsTarget,
                                 float dt,
                                 const std::optional<std::string>& shape)
    {
        ARMARX_TRACE;
        KeypointPtr prev = lastKeypoint();
        KeypointPtr keypoint(new Keypoint(tcpTarget, handJointsTarget, shape));
        keypoint->updateVelocities(prev, dt);
        float t = getDuration() + dt;
        keypointMap[t] = keypoints.size();
        keypoints.push_back(keypoint);
    }
 
    size_t
    GraspTrajectory::getKeypointCount() const
    {
        ARMARX_TRACE;
        return keypoints.size();
    }
 
    const std::vector<GraspTrajectory::KeypointPtr>&
    GraspTrajectory::getAllKeypoints() const
    {
        return keypoints;
    }
 
    void
    GraspTrajectory::insertKeypoint(size_t index,
                                    const Eigen::Matrix4f& tcpTarget,
                                    const armarx::NameValueMap& handJointsTarget,
                                    float dt,
                                    const std::optional<std::string>& shape)
    {
        ARMARX_TRACE;
        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, shape));
        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)
    {
        ARMARX_TRACE;
        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())
        {
            ARMARX_TRACE;
            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 armarx::NameValueMap& handJointsTarget,
                                     float dt,
                                     const std::optional<std::string>& shape)
    {
        ARMARX_TRACE;
        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, shape));
        keypoint->updateVelocities(prev, dt);
        keypoints.at(index) = keypoint;
        updateKeypointMap();
    }
 
    void
    GraspTrajectory::setKeypointDt(size_t index, float dt)
    {
        ARMARX_TRACE;
        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();
    }
 
    const GraspTrajectory::KeypointPtr&
    GraspTrajectory::lastKeypoint() const
    {
        ARMARX_TRACE;
        return keypoints.at(keypoints.size() - 1);
    }
 
    const GraspTrajectory::KeypointPtr&
    GraspTrajectory::getKeypoint(int i) const
    {
        ARMARX_TRACE;
        return keypoints.at(i);
    }
 
    Eigen::Matrix4f
    GraspTrajectory::getStartPose() const
    {
        ARMARX_TRACE;
        return getKeypoint(0)->getTargetPose();
    }
 
    void
    GraspTrajectory::getIndex(float t, int& i1, int& i2, float& f) const
    {
        ARMARX_TRACE;
        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) const
    {
        ARMARX_TRACE;
        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) const
    {
        ARMARX_TRACE;
        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) const
    {
        ARMARX_TRACE;
        return ::math::Helpers::CreatePose(GetPosition(t), GetOrientation(t));
    }
 
    std::vector<Eigen::Matrix4f>
    GraspTrajectory::getAllKeypointPoses() const
    {
        ARMARX_TRACE;
        std::vector<Eigen::Matrix4f> res;
        for (const KeypointPtr& keypoint : keypoints)
        {
            res.emplace_back(keypoint->getTargetPose());
        }
        return res;
    }
 
    std::vector<Eigen::Vector3f>
    GraspTrajectory::getAllKeypointPositions() const
    {
        ARMARX_TRACE;
        std::vector<Eigen::Vector3f> res;
        for (const KeypointPtr& keypoint : keypoints)
        {
            res.emplace_back(keypoint->getTargetPosition());
        }
        return res;
    }
 
    std::vector<Eigen::Matrix3f>
    GraspTrajectory::getAllKeypointOrientations() const
    {
        ARMARX_TRACE;
        std::vector<Eigen::Matrix3f> res;
        for (const KeypointPtr& keypoint : keypoints)
        {
            res.emplace_back(keypoint->getTargetOrientation());
        }
        return res;
    }
 
    armarx::NameValueMap
    GraspTrajectory::GetHandValues(float t) const
    {
        ARMARX_TRACE;
        int i1, i2;
        float f;
        getIndex(t, i1, i2, f);
        auto handTargetsMap = getKeypoint(i1)->handJointsTarget;
        const auto handTargets1 = mapValuesToVector(handTargetsMap);
        const auto handTargets2 = mapValuesToVector(getKeypoint(i2)->handJointsTarget);
        const Eigen::VectorXf lerpTargets = handTargets1 * (1 - f) + handTargets2 * f;
        auto* lerpTargetsIt = lerpTargets.data();
        for (auto& [name, value] : handTargetsMap)
        {
            value = *(lerpTargetsIt++);
        }
        return handTargetsMap;
    }
 
    std::optional<std::string>
    GraspTrajectory::GetShape(float t) const
    {
        ARMARX_TRACE;
        int i1, i2;
        float f;
        getIndex(t, i1, i2, f);
 
        // i1 is the one before or at the timestamp t
        return getKeypoint(i1)->shape;
    }
 
    Eigen::Vector3f
    GraspTrajectory::GetPositionDerivative(float t) const
    {
        ARMARX_TRACE;
        int i1, i2;
        float f;
        getIndex(t, i1, i2, f);
        return getKeypoint(i2)->feedForwardPosVelocity;
    }
 
    Eigen::Vector3f
    GraspTrajectory::GetOrientationDerivative(float t) const
    {
        ARMARX_TRACE;
        int i1, i2;
        float f;
        getIndex(t, i1, i2, f);
        return getKeypoint(i2)->feedForwardOriVelocity;
    }
 
    Eigen::Matrix<float, 6, 1>
    GraspTrajectory::GetTcpDerivative(float t) const
    {
        ARMARX_TRACE;
        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) const
    {
        ARMARX_TRACE;
        int i1, i2;
        float f;
        getIndex(t, i1, i2, f);
        return getKeypoint(i2)->feedForwardHandJointsVelocity;
    }
 
    float
    GraspTrajectory::getDuration() const
    {
        ARMARX_TRACE;
        return keypointMap.rbegin()->first;
    }
 
    GraspTrajectory::Length
    GraspTrajectory::calculateLength() const
    {
        ARMARX_TRACE;
        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) const
    {
        ARMARX_TRACE;
        GraspTrajectoryPtr traj(
            new GraspTrajectory(::math::Helpers::TranslateAndRotatePose(
                                    getKeypoint(0)->getTargetPose(), translation, rotation),
                                getKeypoint(0)->handJointsTarget));
        for (size_t i = 1; i < keypoints.size(); i++)
        {
            const KeypointPtr& kp = keypoints.at(i);
            traj->addKeypoint(
                ::math::Helpers::TranslateAndRotatePose(kp->getTargetPose(), translation, rotation),
                kp->handJointsTarget,
                kp->dt,
                kp->shape);
        }
        return traj;
    }
 
    GraspTrajectoryPtr
    GraspTrajectory::getTransformed(const Eigen::Matrix4f& transform) const
    {
        ARMARX_TRACE;
        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, kp->shape);
        }
        return traj;
    }
 
    GraspTrajectoryPtr
    GraspTrajectory::getClone() const
    {
        ARMARX_TRACE;
        return getTransformed(Eigen::Matrix4f::Identity());
    }
 
    GraspTrajectoryPtr
    GraspTrajectory::getTransformedToGraspPose(const Eigen::Matrix4f& target,
                                               const Eigen::Vector3f& handForward) const
    {
        ARMARX_TRACE;
        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() const
    {
        ARMARX_TRACE;
        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++)
        {
            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, kp->shape);
        }
        return output_trajectory;
    }
 
    SimpleDiffIK::Reachability
    GraspTrajectory::calculateReachability(VirtualRobot::RobotNodeSetPtr rns,
                                           VirtualRobot::RobotNodePtr tcp,
                                           SimpleDiffIK::Parameters params)
    {
        ARMARX_TRACE;
        return SimpleDiffIK::CalculateReachability(
            getAllKeypointPoses(), Eigen::VectorXf::Zero(rns->getSize()), rns, tcp, params);
    }
 
    void
    GraspTrajectory::updateKeypointMap()
    {
        ARMARX_TRACE;
        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 deltat)
    {
        ARMARX_TRACE;
        Eigen::Vector3f pos0 = ::math::Helpers::GetPosition(prev->tcpTarget);
        Eigen::Matrix3f ori0 = ::math::Helpers::GetOrientation(prev->tcpTarget);
        auto hnd0 = mapValuesToVector(prev->handJointsTarget);
 
        Eigen::Vector3f pos1 = ::math::Helpers::GetPosition(tcpTarget);
        Eigen::Matrix3f ori1 = ::math::Helpers::GetOrientation(tcpTarget);
        auto hnd1 = mapValuesToVector(handJointsTarget);
 
        Eigen::Vector3f dpos = pos1 - pos0;
        Eigen::Vector3f dori = ::math::Helpers::GetRotationVector(ori0, ori1);
        Eigen::VectorXf dhnd = hnd1 - hnd0;
 
        this->dt = deltat;
        feedForwardPosVelocity = dpos / deltat;
        feedForwardOriVelocity = dori / deltat;
        feedForwardHandJointsVelocity = dhnd / deltat;
    }
 
    Eigen::Vector3f
    GraspTrajectory::Keypoint::getTargetPosition() const
    {
        ARMARX_TRACE;
        return ::math::Helpers::GetPosition(tcpTarget);
    }
 
    Eigen::Matrix3f
    GraspTrajectory::Keypoint::getTargetOrientation() const
    {
        ARMARX_TRACE;
        return ::math::Helpers::GetOrientation(tcpTarget);
    }
 
    Eigen::Matrix4f
    GraspTrajectory::Keypoint::getTargetPose() const
    {
        ARMARX_TRACE;
        return tcpTarget;
    }
 
    GraspTrajectoryKeypoint::GraspTrajectoryKeypoint(
        const Eigen::Matrix4f& tcpTarget,
        const armarx::NameValueMap& handJointsTarget,
        float dt,
        const Eigen::Vector3f& feedForwardPosVelocity,
        const Eigen::Vector3f& feedForwardOriVelocity,
        const Eigen::VectorXf& feedForwardHandJointsVelocity,
        const std::optional<std::string>& shape) :
        tcpTarget(tcpTarget),
        handJointsTarget(handJointsTarget),
        shape(shape),
        dt(dt),
        feedForwardPosVelocity(feedForwardPosVelocity),
        feedForwardOriVelocity(feedForwardOriVelocity),
        feedForwardHandJointsVelocity(feedForwardHandJointsVelocity)
    {
    }
 
    GraspTrajectoryKeypoint::GraspTrajectoryKeypoint(const Eigen::Matrix4f& tcpTarget,
                                                     const armarx::NameValueMap& handJointsTarget,
                                                     const std::optional<std::string>& shape) :
        tcpTarget(tcpTarget),
        handJointsTarget(handJointsTarget),
        shape(shape),
        dt(0),
        feedForwardPosVelocity(0, 0, 0),
        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