MotionModelAttachedToOtherObject.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  */
25 
26 
29 
33 
34 
35 namespace memoryx
36 {
38  armarx::ChannelRefPtr channelRefToObjectToWhichThisIsAttached) : AbstractMotionModel(robotStateProxy)
39  {
40  this->channelRefToObjectToWhichThisIsAttached = channelRefToObjectToWhichThisIsAttached;
41  channelRefIsValid = true;
42 
43  armarx::FramedPositionPtr positionOfAtObj = channelRefToObjectToWhichThisIsAttached->get<armarx::FramedPosition>("position");
44  armarx::FramedOrientationPtr orientationOfAtObj = channelRefToObjectToWhichThisIsAttached->get<armarx::FramedOrientation>("orientation");
45  armarx::LinkedPosePtr poseOfAtObj = new armarx::LinkedPose(orientationOfAtObj->toEigen(), positionOfAtObj->toEigen(), positionOfAtObj->frame, robotStateProxy->getSynchronizedRobot());
46  // ARMARX_INFO_S << VAROUT(*positionOfAtObj);
47  // ARMARX_INFO_S << VAROUT(*poseOfAtObj);
48  Eigen::Matrix4f globalPoseOfAtObj = poseOfAtObj->toGlobal()->toEigen();
49  // ARMARX_INFO_S << VAROUT(globalPoseOfAtObj);
50  initialGlobalPoseOfObjectToWhichThisIsAttached = new armarx::Variant();
51  armarx::VariantPtr::dynamicCast(initialGlobalPoseOfObjectToWhichThisIsAttached)->setClass(new armarx::MatrixFloat(globalPoseOfAtObj));
52  globalPoseOfObjectToWhichThisIsAttachedAtLocalizationStart = new armarx::Variant();
53  armarx::VariantPtr::dynamicCast(globalPoseOfObjectToWhichThisIsAttachedAtLocalizationStart)->setClass(new armarx::MatrixFloat(globalPoseOfAtObj));
54  }
55 
56 
57 
59  {
60  if (channelRefIsValid)
61  {
62  try
63  {
64  // ARMARX_INFO_S << VAROUT(*poseAtLastLocalization);
65  armarx::LinkedPosePtr oldPoseGlobal = armarx::LinkedPosePtr::dynamicCast(poseAtLastLocalization)->toGlobal();
66 
67  armarx::SharedRobotInterfacePrx currentRobotSnapshot = robotStateProxy->getRobotSnapshot(robotStateProxy->getSynchronizedRobot()->getName());
68 
69  // determine transformation of the object to which this one is attached in global coordinates
70  armarx::FramedPositionPtr positionOfAtObj = armarx::ChannelRefPtr::dynamicCast(channelRefToObjectToWhichThisIsAttached)->get<armarx::FramedPosition>("position");
71  armarx::FramedOrientationPtr orientationOfAtObj = armarx::ChannelRefPtr::dynamicCast(channelRefToObjectToWhichThisIsAttached)->get<armarx::FramedOrientation>("orientation");
72  armarx::LinkedPosePtr poseOfAtObj = new armarx::LinkedPose(orientationOfAtObj->toEigen(), positionOfAtObj->toEigen(), positionOfAtObj->frame, currentRobotSnapshot);
73  armarx::LinkedPosePtr globalPoseOfAtObj = poseOfAtObj->toGlobal();
74  Eigen::Matrix4f initPoseOfAtObj = armarx::VariantPtr::dynamicCast(initialGlobalPoseOfObjectToWhichThisIsAttached)->getClass<armarx::MatrixFloat>()->toEigen();
75  Eigen::Matrix4f trafoOfObjectToWhichThisIsAttached = globalPoseOfAtObj->toEigen() * initPoseOfAtObj.inverse();
76 
77  // ARMARX_VERBOSE_S << "new pose: " << globalPoseOfAtObj->toEigen() * initPoseOfAtObj.inverse() * oldPoseGlobal->toEigen();
78 
79  Eigen::Matrix4f predictedPose = trafoOfObjectToWhichThisIsAttached * oldPoseGlobal->toEigen();
80 
81  armarx::LinkedPosePtr ret = new armarx::LinkedPose(predictedPose, armarx::GlobalFrame, currentRobotSnapshot);
82 
83  return ret;
84  }
85  catch (...)
86  {
88  channelRefIsValid = false;
89  ARMARX_ERROR_S << "The ChannelRef of the object to which this is attached is not valid (anymore). Did you release that object?";
90  }
91  }
92  return armarx::LinkedPosePtr::dynamicCast(poseAtLastLocalization);
93  }
94 
95 
96 
97  void MotionModelAttachedToOtherObject::setPoseAtLastLocalisation(const armarx::LinkedPoseBasePtr& poseAtLastLocalization, const armarx::PoseBasePtr& globalRobotPoseAtLastLocalization, const MultivariateNormalDistributionBasePtr& uncertaintyAtLastLocalization, const Ice::Current& c)
98  {
99  if (channelRefIsValid)
100  {
101  try
102  {
103 
104  std::unique_lock lock(motionPredictionLock);
105  // ARMARX_WARNING_S << "setpose attached: " << VAROUT(*poseAtLastLocalization);
106  this->poseAtLastLocalization = poseAtLastLocalization;
107 
108  // this one is probably unnecessary now
109  this->globalRobotPoseAtLastLocalization = globalRobotPoseAtLastLocalization;
110 
111  initialGlobalPoseOfObjectToWhichThisIsAttached = armarx::VariantPtr::dynamicCast(globalPoseOfObjectToWhichThisIsAttachedAtLocalizationStart)->clone();
112 
113  if (uncertaintyAtLastLocalization)
114  {
115  this->uncertaintyAtLastLocalization = uncertaintyAtLastLocalization;
116  }
117  }
118  catch (...)
119  {
121  channelRefIsValid = false;
122  ARMARX_ERROR_S << "The ChannelRef of the object to which this is attached is not valid (anymore). Did you release that object?";
123  }
124  }
125  }
126 
127 
128 
129  memoryx::MultivariateNormalDistributionBasePtr MotionModelAttachedToOtherObject::getUncertaintyInternal()
130  {
131  if (uncertaintyAtLastLocalization)
132  {
133  return uncertaintyAtLastLocalization;
134  }
135  else
136  {
137  return NULL;
138  }
139  }
140 
141 
143  {
145 
146  armarx::FramedPositionPtr positionOfAtObj = armarx::ChannelRefPtr::dynamicCast(channelRefToObjectToWhichThisIsAttached)->get<armarx::FramedPosition>("position");
147  armarx::FramedOrientationPtr orientationOfAtObj = armarx::ChannelRefPtr::dynamicCast(channelRefToObjectToWhichThisIsAttached)->get<armarx::FramedOrientation>("orientation");
148  armarx::LinkedPosePtr poseOfAtObj = new armarx::LinkedPose(orientationOfAtObj->toEigen(), positionOfAtObj->toEigen(), positionOfAtObj->frame, poseAtLastLocalization->referenceRobot);
149  armarx::LinkedPosePtr globalPoseOfAtObj = poseOfAtObj->toGlobal();
150  armarx::VariantPtr::dynamicCast(globalPoseOfObjectToWhichThisIsAttachedAtLocalizationStart)->setClass(new armarx::MatrixFloat(globalPoseOfAtObj->toEigen()));
151  }
152 
153 }
armarx::Variant
The Variant class is described here: Variants.
Definition: Variant.h:224
armarx::aron::ret
ReaderT::InputType T & ret
Definition: rw.h:21
memoryx::MotionModelAttachedToOtherObject::setPoseAtLastLocalisation
void setPoseAtLastLocalisation(const armarx::LinkedPoseBasePtr &poseAtLastLocalization, const armarx::PoseBasePtr &globalRobotPoseAtLastLocalization, const MultivariateNormalDistributionBasePtr &uncertaintyAtLastLocalization, const Ice::Current &c=Ice::emptyCurrent) override
Definition: MotionModelAttachedToOtherObject.cpp:97
LocalException.h
armarx::viz::toEigen
Eigen::Matrix4f toEigen(data::GlobalPose const &pose)
Definition: Interaction.h:46
memoryx::AbstractMotionModel::savePredictedPoseAtStartOfCurrentLocalization
void savePredictedPoseAtStartOfCurrentLocalization(const Ice::Current &=Ice::emptyCurrent) override
Definition: AbstractMotionModel.cpp:61
MatrixVariant.h
Pose.h
armarx::GlobalFrame
const std::string GlobalFrame
Definition: FramedPose.h:62
memoryx
VirtualRobot headers.
Definition: CommonPlacesTester.cpp:48
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:43
memoryx::MotionModelAttachedToOtherObject::getUncertaintyInternal
MultivariateNormalDistributionBasePtr getUncertaintyInternal() override
Definition: MotionModelAttachedToOtherObject.cpp:129
memoryx::MotionModelAttachedToOtherObject::savePredictedPoseAtStartOfCurrentLocalization
void savePredictedPoseAtStartOfCurrentLocalization(const Ice::Current &c=Ice::emptyCurrent) override
Definition: MotionModelAttachedToOtherObject.cpp:142
IceInternal::Handle< ChannelRef >
armarx::VariantType::LinkedPose
const VariantTypeId LinkedPose
Definition: LinkedPose.h:43
ARMARX_ERROR_S
#define ARMARX_ERROR_S
Definition: Logging.h:209
armarx::FramedPosition
The FramedPosition class.
Definition: FramedPose.h:142
armarx::MatrixFloat
The MatrixFloat class.
Definition: MatrixVariant.h:48
memoryx::AbstractMotionModel
Definition: AbstractMotionModel.h:39
armarx::FramedOrientation
The FramedOrientation class.
Definition: FramedPose.h:199
MotionModelAttachedToOtherObject.h
memoryx::AbstractMotionModel::motionPredictionLock
std::recursive_mutex motionPredictionLock
Definition: AbstractMotionModel.h:95
memoryx::MotionModelAttachedToOtherObject::MotionModelAttachedToOtherObject
MotionModelAttachedToOtherObject()
Definition: MotionModelAttachedToOtherObject.h:58
ProbabilityMeasures.h
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:601
IceInternal::ProxyHandle<::IceProxy::armarx::RobotStateComponentInterface >
Logging.h
armarx::handleExceptions
void handleExceptions()
Definition: Exception.cpp:141
memoryx::MotionModelAttachedToOtherObject::getPredictedPoseInternal
armarx::LinkedPosePtr getPredictedPoseInternal() override
Definition: MotionModelAttachedToOtherObject.cpp:58