MotionFileWrapper.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of ArmarX.
3  *
4  * Copyright (C) 2020, 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 RobotComponents::Libraries::MMM
19  * @author Andre Meixner
20  * @date 2020
21  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
22  * GNU General Public License
23  */
24 
25 
26 #include "MotionFileWrapper.h"
27 
28 #include <MMM/Motion/MotionReaderXML.h>
29 #include <MMM/Motion/Legacy/LegacyMotionReaderXML.h>
30 #include <MMM/Motion/Legacy/LegacyMotion.h>
31 #include <MMM/Motion/Plugin/KinematicPlugin/KinematicSensor.h>
32 #include <MMM/Motion/Plugin/ModelPosePlugin/ModelPoseSensor.h>
33 #include <MMM/Model/ModelProcessorWinter.h>
34 
37 
38 #include <VirtualRobot/MathTools.h>
39 #include <VirtualRobot/Robot.h>
40 #include <SimoxUtility/math/convert.h>
41 #include <SimoxUtility/xml/rapidxml/RapidXMLWrapper.h>
42 #include <MMM/Motion/XMLTools.h>
43 
44 #include <Eigen/LU>
45 
46 using namespace armarx;
47 using namespace MMM;
48 
49 MotionFileWrapperPtr MotionFileWrapper::create(const std::string& motionFilePath, double butterworthFilterCutOffFreq, const std::string relativeModelRoot)
50 {
51  MotionFileWrapperPtr wrapper(new MotionFileWrapper(butterworthFilterCutOffFreq));
52 
53  simox::xml::RapidXMLWrapperRootNodePtr root = simox::xml::RapidXMLWrapperRootNode::FromFile(motionFilePath);
54  if (root && root->name() == xml::tag::MMM_ROOT)
55  {
56  if (!root->has_attribute(xml::attribute::VERSION))
57  {
58  if (!wrapper->loadLegacyMotion(motionFilePath))
59  {
60  return nullptr;
61  }
62  }
63  else
64  {
65  std::string version = root->attribute_value(xml::attribute::VERSION);
66  if (version == "2.0")
67  {
68  if (!wrapper->loadMotion(motionFilePath, relativeModelRoot))
69  {
70  return nullptr;
71  }
72  }
73  else if (version == "1.0 ")
74  {
75  if (!wrapper->loadLegacyMotion(motionFilePath, relativeModelRoot))
76  {
77  return nullptr;
78  }
79  }
80  else
81  {
82  ARMARX_ERROR << "Could not load mmm motion with version '" << version << "'";
83  return nullptr;
84  }
85  }
86  }
87  else
88  {
89  return nullptr;
90  }
91 
92  return wrapper;
93 }
94 
95 MotionFileWrapper::MotionFileWrapper(int butterworthFilterCutOffFreq) : butterworthFilterCutOffFreq(butterworthFilterCutOffFreq)
96 {
97 }
98 
99 bool MotionFileWrapper::loadMotion(const std::string& motionFilePath, const std::string relativeModelRoot)
100 {
101  ARMARX_INFO << "Loading motion";
102  MotionRecordingPtr motions;
103  try
104  {
105  MotionReaderXMLPtr motionReader(new MotionReaderXML());
106  motions = motionReader->loadMotionRecording(motionFilePath);
107  }
108  catch (Exception::MMMException& e)
109  {
110  ARMARX_ERROR << "Could not load mmm motion! " << e.what();
111  return false;
112  }
113 
114  Eigen::Matrix4f modelRootTransformation = Eigen::Matrix4f::Identity();
115  for (auto motion : *motions)
116  {
117  ModelPtr model = motion->getModel();
118  if (model)
119  {
120  if (!model->getFilename().empty() && std::filesystem::path(model->getFilename()).stem() == relativeModelRoot)
121  {
122  auto modelPoseSensor = motion->getSensorByType<ModelPoseSensor>();
123  if (modelPoseSensor && modelPoseSensor->getTimesteps().size() > 0)
124  {
125  auto modelPoseSensorMeasurement = modelPoseSensor->getDerivedMeasurement(modelPoseSensor->getTimesteps().at(0));
126  Eigen::Matrix4f rootPose = modelPoseSensorMeasurement->getRootPose();
127  Eigen::Vector3f orientation = simox::math::mat4f_to_rpy(rootPose);
128  modelRootTransformation.block(0, 0, 3, 3) = simox::math::rpy_to_mat3f(0, 0, -orientation(2));
129  modelRootTransformation.block(0, 3, 3, 1) = Eigen::Vector3f(-rootPose(0, 3), -rootPose(1, 3), 0);
130  }
131  }
132  }
133  }
134 
135  for (auto motion : *motions)
136  {
137  MotionDataPtr m(new MotionData());
138 
139  ModelPtr model = motion->getModel();
140  if (model)
141  {
142  m->modelPath = model->getFilename(); // TODO if model could not be loaded!
143  }
144 
145  ModelPoseSensorPtr modelPoseSensor = motion->getSensorByType<ModelPoseSensor>();
146  if (!modelPoseSensor)
147  {
148  ARMARX_ERROR << "Ignoring motion with name '" << motion->getName() << "', because it has no model pose sensor!";
149  break;
150  }
151 
152  std::vector<float> t = modelPoseSensor->getTimesteps();
153  m->timestamp = ::Ice::DoubleSeq(t.begin(), t.end());
154  m->numberOfFrames = m->timestamp.size();
155  for (float timestep : m->timestamp)
156  {
157  ModelPoseSensorMeasurementPtr modelPoseSensorMeasurement = modelPoseSensor->getDerivedMeasurement(timestep);
158  Eigen::Matrix4f pose = modelRootTransformation * modelPoseSensorMeasurement->getRootPose();
159  m->poseTrajData[timestep] = getTrajData(pose);
160  }
161 
162  KinematicSensorList kinematicSensors = motion->getSensorsByType<KinematicSensor>();
163  for (KinematicSensorPtr kinematicSensor : kinematicSensors)
164  {
165  KinematicSensorPtr kSensor = kinematicSensor;
166  std::vector<std::string> jointNames = kinematicSensor->getJointNames();
167  m->jointNames.insert(m->jointNames.end(), jointNames.begin(), jointNames.end());
168  if (butterworthFilterCutOffFreq > 0.0f)
169  {
170  kSensor = KinematicSensorPtr(new KinematicSensor(jointNames));
171  std::vector<filters::ButterworthFilter> filters;
172  for (size_t j = 0; j < jointNames.size(); j++)
173  {
174  filters.push_back(filters::ButterworthFilter(butterworthFilterCutOffFreq, 100, Lowpass, 5));
175  }
176 
177  bool initialized = false;
178  for (float timestep : m->timestamp)
179  {
180  KinematicSensorMeasurementPtr kinematicSensorMeasurement = kinematicSensor->getDerivedMeasurement(timestep); // TODO nullptr
181  if (!kinematicSensorMeasurement)
182  {
183  MMM_ERROR << "No kinematic measurement at timestep " << timestep << " for motion with name '" << motion->getName() << "'";
184  goto end;
185  }
186  Eigen::VectorXf jointAngles = kinematicSensorMeasurement->getJointAngles();
187  for (size_t j = 0; j < jointNames.size(); j++)
188  {
189  if (!initialized)
190  {
191  filters[j].setInitialValue(jointAngles[j]);
192  initialized = true;
193  }
194  filters[j].update(0, new Variant(jointAngles[j]));
195  jointAngles[j] = filters[j].getValue()->getDouble();
196  }
197  kSensor->addSensorMeasurement(KinematicSensorMeasurementPtr(new KinematicSensorMeasurement(timestep, jointAngles)));
198  }
199  }
200 
201  for (float timestep : m->timestamp)
202  {
203  KinematicSensorMeasurementPtr kinematicSensorMeasurement = kSensor->getDerivedMeasurement(timestep);
204  if (kinematicSensorMeasurement)
205  {
206  m->jointTrajData.push_back(getTrajData(kinematicSensorMeasurement->getJointAngles()));
207  }
208  else
209  {
210  MMM_ERROR << "No kinematic measurement at timestep " << timestep << " for motion with name '" << motion->getName() << "'";
211  goto end;
212  }
213  }
214 end:
215  continue;
216  }
217  if (model)
218  {
219  m->scaling = model->getScaling();
220  }
221  else
222  {
223  auto modelProcessor = std::dynamic_pointer_cast<MMM::ModelProcessorWinter>(motion->getModelProcessor());
224  if (modelProcessor)
225  {
226  m->scaling = modelProcessor->getHeight();
227  }
228  else
229  {
230  m->scaling = 1.0;
231  }
232  }
233 
234  motionNames.push_back(motion->getName());
235  motionData[motion->getName()] = m;
236  }
237  return motionData.size();
238 }
239 
240 bool MotionFileWrapper::loadLegacyMotion(const std::string& motionFilePath, const std::string relativeModelRoot)
241 {
242  ARMARX_INFO << "Loading legacy motion";
243  LegacyMotionReaderXML reader;
244  LegacyMotionList motions = reader.loadAllMotions(motionFilePath);
245 
246  Eigen::Matrix4f modelRootTransformation = Eigen::Matrix4f::Identity();
247  for (auto motion : motions)
248  {
249  ModelPtr model = motion->getModel();
250  std::filesystem::path filename = model ? std::filesystem::path(model->getFilename()) : motion->getModelFilePath();
251  if (std::filesystem::path(filename).stem() == relativeModelRoot)
252  {
253  auto motionFrames = motion->getMotionFrames();
254  if (motionFrames.size() > 0)
255  {
256  Eigen::Matrix4f rootPose = motionFrames.at(0)->getRootPose();
257  Eigen::Vector3f orientation = simox::math::mat4f_to_rpy(rootPose);
258  modelRootTransformation.block(0, 0, 3, 3) = simox::math::rpy_to_mat3f(0, 0, -orientation(2));
259  modelRootTransformation.block(0, 3, 3, 1) = Eigen::Vector3f(-rootPose(0, 3), -rootPose(1, 3), 0);
260  }
261  }
262  }
263 
264  for (auto motion : motions)
265  {
266  MotionDataPtr m(new MotionData());
267  ModelPtr model = motion->getModel();
268  m->modelPath = model ? model->getFilename() : motion->getModelFilePath().c_str();
269  m->numberOfFrames = motion->getNumFrames();
270  m->jointNames = motion->getJointNames();
271 
272  if (butterworthFilterCutOffFreq > 0.0f)
273  {
274  for (size_t j = 0; j < motion->getJointNames().size(); j++)
275  {
276  filters::ButterworthFilter filter(butterworthFilterCutOffFreq, 100, Lowpass, 5);
277  filter.setInitialValue(motion->getMotionFrame(0)->joint[j]);
278  for (size_t i = 0; i < motion->getNumFrames(); i++)
279  {
280  filter.update(0, new Variant(motion->getMotionFrame(i)->joint[j]));
281  motion->getMotionFrame(i)->joint[j] = filter.getValue()->getDouble();
282  }
283  }
284  }
285 
286 
287  for (size_t i = 0; i < motion->getNumFrames(); ++i)
288  {
289  MMM::MotionFramePtr frame = motion->getMotionFrame(i);
290  Eigen::Matrix4f pose = modelRootTransformation * frame->getRootPose();
291  m->poseTrajData[frame->timestep] = getTrajData(pose);
292  m->timestamp.push_back(frame->timestep);
293  m->jointTrajData.push_back(getTrajData(frame->joint));
294  }
295 
296  if (model)
297  {
298  m->scaling = model->getScaling();
299  }
300  else
301  {
302  auto modelProcessor = std::dynamic_pointer_cast<MMM::ModelProcessorWinter>(motion->getModelProcessor());
303  if (modelProcessor)
304  {
305  m->scaling = modelProcessor->getHeight();
306  }
307  else
308  {
309  m->scaling = 1.0;
310  }
311  }
312 
313  motionNames.push_back(motion->getName());
314  motionData[motion->getName()] = m;
315  }
316  return motionData.size();
317 }
318 
319 ::Ice::DoubleSeq MotionFileWrapper::getTrajData(const Eigen::Matrix4f& rootPose)
320 {
321  VirtualRobot::MathTools::Quaternion q = VirtualRobot::MathTools::eigen4f2quat(rootPose);
322  ::Ice::DoubleSeq curData;
323  curData.push_back(rootPose(0, 3));
324  curData.push_back(rootPose(1, 3));
325  curData.push_back(rootPose(2, 3));
326  curData.push_back(q.w);
327  curData.push_back(q.x);
328  curData.push_back(q.y);
329  curData.push_back(q.z);
330  return curData;
331 }
332 
333 DoubleSeqSeq MotionFileWrapper::getTrajData(const Eigen::VectorXf& jointAngles)
334 {
335  DoubleSeqSeq curdata;
336  for (int j = 0; j < jointAngles.rows(); j++)
337  {
338  ::Ice::DoubleSeq dimData;
339  dimData.push_back(jointAngles[j]);
340  curdata.push_back(dimData);
341  }
342  return curdata;
343 }
344 
346 {
347  if (motionNames.size() == 0)
348  {
349  return nullptr;
350  }
351  else
352  {
353  return motionData[motionNames.at(0)];
354  }
355 
356 }
357 
359 {
360  for (auto m : motionData)
361  {
362  if (std::filesystem::path(m.second->modelPath).stem() == modelName)
363  {
364  return m.second;
365  }
366  }
367  return nullptr;
368 }
369 
370 MotionDataPtr MotionFileWrapper::getMotionData(const std::string& motionName)
371 {
372  return motionData[motionName];
373 }
374 
376 {
377  ::Ice::StringSeq modelNames;
378  for (auto m : motionData)
379  {
380  modelNames.push_back(std::filesystem::path(m.second->modelPath).stem());
381  }
382  return modelNames;
383 }
384 
386 {
387  return motionNames;
388 }
armarx::Variant
The Variant class is described here: Variants.
Definition: Variant.h:224
armarx::MotionFileWrapperPtr
std::shared_ptr< MotionFileWrapper > MotionFileWrapperPtr
Definition: MotionFileWrapper.h:73
ButterworthFilter.h
armarx::VariantType::Quaternion
const VariantTypeId Quaternion
Definition: Pose.h:39
GfxTL::Identity
void Identity(MatrixXX< N, N, T > *a)
Definition: MatrixXX.h:523
armarx::MotionFileWrapper::getMotionDataByModel
MotionDataPtr getMotionDataByModel(const std::string &modelName)
Definition: MotionFileWrapper.cpp:358
armarx::MotionDataPtr
std::shared_ptr< MotionData > MotionDataPtr
Definition: MotionFileWrapper.h:71
armarx::filters::ButterworthFilter
Definition: ButterworthFilter.h:40
armarx::MotionFileWrapper::getMotionData
MotionDataPtr getMotionData(const std::string &motionName)
Definition: MotionFileWrapper.cpp:370
armarx::MotionFileWrapper
Definition: MotionFileWrapper.h:77
filename
std::string filename
Definition: VisualizationRobot.cpp:83
ARMARX_ERROR
#define ARMARX_ERROR
Definition: Logging.h:189
q
#define q
armarx::channels::KinematicUnitObserver::jointAngles
const KinematicUnitDatafieldCreator jointAngles("jointAngles")
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:601
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:174
MotionFileWrapper.h
armarx::MotionFileWrapper::getFirstMotionData
MotionDataPtr getFirstMotionData()
Definition: MotionFileWrapper.cpp:345
Logging.h
armarx::MotionFileWrapper::getModelNames
::Ice::StringSeq getModelNames()
Definition: MotionFileWrapper.cpp:375
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:28
armarx::MotionFileWrapper::getMotionNames
::Ice::StringSeq getMotionNames()
Definition: MotionFileWrapper.cpp:385
armarx::MotionFileWrapper::create
static MotionFileWrapperPtr create(const std::string &motionFilePath, double butterworthFilterCutOffFreq=0.0, const std::string relativeModelRoot="mmm")
Definition: MotionFileWrapper.cpp:49
armarx::MotionData
Definition: MotionFileWrapper.h:32