MotionModelBodySchema.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 "MotionModelBodySchema.h"
25 #include <boost/foreach.hpp>
26 #include <MemoryX/interface/components/LongtermMemoryInterface.h>
28 
29 
30 namespace memoryx
31 {
32  MotionModelBodySchema::MotionModelBodySchema(armarx::RobotStateComponentInterfacePrx robotStateProxy, std::string handNodeName, LongtermMemoryInterfacePrx longtermMemory) : AbstractMotionModel(robotStateProxy)
33  {
34  armarx::SharedRobotInterfacePrx currentRobotSnapshot = robotStateProxy->getRobotSnapshot("MotionModelBodySchemaPrediction");
35  armarx::SharedRobotNodeInterfacePrx handNode = currentRobotSnapshot->getRobotNode(handNodeName);
36 
37  this->longtermMemory = longtermMemory;
38  // obtain the kinematic chain
39 
40  //TODO: how to get the desired frame?? probably only possible in SetPoseAtLastLocalisation()?
41  std::string parentName = handNode->getParent();
42 
43  while (!parentName.empty())
44  {
45  armarx::SharedRobotNodeInterfacePrx node = currentRobotSnapshot->getRobotNode(parentName);
46 
47  if (node->getType() == armarx::eRevolute)
48  {
49  this->parentNodeNames.push_back(parentName);
50  }
51 
52  parentName = node->getParent();
53  }
54 
55  this->model.reset(new KBM(parentNodeNames.size(), 16, 3.14 / 4.0));
56 
57  // Load from longterm memory
58  }
59 
61  {
62  // Store to Longtermmemory
63 
64  }
65 
67  {
68  armarx::PosePtr oldHandNodePose = armarx::PosePtr::dynamicCast(poseAtLastLocalization->referenceRobot->getRobotNode(handNodeName)->getGlobalPose());
69  armarx::PosePtr oldGlobalPoseOfFrame = armarx::PosePtr::dynamicCast(poseAtLastLocalization->referenceRobot->getRobotNode(poseAtLastLocalization->frame)->getGlobalPose());
70 
71  armarx::SharedRobotInterfacePrx currentRobotSnapshot = robotStateProxy->getRobotSnapshot("MotionModelBodySchemaPrediction");
72 
73  // set the proprioception vector
74  Eigen::VectorXf proprioception(this->parentNodeNames.size());
75  unsigned int index = 0;
76  BOOST_FOREACH(std::string nodeName, this->parentNodeNames)
77  {
78  proprioception(index) = currentRobotSnapshot->getRobotNode(nodeName)->getJointValue();
79  index++;
80  }
81 
82  Eigen::VectorXf predictionXf = this->model->predict(proprioception);
83  Eigen::Matrix4f prediction = Eigen::Matrix4f::Map(predictionXf.data());
84 
85  armarx::LinkedPosePtr ret = new armarx::LinkedPose(prediction, currentRobotSnapshot->getRootNode()->getName(), currentRobotSnapshot);
86  return ret;
87  }
88 
89  void MotionModelBodySchema::SetPoseAtLastLocalisation(const armarx::LinkedPoseBasePtr& poseAtLastLocalization, const armarx::PoseBasePtr& globalRobotPoseAtLastLocalization, const Ice::Current& c)
90  {
91  boost::mutex::scoped_lock lock(motionPredictionLock);
92 
93  // get the shape|extereoception|input vector
94  this->poseAtLastLocalization = poseAtLastLocalization;
95  this->globalRobotPoseAtLastLocalization = globalRobotPoseAtLastLocalization;
96 
97  Eigen::Matrix4f mat = armarx::LinkedPosePtr::dynamicCast(this->poseAtLastLocalization)->toEigen();
98 
99  // Transform matrix to column vector
100  Eigen::VectorXf shape = Eigen::VectorXf::Map(mat.data(), mat.rows() * mat.cols());
101 
102  armarx::SharedRobotNodeInterfacePrx handNode = poseAtLastLocalization->referenceRobot->getRobotNode(handNodeName);
103 
104  // set the proprioception vector
105  Eigen::VectorXf proprioception(this->parentNodeNames.size());
106  unsigned int index = 0;
107  BOOST_FOREACH(std::string nodeName, this->parentNodeNames)
108  {
109  proprioception(index) = poseAtLastLocalization->referenceRobot->getRobotNode(nodeName)->getJointValue();
110  index++;
111  }
112 
113  this->model->online(proprioception, shape);
114  }
115 
116 
117 
118  memoryx::MultivariateNormalDistributionBasePtr MotionModelBodySchema::getUncertaintyInternal()
119  {
120  if (uncertaintyAtLastLocalization)
121  {
122  return uncertaintyAtLastLocalization;
123  }
124  else
125  {
126  return NULL;
127  }
128  }
129 
130 }
memoryx::MotionModelBodySchema::GetPredictedPoseInternal
virtual armarx::LinkedPosePtr GetPredictedPoseInternal()
Definition: MotionModelBodySchema.cpp:66
armarx::aron::ret
ReaderT::InputType T & ret
Definition: rw.h:21
memoryx::MotionModelBodySchema::SetPoseAtLastLocalisation
virtual void SetPoseAtLastLocalisation(const armarx::LinkedPoseBasePtr &poseAtLastLocalization, const armarx::PoseBasePtr &globalRobotPoseAtLastLocalization, const Ice::Current &c=Ice::emptyCurrent)
Definition: MotionModelBodySchema.cpp:89
memoryx::MotionModelBodySchema::parentNodeNames
armarx::NameList parentNodeNames
Definition: MotionModelBodySchema.h:55
index
uint8_t index
Definition: EtherCATFrame.h:59
memoryx::MotionModelBodySchema::handNodeName
std::string handNodeName
Definition: MotionModelBodySchema.h:53
armarx::VariantType::Map
const VariantContainerType Map
Definition: StringValueMap.h:247
memoryx
VirtualRobot headers.
Definition: CommonPlacesTester.cpp:48
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:43
memoryx::MotionModelBodySchema::~MotionModelBodySchema
~MotionModelBodySchema()
Definition: MotionModelBodySchema.cpp:60
IceInternal::Handle
Definition: forward_declarations.h:8
MotionModelBodySchema.h
armarx::VariantType::LinkedPose
const VariantTypeId LinkedPose
Definition: LinkedPose.h:43
memoryx::MotionModelBodySchema::longtermMemory
LongtermMemoryInterfacePrx longtermMemory
Definition: MotionModelBodySchema.h:56
memoryx::AbstractMotionModel
Definition: AbstractMotionModel.h:39
memoryx::MotionModelBodySchema::model
std::shared_ptr< memoryx::KBM > model
Definition: MotionModelBodySchema.h:48
memoryx::MotionModelBodySchema::getUncertaintyInternal
virtual MultivariateNormalDistributionBasePtr getUncertaintyInternal()
Definition: MotionModelBodySchema.cpp:118
memoryx::AbstractMotionModel::motionPredictionLock
std::recursive_mutex motionPredictionLock
Definition: AbstractMotionModel.h:95
ProbabilityMeasures.h
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:601
IceInternal::ProxyHandle<::IceProxy::armarx::RobotStateComponentInterface >
memoryx::MotionModelBodySchema::MotionModelBodySchema
MotionModelBodySchema(armarx::RobotStateComponentInterfacePrx robotStateProxy, std::string handNodeName, LongtermMemoryInterfacePrx longtermMemory)
Definition: MotionModelBodySchema.cpp:32