33 #include <RobotAPI/interface/units/RobotUnit/NJointCartesianVelocityControllerWithRamp.h>
49 using namespace HandOverGroup;
52 ReachOutToHumanPose::SubClassRegistry ReachOutToHumanPose::Registry(ReachOutToHumanPose::GetName(), &ReachOutToHumanPose::CreateInstance);
54 #define ENABLE_DEBUG_DRAWER 0
57 void ReachOutToHumanPose::onEnter()
65 void ReachOutToHumanPose::run()
71 c->getOpenPoseEstimation()->start3DPoseEstimation();
77 std::vector<filters::AverageFilter> targetPositionFilter(3), targetVelocityFilter(3);
78 for (
auto& filter : targetPositionFilter)
80 filter.windowFilterSize = in.getTargetPositionAverageFilterSize();
82 for (
auto& filter : targetVelocityFilter)
84 filter.windowFilterSize = in.getTargetPositionAverageFilterSize();
86 float maxTCPVelocity = in.getMaxTCPVelocity();
87 float considerForceDistanceThreshold = in.getConsiderForceDistanceThreshold();
88 auto robot = getLocalRobot();
89 auto cameraRobot = getLocalRobot();
92 forceMagnitudeAvgFilter.windowFilterSize = 10;
94 getHeadIKUnit()->request();
97 ARMARX_INFO <<
"InitialHeadIKTarget(): " << in.getInitialHeadIKTarget()->output();
98 getHeadIKUnit()->setHeadTarget(in.getHeadIKNodeSet(), in.getInitialHeadIKTarget());
101 auto kinematicChainName = in.getKinematicChainName();
105 auto nodeset = robot->getRobotNodeSet(kinematicChainName);
106 auto tcp = nodeset->getTCP();
107 auto kinematicRootName = nodeset->getKinematicRoot()->getName();
109 ARMARX_INFO <<
VAROUT(kinematicRootName) <<
" position in root: " << nodeset->getKinematicRoot()->getPositionInRootFrame();
111 DatafieldRefPtr forceTorqueRef = DatafieldRefPtr::dynamicCast(getForceTorqueObserver()->getDataFieldRef(
new DataFieldIdentifier(getForceTorqueObserver()->getObserverName(),
112 in.getFTSensorName(),
"forces")));
115 NJointCartesianVelocityControllerWithRampConfigPtr ctrlCfg =
new NJointCartesianVelocityControllerWithRampConfig();
116 ctrlCfg->KpJointLimitAvoidance = 0;
117 ctrlCfg->jointLimitAvoidanceScale = 1;
119 ctrlCfg->nodeSetName = kinematicChainName;
120 ctrlCfg->tcpName = tcp->getName();
121 ctrlCfg->maxNullspaceAcceleration = 2;
122 ctrlCfg->maxPositionAcceleration = in.getTCPAcceleration();
123 ctrlCfg->maxOrientationAcceleration = 1;
126 posController.
maxOriVel = in.getMaxOrientationVelocity();
127 posController.
maxPosVel = in.getMaxTCPVelocity();
128 posController.
KpOri = in.getKpOrientation();
129 posController.
KpPos = in.getKp();
131 auto ctrlName =
"ReachOutToHumanTCPCtrl";
134 if (
auto oldCtrl =
getRobotUnit()->getNJointController(ctrlName))
137 while (oldCtrl->isControllerActive())
139 oldCtrl->deactivateController();
150 NJointCartesianVelocityControllerWithRampInterfacePrx ctrl = NJointCartesianVelocityControllerWithRampInterfacePrx::checkedCast(baseCtrl);
153 ctrl->activateController();
155 auto targetOrientation = in.getHandOrientation();
160 const Eigen::Vector3f offset {0, 0, 300};
161 Eigen::Vector3f idlePosi = tcp->getPositionInRootFrame() + offset;
163 idlePosi, robot->getRootNode()->getName(), robot->getName());
166 bool drawDebugInfos = in.getDrawDebugInfos();
168 while (!isRunningTaskStopped())
173 auto timeSinceSinceStart = now - startTime;
177 c->getImageSourceSelectionTopic()->setImageSource(
"OpenPoseEstimationResult", 5000);
180 std::vector<Keypoint3DMap> keypointList;
182 std::tie(timestamp, keypointList) =
c->getPoseData()->getLatestData();
186 Keypoint3DMap poseData = getClosestPerson(keypointList);
187 int validKeypoints = 0;
188 for (
auto& pair : poseData)
190 if (isKeypointValid(pair.second))
197 Keypoint3D rwrist = poseData[
"RWrist"];
199 bool validTarget =
false;
200 if (isKeypointValid(rwrist))
202 rwristFilter.
update(now.toMicroSeconds(),
new Variant(rwristPosition));
207 Vector3Ptr filteredRWristPosition = VariantPtr::dynamicCast(tmp)->get<
Vector3>();
213 #if ENABLE_DEBUG_DRAWER
214 this->getDebugDrawerTopic()->setSphereVisu(
"HandOverState",
"rwrist", globalrWristPosition, DrawColor {1.0f, 0.0f, 0.0f, rwrist.confidence}, 30);
217 .position(globalrWristPosition->toEigen())
221 getDebugObserver()->setDebugChannel(
224 {
"rwristpoint_z",
new Variant(rwristPosition->z) },
225 {
"rwristpoint_z_filtered",
new Variant(filteredRWristPosition->z) },
229 rwristPosition = filteredRWristPosition;
232 Keypoint3D lwrist = poseData[
"LWrist"];
234 if (isKeypointValid(lwrist))
236 lwristFilter.update(now.toMicroSeconds(),
new Variant(lwristPosition));
238 tmp = lwristFilter.getValue();
241 lwristPosition = VariantPtr::dynamicCast(tmp)->get<
Vector3>();
247 #if ENABLE_DEBUG_DRAWER
248 this->getDebugDrawerTopic()->setSphereVisu(
"HandOverState",
"lwrist", globallWristPosition, DrawColor {0.0f, 1.0f, 0.0f, lwrist.confidence}, 30);
251 .position(globallWristPosition->toEigen())
262 Vector3Ptr currentHandPosition = leftHandDistance < rightHandDistance ? lwristPosition : rwristPosition;
266 getDebugObserver()->setDebugChannel(
269 {
"zDiffLShoulder",
new Variant(
std::abs(poseData[
"LWrist"].z - poseData[
"LShoulder"].z)) },
270 {
"zDiffNeckR",
new Variant(
std::abs(poseData[
"RWrist"].z - poseData[
"Neck"].z)) },
271 {
"zDiffNeckL",
new Variant(
std::abs(poseData[
"LWrist"].z - poseData[
"Neck"].z)) }
276 Eigen::Vector3f headIkTarget = getHeadIKTarget(poseData);
278 globalHeadIkTarget->z =
std::max(in.getMinimumGazeZPosition(), globalHeadIkTarget->z);
280 if (validKeypoints > 3 && !in.getHeadIKNodeSet().empty() && headIkTarget.norm() > 0)
284 #if ENABLE_DEBUG_DRAWER
285 this->getDebugDrawerTopic()->setSphereVisu(
"HandOverState",
"Median", globalHeadIkTarget, DrawColor {1.0f, 1.0f, 0.0f, 1.0f}, 50);
288 .position(globalHeadIkTarget->toEigen())
289 .
color(simox::Color::yellow())
293 headIKTargetFilter.
update(now.toMicroSeconds(),
new Variant(globalHeadIkTarget));
294 auto tmp = headIKTargetFilter.
getValue();
300 if (!lastIKTarget || (lastIKTarget->toEigen() - globalFilteredMedianKeypoint->toEigen()).norm() > 100)
302 getHeadIKUnit()->setHeadTarget(in.getHeadIKNodeSet(), globalFilteredMedianKeypoint);
303 lastIKTarget = globalFilteredMedianKeypoint;
310 #if ENABLE_DEBUG_DRAWER
311 this->getDebugDrawerTopic()->removeSphereVisu(
"HandOverState",
"Median");
318 float currentHumanReachDistance = headIkTarget.norm() == 0 && currentHandPosition->toEigen().norm() > 100 ? 0 :
std::abs((currentHandPosition->toEigen() - headIkTarget)(2));
319 bool humanIsReachingOut = currentHumanReachDistance > in.getHumanMinimumReachOutDistance();
321 Eigen::Vector3f currentTargetVec = currentHandPosition->toEigen();
322 for (
int i = 0; i < 3; ++i)
324 targetPositionFilter.at(i).update(now.toMicroSeconds(),
new Variant(currentTargetVec(i)));
326 Eigen::Vector3f currentFilteredTargetPositionVec;
327 for (
int i = 0; i < 3; ++i)
329 currentFilteredTargetPositionVec(i) = targetPositionFilter.at(i).getValue()->getFloat();
333 targetPositioninRootFrame->changeFrame(robot, robot->getRootNode()->getName());
339 targetInKinematicRootFrame->changeFrame(robot, kinematicRootName);
340 float reachDistanceToTarget = targetInKinematicRootFrame->toEigen().norm();
342 getDebugObserver()->setDebugDatafield(
"ReachOutToHuman",
"reachDistanceToTarget",
new Variant(reachDistanceToTarget));
343 float reachDistanceToTargetClamped = reachDistanceToTarget;
344 if (reachDistanceToTarget > in.getDistanceThresholdSoft() && reachDistanceToTarget < in.getDistanceThreshold())
346 Eigen::Vector3f targetPositioninKinematicRootFrameClamped = targetInKinematicRootFrame->toEigen().normalized() * in.getDistanceThresholdSoft();
347 targetPositioninRootFrame =
new FramedPosition(targetPositioninKinematicRootFrameClamped, targetInKinematicRootFrame->frame,
348 targetInKinematicRootFrame->agent);
349 targetPositioninRootFrame->changeFrame(robot, robot->getRootNode()->getName());
350 reachDistanceToTargetClamped = targetPositioninKinematicRootFrameClamped.norm();
355 getDebugObserver()->setDebugChannel(
358 {
"targetPositioninRootFrameX",
new Variant(targetPositioninRootFrame->x) },
359 {
"targetPositioninRootFrameY",
new Variant(targetPositioninRootFrame->y) },
360 {
"targetPositioninRootFrameZ",
new Variant(targetPositioninRootFrame->z) },
361 {
"targetPositioninRootNorm",
new Variant(targetPositioninRootFrame->toEigen().norm()) },
362 {
"reachDistanceToTargetClamped",
new Variant(reachDistanceToTargetClamped) }
366 float distanceToTarget = (tcp->getPositionInRootFrame() - targetPositioninRootFrame->toEigen()).
norm();
368 bool returnToIdlePose = reachDistanceToTarget > in.getDistanceThreshold() || !validTarget || !humanIsReachingOut || validKeypoints < 5 || timeSinceSinceStart.toMilliSeconds() < in.getMinWaitBeforeReachTime();
369 if (returnToIdlePose)
371 targetPositioninRootFrame = in.getIdlePosition();
375 idlePosition = in.getIdlePosition();
376 targetPositioninRootFrame->y -= in.getTargetYOffset();
382 FramedPositionPtr globalTargetPosition = targetPositioninRootFrame->toGlobal(robot);
385 #if ENABLE_DEBUG_DRAWER
386 getDebugDrawerTopic()->setSphereVisu(
"HandOverState",
"TargetPosition", globalTargetPosition, DrawColor {0.0f, 1.0f, 1.0f, 1.0}, 30);
387 getDebugDrawerTopic()->setLineVisu(
"HandOverState",
"ReachOutLine", globalTargetPosition, globalHeadIkTarget, 5,
388 humanIsReachingOut ? DrawColor {0.0f, 1.0f, 0.0f, 1.0} : DrawColor {1.0f, 0, 0, 1.0});
391 .position(globalTargetPosition->toEigen())
392 .
color(simox::Color::cyan())
394 vizLayer.
add(
viz::Cylinder(
"ReachOutLine").fromTo(globalTargetPosition->toEigen(), globalHeadIkTarget->toEigen())
400 pid.
update(duration.toMicroSecondsDouble(), tcp->getPositionInRootFrame(), targetPositioninRootFrame->toEigen());
402 if (targetVelocityVec.norm() > maxTCPVelocity)
404 targetVelocityVec = targetVelocityVec.normalized() * maxTCPVelocity;
406 if (distanceToTarget < considerForceDistanceThreshold)
408 targetVelocityVec = Eigen::Vector3f::Zero();
409 if (!nulledForceTorqueRef)
411 nulledForceTorqueRef = DatafieldRefPtr::dynamicCast(getForceTorqueObserver()->createNulledDatafield(forceTorqueRef));
414 float forceMagnitude = nulledFT->toEigen().norm();
417 getDebugObserver()->setDebugChannel(
420 {
"ForceX",
new Variant(nulledFT->x) },
421 {
"ForceZ",
new Variant(nulledFT->z) },
422 {
"ForceMagnitude",
new Variant(forceMagnitude) }
426 forceMagnitudeAvgFilter.
update(now.toMicroSeconds(),
new Variant(forceMagnitude));
427 float forceMagnitudeFiltered = forceMagnitudeAvgFilter.
getValue()->getFloat();
430 getDebugObserver()->setDebugDatafield(
"ReachOutToHuman",
"forceMagnitudeFiltered",
new Variant(forceMagnitudeFiltered));
434 if (forceMagnitude > in.getForceThreshold())
437 this->emitForceThresholdReached();
443 nulledForceTorqueRef =
nullptr;
457 Eigen::VectorXf
cv = posController.
calculate(
Pose(targetOrientation->toEigen(), targetPositioninRootFrame->toEigen()).toEigen(), VirtualRobot::IKSolver::All);
458 ctrl->setTargetVelocity(
cv(0),
cv(1),
cv(2),
cv(3),
cv(4),
cv(5));
472 ctrl->deactivateAndDeleteController();
477 if (in.isObjectIDSet() && in.getObjectID().find(
"/") != std::string::npos)
479 objpose::DetachObjectFromRobotNodeInput
input;
482 objpose::DetachObjectFromRobotNodeOutput result = getObjectPoseStorage()->detachObjectFromRobotNode(
input);
485 else if (in.isObjectIDSet())
487 ARMARX_IMPORTANT <<
"Not detaching object '" << in.getObjectID() <<
" because it does not look like an ObjectID.";
491 ARMARX_IMPORTANT <<
"Not detaching object in ObjectPoseStorage because 'ObjectID' is not set.";
501 void ReachOutToHumanPose::onExit()
505 #if ENABLE_DEBUG_DRAWER
506 getDebugDrawerTopic()->removeLayer(
"HandOverState");
508 if (in.getStopOpenPoseAfterRun())
511 getOpenPoseEstimation()->stop();
519 Eigen::Vector3f ReachOutToHumanPose::getHeadIKTarget(
const Keypoint3DMap& keypointMap)
540 std::vector<std::pair<std::string, float>> keypointPriorityOrderWithOffset =
542 {
"Neck", 20}, {
"RShoulder", 20}, {
"LShoulder", 20}, {
"Nose", 0}, {
"RElbow", 300}, {
"LElbow", 300},
543 {
"RWrist", 500}, {
"LWrist", 500}, {
"LHip", 600}, {
"RHip", 600}, {
"RKnee", 1100}, {
"LKnee", 1100},
545 for (
auto& pair : keypointPriorityOrderWithOffset)
547 auto it = keypointMap.find(pair.first);
548 if (it != keypointMap.end() && isKeypointValid(it->second))
551 Eigen::Vector3f result(it->second.x, it->second.y - pair.second, it->second.z);
556 return Eigen::Vector3f::Zero();
559 Eigen::VectorXf ReachOutToHumanPose::calculateMedian(
const Keypoint3DMap& keypointMap)
561 if (keypointMap.empty())
563 return Eigen::Vector3f::Zero();
565 Eigen::MatrixXf keypointPositions(keypointMap.size(), 3);
566 std::vector<std::vector<float>>
values(3, Ice::FloatSeq(keypointMap.size(), 0.0));
568 for (
auto& pair : keypointMap)
570 values.at(0).at(i) = pair.second.x;
571 values.at(1).at(i) = pair.second.y;
572 values.at(2).at(i) = pair.second.z;
580 size_t center = keypointMap.size() / 2;
581 return Eigen::Vector3f(
values.at(0).at(center),
586 Eigen::VectorXf ReachOutToHumanPose::calculateCentroid(
const Keypoint3DMap& keypointMap)
588 Eigen::MatrixXf keypointPositions(keypointMap.size(), 3);
590 for (
auto& pair : keypointMap)
592 keypointPositions(i, 0) = pair.second.x;
593 keypointPositions(i, 1) = pair.second.y;
594 keypointPositions(i, 2) = pair.second.z;
597 return keypointPositions.colwise().mean();
600 Keypoint3DMap ReachOutToHumanPose::getClosestPerson(
const std::vector<Keypoint3DMap>& trackingData)
603 Keypoint3DMap result;
604 for (
auto& keypointMap : trackingData)
606 if (keypointMap.empty())
610 float distance = calculateMedian(keypointMap).norm();
614 result = keypointMap;
620 bool ReachOutToHumanPose::isKeypointValid(
const Keypoint3D& point)
const
622 if (point.confidence < in.getMinimumKeypointConfidence())
627 return Eigen::Vector3f(point.x, point.y, point.z).norm() > 100;