34 std::string handNodeName) :
37 this->handNodeName = handNodeName;
38 syncedRobot = robotStateProxy->getSynchronizedRobot();
46 << poseAtLastLocalization->referenceRobot->getName();
47 ARMARX_DEBUG_S <<
"robot name of internal proxy: " << robotStateProxy->getRobotName();
50 poseAtLastLocalization->referenceRobot->getRobotNode(handNodeName)->getGlobalPose());
53 robotStateProxy->getRobotSnapshot(
robotName);
56 currentRobotSnapshot->getRobotNode(handNodeName)->getGlobalPose());
59 newHandNodePose->toEigen() * oldHandNodePose->toEigen().inverse();
60 Eigen::Vector3f kinPosDelta = newHandNodePose->toEigen().block<3, 1>(0, 3) -
61 oldHandNodePose->toEigen().block<3, 1>(0, 3);
64 armarx::LinkedPosePtr::dynamicCast(poseAtLastLocalization)
65 ->toGlobalEigen(poseAtLastLocalization->referenceRobot);
68 predictedPose.block<3, 3>(0, 0) = transformationOldToNewInRootFrame.block<3, 3>(0, 0) *
69 lastLocalization.block<3, 3>(0, 0);
70 predictedPose.block<3, 1>(0, 3) = kinPosDelta + lastLocalization.block<3, 1>(0, 3);
72 if (poseAtLastLocalization->frame.empty())
85 MultivariateNormalDistributionBasePtr
88 if (uncertaintyAtLastLocalization)
91 poseAtLastLocalization->referenceRobot->getRobotNode(handNodeName)
92 ->getPoseInRootFrame());
94 syncedRobot->getRobotNode(handNodeName)->getPoseInRootFrame());
97 float dist = (oldHandNodePose->toEigen().block<3, 1>(0, 3) -
98 newHandNodePose->toEigen().block<3, 1>(0, 3))
104 MultivariateNormalDistributionPtr::dynamicCast(uncertaintyAtLastLocalization)
105 ->toEigenCovariance();
106 Eigen::Matrix3f newUncertainty = oldUncertainty + additionalUncertainty;
109 armarx::Vector3Ptr::dynamicCast(poseAtLastLocalization->position)->toEigen(),
120 const armarx::LinkedPoseBasePtr& poseAtLastLocalization,
121 const armarx::PoseBasePtr& globalRobotPoseAtLastLocalization,
122 const MultivariateNormalDistributionBasePtr& uncertaintyAtLastLocalization,
123 const Ice::Current&
c)
127 globalRobotPoseAtLastLocalization,
128 uncertaintyAtLastLocalization,
130 ARMARX_DEBUG_S <<
"MotionModelRobotHand::setPoseAtLastLocalisation"
131 << this->poseAtLastLocalization->referenceRobot->getName();
132 if (robotStateProxy->getRobotName() !=
133 this->poseAtLastLocalization->referenceRobot->getName())
136 << this->poseAtLastLocalization->agent <<
" to " <<
robotName;
138 armarx::LinkedPosePtr::dynamicCast(this->poseAtLastLocalization)->
toEigen(),
140 robotStateProxy->getRobotSnapshot(
robotName));