MotionModelRobotHand.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of ArmarX.
3  *
4  * Copyright (C) 2011-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
5  *
6  * ArmarX is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License version 2 as
8  * published by the Free Software Foundation.
9  *
10  * ArmarX is distributed in the hope that it will be useful, but
11  * WITHOUT ANY WARRANTY; without even the implied warranty of
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13  * GNU General Public License for more details.
14  *
15  * You should have received a copy of the GNU General Public License
16  * along with this program. If not, see <http://www.gnu.org/licenses/>.
17  *
18  * @package
19  * @author
20  * @date
21  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
22  * GNU General Public License
23  */
24 #include "MotionModelRobotHand.h"
25 
27 
29 
30 namespace memoryx
31 {
34  std::string handNodeName) :
35  AbstractMotionModel(robotStateProxy)
36  {
37  this->handNodeName = handNodeName;
38  syncedRobot = robotStateProxy->getSynchronizedRobot();
39  robotName = syncedRobot->getName();
40  }
41 
44  {
45  ARMARX_DEBUG_S << "robot name of last pose: "
46  << poseAtLastLocalization->referenceRobot->getName();
47  ARMARX_DEBUG_S << "robot name of internal proxy: " << robotStateProxy->getRobotName();
48  //armarx::PosePtr oldHandNodePose = armarx::PosePtr::dynamicCast(poseAtLastLocalization->referenceRobot->getRobotNode(handNodeName)->getPoseInRootFrame());
49  armarx::PosePtr oldHandNodePose = armarx::PosePtr::dynamicCast(
50  poseAtLastLocalization->referenceRobot->getRobotNode(handNodeName)->getGlobalPose());
51 
52  armarx::SharedRobotInterfacePrx currentRobotSnapshot =
53  robotStateProxy->getRobotSnapshot(robotName);
54  //armarx::PosePtr newHandNodePose = armarx::PosePtr::dynamicCast(currentRobotSnapshot->getRobotNode(handNodeName)->getPoseInRootFrame());
55  armarx::PosePtr newHandNodePose = armarx::PosePtr::dynamicCast(
56  currentRobotSnapshot->getRobotNode(handNodeName)->getGlobalPose());
57 
58  Eigen::Matrix4f transformationOldToNewInRootFrame =
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);
62  //Eigen::Matrix4f lastLocalization = armarx::LinkedPosePtr::dynamicCast(poseAtLastLocalization)->toRootEigen(poseAtLastLocalization->referenceRobot);
63  Eigen::Matrix4f lastLocalization =
64  armarx::LinkedPosePtr::dynamicCast(poseAtLastLocalization)
65  ->toGlobalEigen(poseAtLastLocalization->referenceRobot);
67 
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);
71 
72  if (poseAtLastLocalization->frame.empty())
73  {
74  ARMARX_INFO_S << "poseAtLastLocalization->frame is empty!";
75  }
76 
77  //armarx::LinkedPosePtr ret = new armarx::LinkedPose(predictedPose, poseAtLastLocalization->frame, currentRobotSnapshot);
78  //armarx::LinkedPosePtr ret = new armarx::LinkedPose(predictedPose, currentRobotSnapshot->getRootNode()->getName(), currentRobotSnapshot);
79  //ret->changeToGlobal();
81  new armarx::LinkedPose(predictedPose, armarx::GlobalFrame, currentRobotSnapshot);
82  return ret;
83  }
84 
85  MultivariateNormalDistributionBasePtr
87  {
88  if (uncertaintyAtLastLocalization)
89  {
90  armarx::PosePtr oldHandNodePose = armarx::PosePtr::dynamicCast(
91  poseAtLastLocalization->referenceRobot->getRobotNode(handNodeName)
92  ->getPoseInRootFrame());
93  armarx::PosePtr newHandNodePose = armarx::PosePtr::dynamicCast(
94  syncedRobot->getRobotNode(handNodeName)->getPoseInRootFrame());
95 
96  // additional uncertainty is 0.07 * the distance that the hand moved since the last localization
97  float dist = (oldHandNodePose->toEigen().block<3, 1>(0, 3) -
98  newHandNodePose->toEigen().block<3, 1>(0, 3))
99  .norm();
100  dist *= 0.07;
101  Eigen::Matrix3f additionalUncertainty = dist * dist * Eigen::Matrix3f::Identity();
102 
103  Eigen::Matrix3f oldUncertainty =
104  MultivariateNormalDistributionPtr::dynamicCast(uncertaintyAtLastLocalization)
105  ->toEigenCovariance();
106  Eigen::Matrix3f newUncertainty = oldUncertainty + additionalUncertainty;
107 
109  armarx::Vector3Ptr::dynamicCast(poseAtLastLocalization->position)->toEigen(),
110  newUncertainty);
111  }
112  else
113  {
114  return NULL;
115  }
116  }
117 
118  void
120  const armarx::LinkedPoseBasePtr& poseAtLastLocalization,
121  const armarx::PoseBasePtr& globalRobotPoseAtLastLocalization,
122  const MultivariateNormalDistributionBasePtr& uncertaintyAtLastLocalization,
123  const Ice::Current& c)
124  {
125  std::unique_lock lock(motionPredictionLock);
126  AbstractMotionModel::setPoseAtLastLocalisation(poseAtLastLocalization,
127  globalRobotPoseAtLastLocalization,
128  uncertaintyAtLastLocalization,
129  c);
130  ARMARX_DEBUG_S << "MotionModelRobotHand::setPoseAtLastLocalisation"
131  << this->poseAtLastLocalization->referenceRobot->getName();
132  if (robotStateProxy->getRobotName() !=
133  this->poseAtLastLocalization->referenceRobot->getName())
134  {
135  ARMARX_DEBUG_S << "Setting robot state proxy from "
136  << this->poseAtLastLocalization->agent << " to " << robotName;
137  this->poseAtLastLocalization = new armarx::LinkedPose(
138  armarx::LinkedPosePtr::dynamicCast(this->poseAtLastLocalization)->toEigen(),
140  robotStateProxy->getRobotSnapshot(robotName));
141  }
142  }
143 
144 } // namespace memoryx
MotionModelRobotHand.h
memoryx::MotionModelRobotHand::getPredictedPoseInternal
armarx::LinkedPosePtr getPredictedPoseInternal() override
Definition: MotionModelRobotHand.cpp:43
memoryx::MotionModelRobotHand::setPoseAtLastLocalisation
void setPoseAtLastLocalisation(const armarx::LinkedPoseBasePtr &poseAtLastLocalization, const armarx::PoseBasePtr &globalRobotPoseAtLastLocalization, const MultivariateNormalDistributionBasePtr &uncertaintyAtLastLocalization, const Ice::Current &c=Ice::emptyCurrent) override
Definition: MotionModelRobotHand.cpp:119
armarx::aron::ret
ReaderT::InputType T & ret
Definition: rw.h:13
armarx::viz::toEigen
Eigen::Matrix4f toEigen(data::GlobalPose const &pose)
Definition: Interaction.h:48
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:650
memoryx::MotionModelRobotHand::getUncertaintyInternal
MultivariateNormalDistributionBasePtr getUncertaintyInternal() override
Definition: MotionModelRobotHand.cpp:86
armarx::GlobalFrame
const std::string GlobalFrame
Definition: FramedPose.h:65
memoryx
VirtualRobot headers.
Definition: CommonPlacesTester.cpp:48
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:46
IceInternal::Handle
Definition: forward_declarations.h:8
memoryx::MotionModelRobotHand::MotionModelRobotHand
MotionModelRobotHand()
Definition: MotionModelRobotHand.h:54
GfxTL::Identity
void Identity(MatrixXX< N, N, T > *a)
Definition: MatrixXX.h:570
armarx::VariantType::LinkedPose
const VariantTypeId LinkedPose
Definition: LinkedPose.h:43
ARMARX_DEBUG_S
#define ARMARX_DEBUG_S
Definition: Logging.h:205
memoryx::AbstractMotionModel
Definition: AbstractMotionModel.h:39
memoryx::AbstractMotionModel::setPoseAtLastLocalisation
void setPoseAtLastLocalisation(const armarx::LinkedPoseBasePtr &poseAtLastLocalization, const armarx::PoseBasePtr &globalRobotPoseAtLastLocalization, const MultivariateNormalDistributionBasePtr &uncertaintyAtLastLocalization, const Ice::Current &=Ice::emptyCurrent) override
Definition: AbstractMotionModel.cpp:40
memoryx::AbstractMotionModel::motionPredictionLock
std::recursive_mutex motionPredictionLock
Definition: AbstractMotionModel.h:100
ProbabilityMeasures.h
memoryx::MotionModelRobotHand::syncedRobot
armarx::SharedRobotInterfacePrx syncedRobot
Definition: MotionModelRobotHand.h:49
IceInternal::ProxyHandle<::IceProxy::armarx::RobotStateComponentInterface >
ARMARX_INFO_S
#define ARMARX_INFO_S
Definition: Logging.h:202
Logging.h
GfxTL::Matrix3f
MatrixXX< 3, 3, float > Matrix3f
Definition: MatrixXX.h:649
memoryx::MotionModelRobotHand::robotName
std::string robotName
Definition: MotionModelRobotHand.h:48
armarx::VariantType::MultivariateNormalDistribution
const armarx::VariantTypeId MultivariateNormalDistribution
Definition: ProbabilityMeasures.h:40