diff --git a/source/armarx/control/pointing/core/Pointing.cpp b/source/armarx/control/pointing/core/Pointing.cpp index 91f13a70df14b0934aec97f56f7fd5361fb2abf3..1f6426fe5f3f14f5e73678689b1ca270d648b002 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 e3ef764b9a08f97ca805475b22d68e5c771a8599..bf34712a33735c3b0b0daf05a786b0f393f32c86 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 043f0c3f541cc411b4422538adf807d2520bb591..c7b903cbfcadb43d3302a90380567edd29643cf4 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);