From 34bf6979cd1be5a07850feac00ef3d3c46be5473 Mon Sep 17 00:00:00 2001
From: Simon Ottenhaus <simon.ottenhaus@kit.edu>
Date: Wed, 13 May 2020 10:01:55 +0200
Subject: [PATCH] natik
---
.../units/GraspCandidateProviderInterface.ice | 1 +
.../RobotAPI/libraries/diffik/CMakeLists.txt | 7 +
.../libraries/diffik/DiffIKProvider.h | 47 ++
.../libraries/diffik/GraspTrajectory.cpp | 461 ++++++++++++++++++
.../libraries/diffik/GraspTrajectory.h | 151 ++++++
.../libraries/diffik/RobotPlacement.cpp | 68 +++
.../libraries/diffik/RobotPlacement.h | 59 +++
.../libraries/diffik/SimpleDiffIK.cpp | 31 ++
.../RobotAPI/libraries/diffik/SimpleDiffIK.h | 16 +
source/RobotAPI/libraries/natik/NaturalIK.cpp | 30 ++
source/RobotAPI/libraries/natik/NaturalIK.h | 16 +
11 files changed, 887 insertions(+)
create mode 100644 source/RobotAPI/libraries/diffik/DiffIKProvider.h
create mode 100644 source/RobotAPI/libraries/diffik/GraspTrajectory.cpp
create mode 100644 source/RobotAPI/libraries/diffik/GraspTrajectory.h
create mode 100644 source/RobotAPI/libraries/diffik/RobotPlacement.cpp
create mode 100644 source/RobotAPI/libraries/diffik/RobotPlacement.h
diff --git a/source/RobotAPI/interface/units/GraspCandidateProviderInterface.ice b/source/RobotAPI/interface/units/GraspCandidateProviderInterface.ice
index 8749be775..ebc8cf7c4 100644
--- a/source/RobotAPI/interface/units/GraspCandidateProviderInterface.ice
+++ b/source/RobotAPI/interface/units/GraspCandidateProviderInterface.ice
@@ -62,6 +62,7 @@ module armarx
ApertureType preshape = AnyAperture;
ApproachType approach = AnyApproach;
string graspTrajectoryName;
+ string graspTrajectoryPackage;
};
class GraspCandidateReachabilityInfo
{
diff --git a/source/RobotAPI/libraries/diffik/CMakeLists.txt b/source/RobotAPI/libraries/diffik/CMakeLists.txt
index 9699d1913..05a777679 100644
--- a/source/RobotAPI/libraries/diffik/CMakeLists.txt
+++ b/source/RobotAPI/libraries/diffik/CMakeLists.txt
@@ -6,15 +6,22 @@ armarx_set_target("Library: ${LIB_NAME}")
set(LIBS
ArmarXCore
RobotAPICore
+ StructuralJson
+ SimpleJsonLogger
)
set(LIB_FILES
NaturalDiffIK.cpp
SimpleDiffIK.cpp
+ RobotPlacement.cpp
+ GraspTrajectory.cpp
)
set(LIB_HEADERS
NaturalDiffIK.h
SimpleDiffIK.h
+ DiffIKProvider.h
+ RobotPlacement.h
+ GraspTrajectory.h
)
diff --git a/source/RobotAPI/libraries/diffik/DiffIKProvider.h b/source/RobotAPI/libraries/diffik/DiffIKProvider.h
new file mode 100644
index 000000000..a75434af4
--- /dev/null
+++ b/source/RobotAPI/libraries/diffik/DiffIKProvider.h
@@ -0,0 +1,47 @@
+/*
+ * 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 <boost/shared_ptr.hpp>
+#include <Eigen/Dense>
+
+namespace armarx
+{
+ typedef boost::shared_ptr<class DiffIKProvider> DiffIKProviderPtr;
+
+ struct DiffIKResult
+ {
+ bool reachable;
+ float posError;
+ float oriError;
+ Eigen::VectorXf jointValues;
+
+ };
+ class DiffIKProvider
+ {
+ public:
+ virtual DiffIKResult SolveAbsolute(const Eigen::Matrix4f& targetPose) = 0;
+ virtual DiffIKResult SolveRelative(const Eigen::Matrix4f& targetPose, const Eigen::VectorXf& startJointValues) = 0;
+ };
+}
diff --git a/source/RobotAPI/libraries/diffik/GraspTrajectory.cpp b/source/RobotAPI/libraries/diffik/GraspTrajectory.cpp
new file mode 100644
index 000000000..5539a9afb
--- /dev/null
+++ b/source/RobotAPI/libraries/diffik/GraspTrajectory.cpp
@@ -0,0 +1,461 @@
+/*
+ * 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 <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 = 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;
+ }
+}
diff --git a/source/RobotAPI/libraries/diffik/GraspTrajectory.h b/source/RobotAPI/libraries/diffik/GraspTrajectory.h
new file mode 100644
index 000000000..3e5633d59
--- /dev/null
+++ b/source/RobotAPI/libraries/diffik/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 <boost/shared_ptr.hpp>
+#include <Eigen/Dense>
+#include <VirtualRobot/math/AbstractFunctionR1R6.h>
+#include <VirtualRobot/math/Helpers.h>
+#include <map>
+#include <ArmarXCore/core/exceptions/Exception.h>
+#include <ArmarXCore/core/rapidxml/wrapper/RapidXmlReader.h>
+#include <ArmarXCore/core/rapidxml/wrapper/RapidXmlWriter.h>
+#include <RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLogger.h>
+#include <ArmarXGui/libraries/StructuralJson/JPathNavigator.h>
+#include <ArmarXGui/libraries/StructuralJson/StructuralJsonParser.h>
+#include <RobotAPI/libraries/diffik/SimpleDiffIK.h>
+#include <RobotAPI/interface/units/GraspCandidateProviderInterface.h>
+#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
+#include <ArmarXCore/interface/serialization/Eigen.h>
+
+namespace armarx
+{
+ typedef boost::shared_ptr<class GraspTrajectory> GraspTrajectoryPtr;
+
+ class GraspTrajectory
+ {
+ public:
+ class Keypoint;
+ typedef boost::shared_ptr<Keypoint> KeypointPtr;
+
+ class Keypoint
+ {
+ public:
+ Eigen::Matrix4f tcpTarget;
+ Eigen::VectorXf handJointsTarget;
+ float dt;
+ Eigen::Vector3f feedForwardPosVelocity;
+ Eigen::Vector3f feedForwardOriVelocity;
+ Eigen::VectorXf feedForwardHandJointsVelocity;
+
+ Keypoint(const Eigen::Matrix4f& tcpTarget, const Eigen::VectorXf& handJointsTarget);
+ Keypoint(const Eigen::Matrix4f& tcpTarget, const Eigen::VectorXf& 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);
+ };
+
+ struct Length
+ {
+ float pos = 0;
+ float ori = 0;
+ };
+
+
+ public:
+ GraspTrajectory(const Eigen::Matrix4f& tcpStart, const Eigen::VectorXf& handJointsStart);
+
+ void addKeypoint(const Eigen::Matrix4f& tcpTarget, const Eigen::VectorXf& handJointsTarget, float dt);
+
+ size_t getKeypointCount() const;
+
+ void insertKeypoint(size_t index, const Eigen::Matrix4f& tcpTarget, const Eigen::VectorXf& handJointsTarget, float dt);
+
+ void removeKeypoint(size_t index);
+
+ void replaceKeypoint(size_t index, const Eigen::Matrix4f& tcpTarget, const Eigen::VectorXf& 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::VectorXf GetHandValues(float t);
+
+ Eigen::Vector3f GetPositionDerivative(float t);
+
+ Eigen::Vector3f GetOrientationDerivative(float t);
+
+ Eigen::Vector6f GetTcpDerivative(float t);
+
+ Eigen::VectorXf GetHandJointsDerivative(float t);
+
+ float getDuration() const;
+
+ Length calculateLength() const;
+ int GetHandJointCount() const;
+
+
+ GraspTrajectoryPtr getTranslatedAndRotated(const Eigen::Vector3f& translation, const Eigen::Matrix3f& rotation);
+ GraspTrajectoryPtr getTransformed(const Eigen::Matrix4f& transform);
+
+ 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 ReadFromFile(const grasping::GraspCandidatePtr& cnd);
+
+ 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/RobotPlacement.cpp b/source/RobotAPI/libraries/diffik/RobotPlacement.cpp
new file mode 100644
index 000000000..4075c9aae
--- /dev/null
+++ b/source/RobotAPI/libraries/diffik/RobotPlacement.cpp
@@ -0,0 +1,68 @@
+/*
+ * 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 "RobotPlacement.h"
+
+#include <VirtualRobot/math/Helpers.h>
+
+using namespace armarx;
+
+RobotPlacement::RobotPlacement(const DiffIKProviderPtr& ikProvider)
+{
+}
+
+std::vector<Eigen::Matrix4f> RobotPlacement::CreateGrid(float dx, int minx, int maxx, float dy, int miny, int maxy, float da, int mina, int maxa)
+{
+ std::vector<Eigen::Matrix4f> r;
+ for (int x = minx; x <= maxx; x++)
+ for (int y = miny; y <= maxy; y++)
+ for (int a = mina; a <= maxa; a++)
+ {
+ r.emplace_back(math::Helpers::CreatePose(Eigen::Vector3f(x * dx, y * dy, 0), Eigen::AngleAxisf(a * da, Eigen::Vector3f::UnitZ()).toRotationMatrix()));
+ }
+ return r;
+}
+
+std::vector<RobotPlacement::Result> RobotPlacement::evaluatePlacements(const std::vector<Eigen::Matrix4f>& robotPlacements, const RobotPlacement::PlacementParams& params)
+{
+ std::vector<RobotPlacement::Result> r;
+ std::vector<Eigen::Matrix4f> tcpTargets;
+ for (const Eigen::Matrix4f& pp : params.prePoses)
+ {
+ tcpTargets.emplace_back(pp);
+ }
+ std::vector<Eigen::Matrix4f> grasPoses = params.graspTrajectory->getAllKeypointPoses();
+
+ for (const Eigen::Matrix4f& placement : robotPlacements)
+ {
+ Eigen::Matrix4f invPlacement = placement.inverse();
+ std::vector<Eigen::Matrix4f> localPoses;
+ for (const Eigen::Matrix4f& tcpPose : tcpTargets)
+ {
+ localPoses.emplace_back(invPlacement * tcpPose);
+ }
+ DiffIKResult ikResult = ikProvider->SolveAbsolute(localPoses.at(0));
+ throw LocalException("not implemented");
+ }
+ return {};
+}
diff --git a/source/RobotAPI/libraries/diffik/RobotPlacement.h b/source/RobotAPI/libraries/diffik/RobotPlacement.h
new file mode 100644
index 000000000..a677a7a11
--- /dev/null
+++ b/source/RobotAPI/libraries/diffik/RobotPlacement.h
@@ -0,0 +1,59 @@
+/*
+ * 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 "DiffIKProvider.h"
+#include "GraspTrajectory.h"
+
+#include <boost/shared_ptr.hpp>
+
+namespace armarx
+{
+ typedef boost::shared_ptr<class RobotPlacement> RobotPlacementPtr;
+
+ class RobotPlacement
+ {
+ public:
+ struct Result
+ {
+ DiffIKResult ikResult;
+ };
+ struct PlacementParams
+ {
+ std::vector<Eigen::Matrix4f> prePoses;
+ GraspTrajectoryPtr graspTrajectory;
+ };
+ public:
+ RobotPlacement(const DiffIKProviderPtr& ikProvider);
+
+ static std::vector<Eigen::Matrix4f> CreateGrid(float dx, int minx, int maxx, float dy, int miny, int maxy, float da, int mina, int maxa);
+
+ std::vector<Result> evaluatePlacements(const std::vector<Eigen::Matrix4f>& robotPlacements, const PlacementParams& params);
+
+
+ private:
+ DiffIKProviderPtr ikProvider;
+ bool returnOnlyReachable = true;
+ };
+}
diff --git a/source/RobotAPI/libraries/diffik/SimpleDiffIK.cpp b/source/RobotAPI/libraries/diffik/SimpleDiffIK.cpp
index 6d0021aec..3e436b8cf 100644
--- a/source/RobotAPI/libraries/diffik/SimpleDiffIK.cpp
+++ b/source/RobotAPI/libraries/diffik/SimpleDiffIK.cpp
@@ -138,4 +138,35 @@ namespace armarx
return r;
}
+ SimpleDiffIKProvider::SimpleDiffIKProvider(VirtualRobot::RobotNodeSetPtr rns, VirtualRobot::RobotNodePtr tcp, SimpleDiffIK::Parameters params)
+ : rns(rns), tcp(tcp), params(params)
+ {
+ }
+
+ DiffIKResult SimpleDiffIKProvider::SolveAbsolute(const Eigen::Matrix4f& targetPose)
+ {
+ params.resetRnsValues = true;
+ SimpleDiffIK::Result result = SimpleDiffIK::CalculateDiffIK(targetPose, rns, tcp, params);
+ DiffIKResult r;
+ r.jointValues = rns->getJointValuesEigen();
+ r.oriError = result.oriError;
+ r.posError = result.posError;
+ r.reachable = result.reached;
+ return r;
+
+ }
+
+ DiffIKResult SimpleDiffIKProvider::SolveRelative(const Eigen::Matrix4f& targetPose, const Eigen::VectorXf& startJointValues)
+ {
+ params.resetRnsValues = false;
+ rns->setJointValues(startJointValues);
+ SimpleDiffIK::Result result = SimpleDiffIK::CalculateDiffIK(targetPose, rns, tcp, params);
+ DiffIKResult r;
+ r.jointValues = rns->getJointValuesEigen();
+ r.oriError = result.oriError;
+ r.posError = result.posError;
+ r.reachable = result.reached;
+ return r;
+ }
+
}
diff --git a/source/RobotAPI/libraries/diffik/SimpleDiffIK.h b/source/RobotAPI/libraries/diffik/SimpleDiffIK.h
index 4ae4e22fa..eeee21dac 100644
--- a/source/RobotAPI/libraries/diffik/SimpleDiffIK.h
+++ b/source/RobotAPI/libraries/diffik/SimpleDiffIK.h
@@ -23,6 +23,8 @@
#pragma once
+#include "DiffIKProvider.h"
+
#include <boost/shared_ptr.hpp>
#include <VirtualRobot/Nodes/RobotNode.h>
@@ -109,4 +111,18 @@ namespace armarx
///@brief Use this to check a trajectory of waypoints
static Reachability CalculateReachability(const std::vector<Eigen::Matrix4f> targets, const Eigen::VectorXf& initialJV, VirtualRobot::RobotNodeSetPtr rns, VirtualRobot::RobotNodePtr tcp = VirtualRobot::RobotNodePtr(), Parameters params = Parameters());
};
+
+ class SimpleDiffIKProvider :
+ public DiffIKProvider
+ {
+ public:
+ SimpleDiffIKProvider(VirtualRobot::RobotNodeSetPtr rns, VirtualRobot::RobotNodePtr tcp = VirtualRobot::RobotNodePtr(), SimpleDiffIK::Parameters params = SimpleDiffIK::Parameters());
+ DiffIKResult SolveAbsolute(const Eigen::Matrix4f& targetPose);
+ DiffIKResult SolveRelative(const Eigen::Matrix4f& targetPose, const Eigen::VectorXf& startJointValues);
+
+ private:
+ VirtualRobot::RobotNodeSetPtr rns;
+ VirtualRobot::RobotNodePtr tcp;
+ SimpleDiffIK::Parameters params;
+ };
}
diff --git a/source/RobotAPI/libraries/natik/NaturalIK.cpp b/source/RobotAPI/libraries/natik/NaturalIK.cpp
index 88c57b357..f0ad1b730 100644
--- a/source/RobotAPI/libraries/natik/NaturalIK.cpp
+++ b/source/RobotAPI/libraries/natik/NaturalIK.cpp
@@ -213,3 +213,33 @@ void NaturalIK::setLowerArmLength(float value)
{
lowerArmLength = value;
}
+
+NaturalIKProvider::NaturalIKProvider(const NaturalIK& natik, const NaturalIK::ArmJoints& arm, const NaturalDiffIK::Mode& setOri, const NaturalIK::Parameters& params)
+ : natik(natik), arm(arm), setOri(setOri), params(params)
+{
+}
+
+DiffIKResult NaturalIKProvider::SolveAbsolute(const Eigen::Matrix4f& targetPose)
+{
+ params.diffIKparams.resetRnsValues = true;
+ NaturalDiffIK::Result result = natik.calculateIK(targetPose, arm, setOri, params);
+ DiffIKResult r;
+ r.jointValues = arm.rns->getJointValuesEigen();
+ r.oriError = result.oriError;
+ r.posError = result.posError;
+ r.reachable = result.reached;
+ return r;
+}
+
+DiffIKResult NaturalIKProvider::SolveRelative(const Eigen::Matrix4f& targetPose, const Eigen::VectorXf& startJointValues)
+{
+ params.diffIKparams.resetRnsValues = false;
+ arm.rns->setJointValues(startJointValues);
+ NaturalDiffIK::Result result = natik.calculateIK(targetPose, arm, setOri, params);
+ DiffIKResult r;
+ r.jointValues = arm.rns->getJointValuesEigen();
+ r.oriError = result.oriError;
+ r.posError = result.posError;
+ r.reachable = result.reached;
+ return r;
+}
diff --git a/source/RobotAPI/libraries/natik/NaturalIK.h b/source/RobotAPI/libraries/natik/NaturalIK.h
index 6db3aa965..d6315096a 100644
--- a/source/RobotAPI/libraries/natik/NaturalIK.h
+++ b/source/RobotAPI/libraries/natik/NaturalIK.h
@@ -27,6 +27,7 @@
//#include <RobotAPI/libraries/core/SimpleDiffIK.h>
#include <VirtualRobot/Nodes/RobotNode.h>
+#include <RobotAPI/libraries/diffik/DiffIKProvider.h>
#include <RobotAPI/libraries/diffik/NaturalDiffIK.h>
#include <optional>
@@ -134,4 +135,19 @@ namespace armarx
float upperArmLength = SoechtingAngles::MMM_UPPER_ARM_LENGTH;
float lowerArmLength = SoechtingAngles::MMM_LOWER_ARM_LENGTH;
};
+
+ class NaturalIKProvider
+ : public DiffIKProvider
+ {
+ public:
+ NaturalIKProvider(const NaturalIK& natik, const NaturalIK::ArmJoints& arm, const NaturalDiffIK::Mode& setOri, const NaturalIK::Parameters& params = NaturalIK::Parameters());
+ DiffIKResult SolveAbsolute(const Eigen::Matrix4f& targetPose);
+ DiffIKResult SolveRelative(const Eigen::Matrix4f& targetPose, const Eigen::VectorXf& startJointValues);
+
+ private:
+ NaturalIK natik;
+ NaturalIK::ArmJoints arm;
+ NaturalDiffIK::Mode setOri;
+ NaturalIK::Parameters params;
+ };
}
--
GitLab