From 1b9076166159af91a09a6616bdc4da4006c774bb Mon Sep 17 00:00:00 2001
From: Patrick Dormanns <patrick.dormanns@student.kit.edu>
Date: Fri, 23 Feb 2024 23:06:48 +0100
Subject: [PATCH] Move wrist into neutral position when pointing
---
.../armarx/control/pointing/core/Pointing.cpp | 42 +++++++++++++------
.../armarx/control/pointing/core/Pointing.h | 8 +++-
.../control/pointing/core/PointingIK.cpp | 6 +--
3 files changed, 38 insertions(+), 18 deletions(-)
diff --git a/source/armarx/control/pointing/core/Pointing.cpp b/source/armarx/control/pointing/core/Pointing.cpp
index 91f13a70..1f6426fe 100644
--- a/source/armarx/control/pointing/core/Pointing.cpp
+++ b/source/armarx/control/pointing/core/Pointing.cpp
@@ -1,5 +1,7 @@
#include "Pointing.h"
+#include <VirtualRobot/RobotNodeSet.h>
+
#include <ArmarXCore/core/time.h>
#include <RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h>
@@ -32,23 +34,18 @@ namespace armarx::control::pointing::core
armarx::TrajectoryPtr traj = computTrajectory(robot, params.side, target);
+ addWristTrajectory(robot, params.side, traj);
+
if (params.handShape)
{
auto shapeJointValues = GetShapeJointValues(robot, params.side, *params.handShape);
addHandTrajectory(robot, traj, shapeJointValues);
}
- if (traj->dim() > 0)
- {
- traj = traj->calculateTimeOptimalTrajectory(
- params.maxVelocity, params.maxAcceleration, 0.0, IceUtil::Time::milliSeconds(10));
+ traj = traj->calculateTimeOptimalTrajectory(
+ params.maxVelocity, params.maxAcceleration, 0.0, IceUtil::Time::milliSeconds(10));
- playTrajectory(traj);
- }
- else
- {
- ARMARX_INFO << "Already pointing at target";
- }
+ playTrajectory(traj);
}
void
@@ -96,16 +93,37 @@ namespace armarx::control::pointing::core
}
ARMARX_VERBOSE << "Done computing trajectory";
- if (traj->size() == 1) // already pointing at target
+ if (traj->size() == 1) // forearm already aligned with target
{
return armarx::TrajectoryPtr(new Trajectory);
}
return traj;
}
+ void
+ Pointing::addWristTrajectory(VirtualRobot::RobotPtr robot, Side side, TrajectoryPtr traj)
+ {
+ auto humanMapping = robot->getHumanMapping();
+ ARMARX_CHECK(humanMapping) << "Robot '" << robot->getName() << "' has no human mapping";
+
+ auto armDescription = (side == LEFT) ? humanMapping->leftArm : humanMapping->rightArm;
+ auto wrist = armDescription.segments.wrist.nodeName;
+
+ auto nodeSetArm = robot->getRobotNodeSet(armDescription.nodeSet);
+
+ // iterate over all joints, starting from the wrist, to move them into a neutral position
+ std::for_each(nodeSetArm->begin() + nodeSetArm->getRobotNodeIndex(wrist),
+ nodeSetArm->end(),
+ [traj](VirtualRobot::RobotNodePtr robotNode) {
+ traj->addDimension({robotNode->getJointValue(), 0.0},
+ Ice::DoubleSeq(),
+ robotNode->getName());
+ });
+ }
+
void
Pointing::addHandTrajectory(VirtualRobot::RobotPtr robot,
- armarx::TrajectoryPtr& traj,
+ armarx::TrajectoryPtr traj,
const std::map<std::string, float>& shapeJointValues)
{
for (const auto& [jointName, targetValue] : shapeJointValues)
diff --git a/source/armarx/control/pointing/core/Pointing.h b/source/armarx/control/pointing/core/Pointing.h
index e3ef764b..bf34712a 100644
--- a/source/armarx/control/pointing/core/Pointing.h
+++ b/source/armarx/control/pointing/core/Pointing.h
@@ -15,7 +15,8 @@ namespace armarx::control::pointing::core
/**
* @brief Implementation of a 'Pointing'-gesture.
*
- * Lets the robot point somewhere by aligning the forearm with the passed target.
+ * Lets the robot point somewhere by aligning the forearm with the passed target, aligning the
+ * wrist, and setting a hand shape.
*
* @ingroup Library-pointing
*/
@@ -95,8 +96,11 @@ namespace armarx::control::pointing::core
armarx::TrajectoryPtr
computTrajectory(VirtualRobot::RobotPtr robot, Side side, Eigen::Vector3f target);
+ void
+ addWristTrajectory(VirtualRobot::RobotPtr robot, Side side, armarx::TrajectoryPtr traj);
+
void addHandTrajectory(VirtualRobot::RobotPtr robot,
- armarx::TrajectoryPtr& traj,
+ armarx::TrajectoryPtr traj,
const std::map<std::string, float>& shapeJointValues);
void playTrajectory(armarx::TrajectoryPtr traj);
diff --git a/source/armarx/control/pointing/core/PointingIK.cpp b/source/armarx/control/pointing/core/PointingIK.cpp
index 043f0c3f..c7b903cb 100644
--- a/source/armarx/control/pointing/core/PointingIK.cpp
+++ b/source/armarx/control/pointing/core/PointingIK.cpp
@@ -15,7 +15,6 @@ namespace armarx::control::pointing::core
auto elbow = armDescription.segments.elbow.nodeName;
auto wrist = armDescription.segments.wrist.nodeName;
auto palm = armDescription.segments.palm.nodeName;
- auto tcp = armDescription.segments.tcp.nodeName;
// actuated joints = all arm joints until (excluding) wrist
auto nodeSetArm = robot->getRobotNodeSet(armDescription.nodeSet);
@@ -24,12 +23,11 @@ namespace armarx::control::pointing::core
// reduced robot must contain shoulder, elbow, wrist and palm for IK to work
reducedRobot_ = std::make_unique<simox::control::simox::robot::Robot>(
- robot, actuatedJoints, std::vector({wrist, palm, tcp}), true);
+ robot, actuatedJoints, std::vector({wrist, palm}), true);
- // use elbow - tcp (instead of wrist) as forearm to line up with the target
ik_ = std::make_unique<simox::control::method::example::PointingIK>(
*reducedRobot_,
- *reducedRobot_->getNode(tcp),
+ *reducedRobot_->getNode(wrist),
*reducedRobot_->getNode(elbow),
side == LEFT ? reducedRobot_->humanMapping()->leftArm
: reducedRobot_->humanMapping()->rightArm);
--
GitLab