diff --git a/source/RobotAPI/libraries/GraspingUtility/CMakeLists.txt b/source/RobotAPI/libraries/GraspingUtility/CMakeLists.txt
index 1ae8862c277d13836d015473953c605db92c9349..fdec590098ce75d6d9e4cc67f9fc8b945ddb43d2 100644
--- a/source/RobotAPI/libraries/GraspingUtility/CMakeLists.txt
+++ b/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
diff --git a/source/RobotAPI/libraries/GraspingUtility/GraspTrajectory.cpp b/source/RobotAPI/libraries/GraspingUtility/GraspTrajectory.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ee722453048da777dbad507f6f9b4e54e25a1865
--- /dev/null
+++ b/source/RobotAPI/libraries/GraspingUtility/GraspTrajectory.cpp
@@ -0,0 +1,484 @@
+/*
+ * 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)
+ { }
+}
diff --git a/source/RobotAPI/libraries/GraspingUtility/GraspTrajectory.h b/source/RobotAPI/libraries/GraspingUtility/GraspTrajectory.h
new file mode 100644
index 0000000000000000000000000000000000000000..18990ca21ae05f41fbe520c0da94374d4f2ae11e
--- /dev/null
+++ b/source/RobotAPI/libraries/GraspingUtility/GraspTrajectory.h
@@ -0,0 +1,151 @@
+/*
+ * 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;
+
+
+ };
+}
diff --git a/source/RobotAPI/libraries/diffik/CMakeLists.txt b/source/RobotAPI/libraries/diffik/CMakeLists.txt
index fce8840d1b6643ea53cc63d18f2703358eb37dde..358be1f028e92c8dd4f3f54ddf8715dbecb90246 100644
--- a/source/RobotAPI/libraries/diffik/CMakeLists.txt
+++ b/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
)