MMMExporter.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of ArmarX.
3  *
4  * ArmarX is free software; you can redistribute it and/or modify
5  * it under the terms of the GNU General Public License version 2 as
6  * published by the Free Software Foundation.
7  *
8  * ArmarX is distributed in the hope that it will be useful, but
9  * WITHOUT ANY WARRANTY; without even the implied warranty of
10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11  * GNU General Public License for more details.
12  *
13  * You should have received a copy of the GNU General Public License
14  * along with this program. If not, see <http://www.gnu.org/licenses/>.
15  *
16  * @package ArmarXGuiPlugins::RobotTrajectoryDesigner::ImportExport
17  * @author Liran Dattner
18  * @date 2018
19  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
20  * GNU General Public License
21  */
22 
23 #include "MMMExporter.h"
24 
25 #include <clocale>
26 
27 #include <boost/make_shared.hpp>
28 
29 #include <QString>
30 #include <QtXml>
31 
32 #include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
33 
34 #include "../Model/DesignerTrajectory.h"
35 #include "../Model/UserWaypoint.h"
36 #include <MMM/Model/Model.h>
37 #include <MMM/Model/ModelReaderXML.h>
38 #include <MMM/Motion/Motion.h>
39 #include <MMM/Motion/MotionWriterXML.h>
40 #include <MMM/Motion/Plugin/KinematicPlugin/KinematicSensor.h>
41 #include <MMM/Motion/Plugin/ModelPosePlugin/ModelPoseSensor.h>
42 #include <MMM/RapidXML/rapidxml.hpp>
43 
44 armarx::MMMExporter::MMMExporter(EnvironmentPtr environment)
45 {
46  this->environment = environment;
47 }
48 
49 void
50 armarx::MMMExporter::exportTrajectory(std::vector<armarx::DesignerTrajectoryPtr> trajectories,
51  const std::string file)
52 {
53  //Ensure that locale is en - punctuation for doubles in exported file
54  setlocale(LC_NUMERIC, "en_US.UTF-8");
55  //ARMARX_INFO << "Received " << trajectories.size() << " trajectories";
56  //Create a ModelPoseSensor. The sensors name is the used rns
57  MMM::ModelReaderXMLPtr modelReader(new MMM::ModelReaderXML());
58  MMM::ModelPtr modelptr = modelReader->loadModel(environment->getRobot()->getFilename());
59  MMM::MotionWriterXML motionWriter;
60  MMM::MotionList motions;
61  MMM::RapidXMLWriter writer;
62  MMM::RapidXMLWriterNodePtr metaDataNode = writer.createRootNode("RobotTrajectoryDesignerData");
63  metaDataNode->append_string_node("Robot", environment->getRobot()->getFilename());
64  MMM::Motion* kinematicMotion =
65  new MMM::Motion("RobotTrajectoryDesigner", modelptr, modelptr, nullptr);
66 
67  MMM::ModelPoseSensor kinematicModelPoseSensor = MMM::ModelPoseSensor("Model");
68  MMM::ModelPoseSensorMeasurementPtr kinematicModelPoseSensorMeasurement =
69  boost::make_shared<MMM::ModelPoseSensorMeasurement>(MMM::ModelPoseSensorMeasurement(
70  0,
71  Eigen::Vector3f(1, 1, 1),
72  MMM::Math::matrix4fToEulerXZY(MMM::Math::quat2eigen4f(0, 0, 0, 0))));
73  kinematicModelPoseSensor.addSensorMeasurement(kinematicModelPoseSensorMeasurement);
74  kinematicMotion->addSensor(boost::make_shared<MMM::ModelPoseSensor>(kinematicModelPoseSensor),
75  0);
76 
77  MMM::MotionPtr mptr(kinematicMotion);
78  motions.push_back(mptr);
79 
80  //ARMARX_INFO << "Exporting";
81  for (armarx::DesignerTrajectoryPtr trajectory : trajectories)
82  {
83  std::string location(TRAJECTORYDESIGNER_SOURCEDIR);
84  location += "/data/RobotTrajectoryDesigner/Resources/UserWaypoint.xml";
85  MMM::ModelPtr modelptr = modelReader->loadModel(location);
86  MMM::Motion* m =
87  new MMM::Motion(trajectory->getRns()->getName(), modelptr, modelptr, nullptr);
88  MMM::ModelPoseSensor modelPoseSensor =
89  MMM::ModelPoseSensor(trajectory->getRns()->getName());
90  //Get all userWayPoints
91  std::vector<armarx::UserWaypointPtr> userWayPoints = trajectory->getAllUserWaypoints();
92  /*
93  for (armarx::UserWaypointPtr up : userWayPoints)
94  {
95  ARMARX_INFO << "Exporting point:";
96  ARMARX_INFO << up;
97  }
98  */
99  //Add the userWaypoints to the ModelPoseSensor
100  //ARMARX_INFO << "Now creating ModelPoses";
101  MMM::RapidXMLWriterNodePtr trajectoryNode =
102  metaDataNode->append_node(trajectory->getRns()->getName());
103  MMM::RapidXMLWriterNodePtr userWaypointsNode = trajectoryNode->append_node("UserWaypoints");
104  MMM::RapidXMLWriterNodePtr transitionsNode = trajectoryNode->append_node("Transitions");
105  for (unsigned int index = 0; index < userWayPoints.size(); index++)
106  {
107  armarx::UserWaypointPtr userWayPoint = userWayPoints[index];
108  //Get the reached Pose
109  armarx::Vector3BasePtr pos = userWayPoint->getPose()->position;
110  Eigen::Vector3f position = Eigen::Vector3f(pos->x, pos->y, pos->z);
111  armarx::QuaternionBasePtr quaternion = userWayPoint->getPose()->orientation;
112  Eigen::Matrix4f matrix = MMM::Math::quat2eigen4f(
113  quaternion->qx, quaternion->qy, quaternion->qz, quaternion->qw);
114  Eigen::Vector3f orientation = MMM::Math::matrix4fToEulerXZY(matrix);
115  //Create a measurement containing the reached pose
116  MMM::ModelPoseSensorMeasurementPtr measurement =
117  boost::make_shared<MMM::ModelPoseSensorMeasurement>(MMM::ModelPoseSensorMeasurement(
118  userWayPoint->getUserTimestamp(), position, orientation));
119  //Add the measurement to the sensor
120  modelPoseSensor.addSensorMeasurement(measurement);
121  MMM::RapidXMLWriterNodePtr userpointNode =
122  userWaypointsNode->append_node("UserWaypoint");
123  std::string jointAngleString;
124  for (unsigned int j = 0; j < userWayPoints[index]->getJointAngles().size(); j++)
125  {
126  jointAngleString +=
127  boost::lexical_cast<std::string>(userWayPoints[index]->getJointAngles()[j]) +
128  " ";
129  }
130  MMM::RapidXMLWriterNodePtr jointAngleNode = userpointNode->append_string_node(
131  "JointAngles", jointAngleString.substr(0, jointAngleString.size() - 1));
132  MMM::RapidXMLWriterNodePtr timeOptimalTimeStampNode = userpointNode->append_string_node(
133  "TimeOptimalTimeStamp",
134  boost::lexical_cast<std::string>(
135  (userWayPoints[index]->getTimeOptimalTimestamp())));
136  MMM::RapidXMLWriterNodePtr breakpointNode = userpointNode->append_string_node(
137  "IsBreakPoint",
138  std::to_string((int)(userWayPoints[index]->getIsTimeOptimalBreakpoint())));
139  MMM::RapidXMLWriterNodePtr cartesianSelectionNode = userpointNode->append_string_node(
140  "CartesianSelection",
141  std::to_string((int)(userWayPoints[index]->getIKSelection())));
142  }
143  for (unsigned int index = 0; index < trajectory->getNrOfUserWaypoints() - 1; index++)
144  {
145  MMM::RapidXMLWriterNodePtr transitionNode = transitionsNode->append_node("Transition");
146  MMM::RapidXMLWriterNodePtr timeOptimalDurationNode = transitionNode->append_string_node(
147  "TimeOptimalDuration",
148  boost::lexical_cast<std::string>(
149  trajectory->getTransition(index)->getTimeOptimalDuration()));
150  MMM::RapidXMLWriterNodePtr userDurationNode = transitionNode->append_string_node(
151  "UserDuration",
152  boost::lexical_cast<std::string>(
153  trajectory->getTransition(index)->getUserDuration()));
154  MMM::RapidXMLWriterNodePtr interpolationTypeNode = transitionNode->append_string_node(
155  "InterpolationType",
156  std::to_string((int)(trajectory->getTransition(index)->getInterpolationType())));
157  }
158  MMM::RapidXMLWriterNodePtr interBreakpointTrajectoriesNode =
159  trajectoryNode->append_node("InterBreakpointTrajectories");
160  for (unsigned int index = 0; index < trajectory->getInterBreakpointTrajectories().size();
161  index++)
162  {
163  MMM::RapidXMLWriterNodePtr interBreakpointTrajectoryNode =
164  interBreakpointTrajectoriesNode->append_node("InterBreakpointTrajectory");
165  MMM::KinematicSensorPtr interBreakpointSensor(new MMM::KinematicSensor(
166  trajectory->getRns()->getNodeNames(), trajectory->getRns()->getName()));
167  for (double t : trajectory->getInterBreakpointTrajectories()[index]->getTimestamps())
168  {
169  std::vector<double> positions =
170  trajectory->getInterBreakpointTrajectories()[index]->getStates(t, 0);
171  const Eigen::VectorXd vectord =
172  Eigen::VectorXd::Map(positions.data(), positions.size());
173  const Eigen::VectorXf vector = vectord.cast<float>();
174  MMM::KinematicSensorMeasurement* measurement =
175  new MMM::KinematicSensorMeasurement(t, vector);
176  MMM::KinematicSensorMeasurementPtr measurementptr(measurement);
177  interBreakpointSensor->addSensorMeasurement(measurementptr);
178  }
179  interBreakpointSensor->setName(trajectory->getRns()->getName());
180  interBreakpointSensor->appendSensorXML(interBreakpointTrajectoryNode);
181  }
182  modelPoseSensor.setName(trajectory->getRns()->getName());
183  //ARMARX_INFO << "Adding Sensor for " << trajectory->getRns()->getName();
184  m->addSensor(boost::make_shared<MMM::ModelPoseSensor>(modelPoseSensor), 0);
185 
186  MMM::MotionPtr mptr(m);
187  motions.push_back(mptr);
188 
189  MMM::KinematicSensorPtr kinematicSensor(new MMM::KinematicSensor(
190  trajectory->getRns()->getNodeNames(), trajectory->getRns()->getName()));
191  /*100FPS*/ //for (double t = 0; t < trajectory->getFinalTrajectory()->getLength(); t += 0.01)
192  //Model implements fps conversion for finalTrajectory
193  for (double t : trajectory->getFinalTrajectory()->getTimestamps())
194  {
195  std::vector<double> positions = trajectory->getFinalTrajectory()->getStates(t, 0);
196  const Eigen::VectorXd vectord =
197  Eigen::VectorXd::Map(positions.data(), positions.size());
198  const Eigen::VectorXf vector = vectord.cast<float>();
199  MMM::KinematicSensorMeasurement* measurement =
200  new MMM::KinematicSensorMeasurement(t, vector);
201  MMM::KinematicSensorMeasurementPtr measurementptr(measurement);
202  kinematicSensor->addSensorMeasurement(measurementptr);
203  }
204  kinematicSensor->setName(trajectory->getRns()->getName());
205  kinematicMotion->addSensor(kinematicSensor, 0);
206  }
207  //ARMARX_INFO << "Done with Sensors";
208 
209  //Write the motion to the target xml file
210  //ARMARX_INFO << "Writing motion";
211  std::string motionString = motionWriter.toXMLString(motions, file);
212  //ARMARX_INFO << "Wrote Motion";
213  std::string metaDataString = writer.print(false);
214 
215  //Cut and merge strings
216  motionString = motionString.substr(0, motionString.find("</MMM>"));
217  //ARMARX_INFO << metaDataString.find("<RobotTrajectoryDesignerData") << "loc";
218  metaDataString = metaDataString.substr(metaDataString.find("<RobotTrajectoryDesignerData"));
219  motionString = motionString + metaDataString + '\n' + "</MMM>";
220 
221  //Autoformat strings to xml format
222  QString xmlInput = QString::fromStdString(motionString);
223  QString xmlOutput;
224  QXmlStreamReader qxmlreader(xmlInput);
225  QXmlStreamWriter qxmlwriter(&xmlOutput);
226  qxmlwriter.setAutoFormatting(true);
227  while (!qxmlreader.atEnd())
228  {
229  qxmlreader.readNext();
230  if (!qxmlreader.isWhitespace())
231  {
232  qxmlwriter.writeCurrentToken(qxmlreader);
233  }
234  }
235 
236  std::ofstream output;
237  output.open(file);
238  output << xmlOutput.toStdString();
239  output.close();
240 }
armarx::UserWaypointPtr
std::shared_ptr< UserWaypoint > UserWaypointPtr
Definition: UserWaypoint.h:142
index
uint8_t index
Definition: EtherCATFrame.h:59
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:650
armarx::VariantType::Map
const VariantContainerType Map
Definition: StringValueMap.h:263
armarx::EnvironmentPtr
std::shared_ptr< Environment > EnvironmentPtr
Definition: Environment.h:29
armarx::MMMExporter::exportTrajectory
void exportTrajectory(std::vector< armarx::DesignerTrajectoryPtr > trajectories, const std::string file)
Exports a trajectory to the target file as a MMM file.
Definition: MMMExporter.cpp:50
armarx::to_string
const std::string & to_string(const std::string &s)
Definition: StringHelpers.h:41
CMakePackageFinder.h
armarx::DesignerTrajectoryPtr
std::shared_ptr< DesignerTrajectory > DesignerTrajectoryPtr
Definition: DesignerTrajectory.h:163
armarx::MMMExporter::MMMExporter
MMMExporter(EnvironmentPtr environment)
Definition: MMMExporter.cpp:44
MMMExporter.h