Skip to content
Snippets Groups Projects
Commit f2e53e90 authored by Christoph Pohl's avatar Christoph Pohl Committed by ARMAR-DE
Browse files

Remove hard coded dependency on 2 dog for finger joints

parent 2e046090
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
source/RobotAPI/libraries/GraspingUtility/GraspTrajectory.cpp
+ 218
− 84
View file @ f2e53e90
......@@ -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
source/RobotAPI/libraries/GraspingUtility/GraspTrajectory.h
+ 40
− 25
View file @ f2e53e90
......@@ -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
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment