From f2e53e902a9be9a250676d355030675fece0c063 Mon Sep 17 00:00:00 2001
From: Christoph Pohl <christoph.pohl@kit.edu>
Date: Tue, 8 Aug 2023 15:53:37 +0200
Subject: [PATCH] Remove hard coded dependency on 2 dog for finger joints
---
.../GraspingUtility/GraspTrajectory.cpp | 302 +++++++++++++-----
.../GraspingUtility/GraspTrajectory.h | 65 ++--
2 files changed, 258 insertions(+), 109 deletions(-)
diff --git a/source/RobotAPI/libraries/GraspingUtility/GraspTrajectory.cpp b/source/RobotAPI/libraries/GraspingUtility/GraspTrajectory.cpp
index c4b170313..045026b0e 100644
--- a/source/RobotAPI/libraries/GraspingUtility/GraspTrajectory.cpp
+++ b/source/RobotAPI/libraries/GraspingUtility/GraspTrajectory.cpp
@@ -21,6 +21,10 @@
* GNU General Public License
*/
+#include "GraspTrajectory.h"
+
+#include <memory>
+
#include <VirtualRobot/math/Helpers.h>
#include <ArmarXCore/core/exceptions/Exception.h>
@@ -34,14 +38,23 @@
#include <RobotAPI/interface/units/GraspCandidateProviderInterface.h>
#include <RobotAPI/libraries/SimpleJsonLogger/SimpleJsonLogger.h>
-#include "GraspTrajectory.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)
@@ -49,7 +62,12 @@ namespace armarx
SimpleJsonLoggerEntry e;
e.Add("dt", keypoint->dt);
e.AddAsArr("Pose", keypoint->tcpTarget);
- e.AddAsArr("HandValues", keypoint->handJointsTarget);
+ 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);
@@ -58,7 +76,8 @@ namespace armarx
GraspTrajectoryPtr
GraspTrajectory::ReadFromReader(const RapidXmlReaderPtr& reader)
{
- RapidXmlReaderNode root = reader->getRoot();
+ ARMARX_TRACE;
+ RapidXmlReaderNode const root = reader->getRoot();
GraspTrajectoryPtr traj;
for (const RapidXmlReaderNode& kpNode : root.nodes("Keypoint"))
@@ -79,16 +98,17 @@ namespace armarx
}
}
- Eigen::Vector3f handValues;
+ armarx::NameValueMap handValues;
std::vector<JPathNavigator> cells = nav.select("HandValues/*");
- for (int j = 0; j < 3; j++)
+ for (const auto& cell : cells)
{
- handValues(j) = cells.at(j).asFloat();
+ handValues[cell.getKey()] = cell.asFloat();
}
+
if (!traj)
{
- traj = GraspTrajectoryPtr(new GraspTrajectory(pose, handValues));
+ traj = std::make_shared<GraspTrajectory>(pose, handValues);
}
else
{
@@ -101,25 +121,32 @@ namespace armarx
GraspTrajectoryPtr
GraspTrajectory::ReadFromFile(const std::string& filename)
{
+ ARMARX_TRACE;
return ReadFromReader(RapidXmlReader::FromFile(filename));
}
GraspTrajectoryPtr
GraspTrajectory::ReadFromString(const std::string& xml)
{
+ ARMARX_TRACE;
return ReadFromReader(RapidXmlReader::FromXmlString(xml));
}
- GraspTrajectory::GraspTrajectory(const Eigen::Matrix4f &tcpStart,
- const Eigen::Vector3f &handJointsStart) {
+ GraspTrajectory::GraspTrajectory(const Eigen::Matrix4f& tcpStart,
+ const armarx::NameValueMap& handJointsStart)
+ {
+ ARMARX_TRACE;
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) {
+ GraspTrajectory::addKeypoint(const Eigen::Matrix4f& tcpTarget,
+ const armarx::NameValueMap& handJointsTarget,
+ float dt)
+ {
+ ARMARX_TRACE;
KeypointPtr prev = lastKeypoint();
KeypointPtr keypoint(new Keypoint(tcpTarget, handJointsTarget));
keypoint->updateVelocities(prev, dt);
@@ -129,13 +156,19 @@ namespace armarx
}
size_t
- GraspTrajectory::getKeypointCount() const {
+ GraspTrajectory::getKeypointCount() const
+ {
+ ARMARX_TRACE;
return keypoints.size();
}
void
- GraspTrajectory::insertKeypoint(size_t index, const Eigen::Matrix4f &tcpTarget,
- const Eigen::Vector3f &handJointsTarget, float dt) {
+ GraspTrajectory::insertKeypoint(size_t index,
+ const Eigen::Matrix4f& tcpTarget,
+ const armarx::NameValueMap& handJointsTarget,
+ float dt)
+ {
+ ARMARX_TRACE;
if (index <= 0 || index > keypoints.size())
{
throw LocalException("Index out of range" + std::to_string(index));
@@ -153,7 +186,9 @@ namespace armarx
}
void
- GraspTrajectory::removeKeypoint(size_t index) {
+ GraspTrajectory::removeKeypoint(size_t index)
+ {
+ ARMARX_TRACE;
if (index <= 0 || index >= keypoints.size())
{
throw LocalException("Index out of range" + std::to_string(index));
@@ -161,6 +196,7 @@ namespace armarx
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);
@@ -169,8 +205,12 @@ namespace armarx
}
void
- GraspTrajectory::replaceKeypoint(size_t index, const Eigen::Matrix4f &tcpTarget,
- const Eigen::Vector3f &handJointsTarget, float dt) {
+ GraspTrajectory::replaceKeypoint(size_t index,
+ const Eigen::Matrix4f& tcpTarget,
+ const armarx::NameValueMap& handJointsTarget,
+ float dt)
+ {
+ ARMARX_TRACE;
if (index <= 0 || index >= keypoints.size())
{
throw LocalException("Index out of range" + std::to_string(index));
@@ -183,7 +223,9 @@ namespace armarx
}
void
- GraspTrajectory::setKeypointDt(size_t index, float dt) {
+ 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));
@@ -194,23 +236,31 @@ namespace armarx
updateKeypointMap();
}
- GraspTrajectory::KeypointPtr &
- GraspTrajectory::lastKeypoint() {
+ GraspTrajectory::KeypointPtr&
+ GraspTrajectory::lastKeypoint()
+ {
+ ARMARX_TRACE;
return keypoints.at(keypoints.size() - 1);
}
- GraspTrajectory::KeypointPtr &
- GraspTrajectory::getKeypoint(int i) {
+ GraspTrajectory::KeypointPtr&
+ GraspTrajectory::getKeypoint(int i)
+ {
+ ARMARX_TRACE;
return keypoints.at(i);
}
Eigen::Matrix4f
- GraspTrajectory::getStartPose() {
+ GraspTrajectory::getStartPose()
+ {
+ ARMARX_TRACE;
return getKeypoint(0)->getTargetPose();
}
void
- GraspTrajectory::getIndex(float t, int &i1, int &i2, float &f) {
+ GraspTrajectory::getIndex(float t, int& i1, int& i2, float& f)
+ {
+ ARMARX_TRACE;
if (t <= 0)
{
i1 = 0;
@@ -234,15 +284,20 @@ namespace armarx
}
Eigen::Vector3f
- GraspTrajectory::GetPosition(float t) {
+ GraspTrajectory::GetPosition(float t)
+ {
+ ARMARX_TRACE;
int i1, i2;
float f;
getIndex(t, i1, i2, f);
- return ::math::Helpers::Lerp(getKeypoint(i1)->getTargetPosition(), getKeypoint(i2)->getTargetPosition(), f);
+ return ::math::Helpers::Lerp(
+ getKeypoint(i1)->getTargetPosition(), getKeypoint(i2)->getTargetPosition(), f);
}
Eigen::Matrix3f
- GraspTrajectory::GetOrientation(float t) {
+ GraspTrajectory::GetOrientation(float t)
+ {
+ ARMARX_TRACE;
int i1, i2;
float f;
getIndex(t, i1, i2, f);
@@ -257,12 +312,16 @@ namespace armarx
}
Eigen::Matrix4f
- GraspTrajectory::GetPose(float t) {
+ GraspTrajectory::GetPose(float t)
+ {
+ ARMARX_TRACE;
return ::math::Helpers::CreatePose(GetPosition(t), GetOrientation(t));
}
std::vector<Eigen::Matrix4f>
- GraspTrajectory::getAllKeypointPoses() {
+ GraspTrajectory::getAllKeypointPoses()
+ {
+ ARMARX_TRACE;
std::vector<Eigen::Matrix4f> res;
for (const KeypointPtr& keypoint : keypoints)
{
@@ -272,7 +331,9 @@ namespace armarx
}
std::vector<Eigen::Vector3f>
- GraspTrajectory::getAllKeypointPositions() {
+ GraspTrajectory::getAllKeypointPositions()
+ {
+ ARMARX_TRACE;
std::vector<Eigen::Vector3f> res;
for (const KeypointPtr& keypoint : keypoints)
{
@@ -282,7 +343,9 @@ namespace armarx
}
std::vector<Eigen::Matrix3f>
- GraspTrajectory::getAllKeypointOrientations() {
+ GraspTrajectory::getAllKeypointOrientations()
+ {
+ ARMARX_TRACE;
std::vector<Eigen::Matrix3f> res;
for (const KeypointPtr& keypoint : keypoints)
{
@@ -291,16 +354,29 @@ namespace armarx
return res;
}
- Eigen::Vector3f
- GraspTrajectory::GetHandValues(float t) {
+ armarx::NameValueMap
+ GraspTrajectory::GetHandValues(float t)
+ {
+ ARMARX_TRACE;
int i1, i2;
float f;
getIndex(t, i1, i2, f);
- return ::math::Helpers::Lerp(getKeypoint(i1)->handJointsTarget, getKeypoint(i2)->handJointsTarget, 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;
}
Eigen::Vector3f
- GraspTrajectory::GetPositionDerivative(float t) {
+ GraspTrajectory::GetPositionDerivative(float t)
+ {
+ ARMARX_TRACE;
int i1, i2;
float f;
getIndex(t, i1, i2, f);
@@ -308,7 +384,9 @@ namespace armarx
}
Eigen::Vector3f
- GraspTrajectory::GetOrientationDerivative(float t) {
+ GraspTrajectory::GetOrientationDerivative(float t)
+ {
+ ARMARX_TRACE;
int i1, i2;
float f;
getIndex(t, i1, i2, f);
@@ -316,7 +394,9 @@ namespace armarx
}
Eigen::Matrix<float, 6, 1>
- GraspTrajectory::GetTcpDerivative(float t) {
+ GraspTrajectory::GetTcpDerivative(float t)
+ {
+ ARMARX_TRACE;
Eigen::Matrix<float, 6, 1> ffVel;
ffVel.block<3, 1>(0, 0) = GetPositionDerivative(t);
ffVel.block<3, 1>(3, 0) = GetOrientationDerivative(t);
@@ -324,7 +404,9 @@ namespace armarx
}
Eigen::Vector3f
- GraspTrajectory::GetHandJointsDerivative(float t) {
+ GraspTrajectory::GetHandJointsDerivative(float t)
+ {
+ ARMARX_TRACE;
int i1, i2;
float f;
getIndex(t, i1, i2, f);
@@ -332,12 +414,16 @@ namespace armarx
}
float
- GraspTrajectory::getDuration() const {
+ GraspTrajectory::getDuration() const
+ {
+ ARMARX_TRACE;
return keypointMap.rbegin()->first;
}
GraspTrajectory::Length
- GraspTrajectory::calculateLength() const {
+ GraspTrajectory::calculateLength() const
+ {
+ ARMARX_TRACE;
Length l;
for (size_t i = 1; i < keypoints.size(); i++)
{
@@ -357,19 +443,31 @@ namespace armarx
}
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));
+ GraspTrajectory::getTranslatedAndRotated(const Eigen::Vector3f& translation,
+ const Eigen::Matrix3f& rotation)
+ {
+ 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++)
{
KeypointPtr& kp = keypoints.at(i);
- traj->addKeypoint(::math::Helpers::TranslateAndRotatePose(kp->getTargetPose(), translation, rotation), kp->handJointsTarget, kp->dt);
+ 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));
+ GraspTrajectory::getTransformed(const Eigen::Matrix4f& transform)
+ {
+ ARMARX_TRACE;
+ GraspTrajectoryPtr traj(
+ new GraspTrajectory(transform * getStartPose(), getKeypoint(0)->handJointsTarget));
for (size_t i = 1; i < keypoints.size(); i++)
{
KeypointPtr& kp = keypoints.at(i);
@@ -379,32 +477,44 @@ namespace armarx
}
GraspTrajectoryPtr
- GraspTrajectory::getClone() {
+ GraspTrajectory::getClone()
+ {
+ ARMARX_TRACE;
return getTransformed(Eigen::Matrix4f::Identity());
}
GraspTrajectoryPtr
- GraspTrajectory::getTransformedToGraspPose(const Eigen::Matrix4f &target, const Eigen::Vector3f &handForward) {
+ GraspTrajectory::getTransformedToGraspPose(const Eigen::Matrix4f& target,
+ const Eigen::Vector3f& handForward)
+ {
+ ARMARX_TRACE;
Eigen::Matrix4f startPose = getStartPose();
- Eigen::Vector3f targetHandForward = ::math::Helpers::TransformDirection(target, handForward);
- Eigen::Vector3f trajHandForward = ::math::Helpers::TransformDirection(startPose, handForward);
+ 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));
+ Eigen::Matrix4f transform = ::math::Helpers::CreateTranslationRotationTranslationPose(
+ -::math::Helpers::GetPosition(startPose),
+ aa.toRotationMatrix(),
+ ::math::Helpers::GetPosition(target));
return getTransformed(transform);
}
GraspTrajectoryPtr
- GraspTrajectory::getTransformedToOtherHand() {
+ GraspTrajectory::getTransformedToOtherHand()
+ {
+ 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));
+ new GraspTrajectory(start_pose, getKeypoint(0)->handJointsTarget));
for (size_t i = 1; i < getKeypointCount(); i++)
{
GraspTrajectory::KeypointPtr& kp = getKeypoint(i);
@@ -416,13 +526,19 @@ namespace armarx
}
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);
+ 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() {
+ GraspTrajectory::updateKeypointMap()
+ {
+ ARMARX_TRACE;
keypointMap.clear();
float t = 0;
for (size_t i = 0; i < keypoints.size(); i++)
@@ -432,54 +548,72 @@ namespace armarx
}
}
-
-
void
- GraspTrajectory::Keypoint::updateVelocities(const GraspTrajectory::KeypointPtr &prev, float dt) {
+ 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);
- Eigen::Vector3f hnd0 = prev->handJointsTarget;
+ auto hnd0 = mapValuesToVector(prev->handJointsTarget);
Eigen::Vector3f pos1 = ::math::Helpers::GetPosition(tcpTarget);
Eigen::Matrix3f ori1 = ::math::Helpers::GetOrientation(tcpTarget);
- Eigen::Vector3f hnd1 = handJointsTarget;
+ auto hnd1 = mapValuesToVector(handJointsTarget);
Eigen::Vector3f dpos = pos1 - pos0;
Eigen::Vector3f dori = ::math::Helpers::GetRotationVector(ori0, ori1);
- Eigen::Vector3f dhnd = hnd1 - hnd0;
+ Eigen::VectorXf dhnd = hnd1 - hnd0;
- this->dt = dt;
- feedForwardPosVelocity = dpos / dt;
- feedForwardOriVelocity = dori / dt;
- feedForwardHandJointsVelocity = dhnd / dt;
+ this->dt = deltat;
+ feedForwardPosVelocity = dpos / deltat;
+ feedForwardOriVelocity = dori / deltat;
+ feedForwardHandJointsVelocity = dhnd / deltat;
}
Eigen::Vector3f
- GraspTrajectory::Keypoint::getTargetPosition() const {
+ GraspTrajectory::Keypoint::getTargetPosition() const
+ {
+ ARMARX_TRACE;
return ::math::Helpers::GetPosition(tcpTarget);
}
Eigen::Matrix3f
- GraspTrajectory::Keypoint::getTargetOrientation() const {
+ GraspTrajectory::Keypoint::getTargetOrientation() const
+ {
+ ARMARX_TRACE;
return ::math::Helpers::GetOrientation(tcpTarget);
}
Eigen::Matrix4f
- GraspTrajectory::Keypoint::getTargetPose() const {
+ GraspTrajectory::Keypoint::getTargetPose() const
+ {
+ ARMARX_TRACE;
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)
- { }
-}
+ GraspTrajectory::Keypoint::Keypoint(const Eigen::Matrix4f& tcpTarget,
+ const armarx::NameValueMap& 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)
+ {
+ }
+
+ GraspTrajectory::Keypoint::Keypoint(const Eigen::Matrix4f& tcpTarget,
+ const armarx::NameValueMap& handJointsTarget) :
+ tcpTarget(tcpTarget),
+ handJointsTarget(handJointsTarget),
+ dt(0),
+ feedForwardPosVelocity(0, 0, 0),
+ feedForwardOriVelocity(0, 0, 0)
+ {
+ }
+} // namespace armarx
diff --git a/source/RobotAPI/libraries/GraspingUtility/GraspTrajectory.h b/source/RobotAPI/libraries/GraspingUtility/GraspTrajectory.h
index 18990ca21..697727b22 100644
--- a/source/RobotAPI/libraries/GraspingUtility/GraspTrajectory.h
+++ b/source/RobotAPI/libraries/GraspingUtility/GraspTrajectory.h
@@ -23,12 +23,15 @@
#pragma once
-#include <Eigen/Core>
-#include <memory>
#include <map>
+#include <memory>
+
+#include <Eigen/Core>
+
#include <VirtualRobot/VirtualRobot.h>
-#include <RobotAPI/libraries/diffik/SimpleDiffIK.h>
+
#include <RobotAPI/interface/units/GraspCandidateProviderInterface.h>
+#include <RobotAPI/libraries/diffik/SimpleDiffIK.h>
namespace armarx
{
@@ -48,20 +51,23 @@ namespace armarx
{
public:
Eigen::Matrix4f tcpTarget;
- Eigen::Vector3f handJointsTarget;
+ armarx::NameValueMap 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::VectorXf feedForwardHandJointsVelocity;
+
+ Keypoint(const Eigen::Matrix4f& tcpTarget, const armarx::NameValueMap& handJointsTarget);
+ Keypoint(const Eigen::Matrix4f& tcpTarget,
+ const armarx::NameValueMap& 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);
+ void updateVelocities(const KeypointPtr& prev, float deltat);
};
struct Length
@@ -73,17 +79,26 @@ namespace armarx
public:
GraspTrajectory() = default;
- GraspTrajectory(const Eigen::Matrix4f& tcpStart, const Eigen::Vector3f& handJointsStart);
+ GraspTrajectory(const Eigen::Matrix4f& tcpStart,
+ const armarx::NameValueMap& handJointsStart);
- void addKeypoint(const Eigen::Matrix4f& tcpTarget, const Eigen::Vector3f& handJointsTarget, float dt);
+ void addKeypoint(const Eigen::Matrix4f& tcpTarget,
+ const armarx::NameValueMap& handJointsTarget,
+ float dt);
size_t getKeypointCount() const;
- void insertKeypoint(size_t index, const Eigen::Matrix4f& tcpTarget, const Eigen::Vector3f& handJointsTarget, float dt);
+ void insertKeypoint(size_t index,
+ const Eigen::Matrix4f& tcpTarget,
+ const armarx::NameValueMap& handJointsTarget,
+ float dt);
void removeKeypoint(size_t index);
- void replaceKeypoint(size_t index, const Eigen::Matrix4f& tcpTarget, const Eigen::Vector3f& handJointsTarget, float dt);
+ void replaceKeypoint(size_t index,
+ const Eigen::Matrix4f& tcpTarget,
+ const armarx::NameValueMap& handJointsTarget,
+ float dt);
void setKeypointDt(size_t index, float dt);
@@ -103,7 +118,7 @@ namespace armarx
std::vector<Eigen::Vector3f> getAllKeypointPositions();
std::vector<Eigen::Matrix3f> getAllKeypointOrientations();
- Eigen::Vector3f GetHandValues(float t);
+ armarx::NameValueMap GetHandValues(float t);
Eigen::Vector3f GetPositionDerivative(float t);
@@ -118,18 +133,21 @@ namespace armarx
Length calculateLength() const;
- GraspTrajectoryPtr getTranslatedAndRotated(const Eigen::Vector3f& translation, const Eigen::Matrix3f& rotation);
+ 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());
-
+ 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);
@@ -139,13 +157,10 @@ namespace armarx
static GraspTrajectoryPtr ReadFromString(const std::string& xml);
private:
-
void updateKeypointMap();
private:
std::vector<KeypointPtr> keypoints;
std::map<float, size_t> keypointMap;
-
-
};
-}
+} // namespace armarx
--
GitLab