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 <VirtualRobot/math/AbstractFunctionR1R6.h>
#include <VirtualRobot/math/Helpers.h>
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
 
using namespace armarx;
 
GraspTrajectory::Keypoint::Keypoint(const Eigen::Matrix4f& tcpTarget,
                                    const Eigen::VectorXf& handJointsTarget) :
    tcpTarget(tcpTarget),
    handJointsTarget(handJointsTarget),
    dt(0),
    feedForwardPosVelocity(0, 0, 0),
    feedForwardOriVelocity(0, 0, 0),
    feedForwardHandJointsVelocity(Eigen::VectorXf::Zero(handJointsTarget.rows()))
{
}
 
GraspTrajectory::Keypoint::Keypoint(const Eigen::Matrix4f& tcpTarget,
                                    const Eigen::VectorXf& handJointsTarget,
                                    float dt,
                                    const Eigen::Vector3f& feedForwardPosVelocity,
                                    const Eigen::Vector3f& feedForwardOriVelocity,
                                    const Eigen::VectorXf& feedForwardHandJointsVelocity) :
    tcpTarget(tcpTarget),
    handJointsTarget(handJointsTarget),
    dt(dt),
    feedForwardPosVelocity(feedForwardPosVelocity),
    feedForwardOriVelocity(feedForwardOriVelocity),
    feedForwardHandJointsVelocity(feedForwardHandJointsVelocity)
{
}
 
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;
}
 
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::VectorXf hnd0 = prev->handJointsTarget;
 
    Eigen::Vector3f pos1 = ::math::Helpers::GetPosition(tcpTarget);
    Eigen::Matrix3f ori1 = ::math::Helpers::GetOrientation(tcpTarget);
    Eigen::VectorXf hnd1 = handJointsTarget;
 
    Eigen::Vector3f dpos = pos1 - pos0;
    Eigen::Vector3f dori = ::math::Helpers::GetRotationVector(ori0, ori1);
    Eigen::VectorXf dhnd = hnd1 - hnd0;
 
    this->dt = dt;
    feedForwardPosVelocity = dpos / dt;
    feedForwardOriVelocity = dori / dt;
    feedForwardHandJointsVelocity = dhnd / dt;
}
 
GraspTrajectory::GraspTrajectory(const Eigen::Matrix4f& tcpStart,
                                 const Eigen::VectorXf& handJointsStart)
{
    KeypointPtr keypoint(new Keypoint(tcpStart, handJointsStart));
    keypointMap[0] = keypoints.size();
    keypoints.push_back(keypoint);
}
 
void
GraspTrajectory::addKeypoint(const Eigen::Matrix4f& tcpTarget,
                             const Eigen::VectorXf& handJointsTarget,
                             float dt)
{
    ARMARX_CHECK_EQUAL(GetHandJointCount(), handJointsTarget.rows());
    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::VectorXf& handJointsTarget,
                                float dt)
{
    ARMARX_CHECK_EQUAL(GetHandJointCount(), handJointsTarget.rows());
    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::VectorXf& handJointsTarget,
                                 float dt)
{
    ARMARX_CHECK_EQUAL(GetHandJointCount(), handJointsTarget.rows());
    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::VectorXf
GraspTrajectory::GetHandValues(float t)
{
    int i1, i2;
    float f;
    getIndex(t, i1, i2, f);
 
    return getKeypoint(i1)->handJointsTarget * (1 - f) + 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::Vector6f
GraspTrajectory::GetTcpDerivative(float t)
{
    Eigen::Vector6f ffVel;
    ffVel.block<3, 1>(0, 0) = GetPositionDerivative(t);
    ffVel.block<3, 1>(3, 0) = GetOrientationDerivative(t);
    return ffVel;
}
 
Eigen::VectorXf
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;
}
 
int
GraspTrajectory::GetHandJointCount() const
{
    return keypoints.at(0)->handJointsTarget.rows();
}
 
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);
}
 
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::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::ReadFromFile(const grasping::GraspCandidatePtr& cnd)
{
    std::string packageName = "armar6_skills"; // cnd->executionHints->graspTrajectoryPackage;
    armarx::CMakePackageFinder finder(packageName);
    std::string dataDir = finder.getDataDir() + "/" + packageName;
    return GraspTrajectory::ReadFromFile(dataDir + "/motions/grasps/" +
                                         cnd->executionHints->graspTrajectoryName + ".xml");
}
 
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));
}
 
void
GraspTrajectory::updateKeypointMap()
{
    keypointMap.clear();
    float t = 0;
    for (size_t i = 0; i < keypoints.size(); i++)
    {
        t += getKeypoint(i)->dt;
        keypointMap[t] = i;
    }
}