utils.h
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  * @author Jianfeng Gao ( jianfeng dot gao at kit dot edu )
17  * @date 2022
18  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
19  * GNU General Public License
20  */
21 
22 #include <Eigen/Geometry>
23 
24 #include <SimoxUtility/json.h>
25 
28 
30 
31 #include <armarx/control/common/mp/aron/MPConfig.aron.generated.h>
33 
34 #include <mplib/core/types.h>
35 
37 {
38 
39  Eigen::VectorXf getEigenVec(nlohmann::json& userConfig,
40  nlohmann::json& defaultConfig,
41  const std::string& entryName,
42  int size = 0,
43  int value = 0);
44 
45  template <typename T>
46  void
47  getEigenVec(nlohmann::json& userConfig,
48  nlohmann::json& defaultConfig,
49  const std::string& entryName,
50  T& vec)
51  {
52  if (userConfig.find(entryName) != userConfig.end())
53  {
54  vec = userConfig[entryName];
55  }
56  else
57  {
58  vec = defaultConfig[entryName];
59  }
60  }
61 
62  void
63  getEigenMatrix(nlohmann::json& userConfig, const std::string& entryName, Eigen::MatrixXf& mat);
64 
65  Eigen::VectorXf getEigenVecWithDefault(nlohmann::json& userConfig,
66  Eigen::VectorXf defaultValue,
67  const std::string& entryName);
68 
69  template <typename T>
70  T
71  getValue(nlohmann::json& userConfig,
72  nlohmann::json& defaultConfig,
73  const std::string& entryName)
74  {
75  if (userConfig.find(entryName) != userConfig.end())
76  {
77  return userConfig[entryName].get<T>();
78  }
79  else
80  {
81  return defaultConfig[entryName].get<T>();
82  }
83  }
84 
85  template <typename T>
86  T
87  getValueWithDefault(nlohmann::json& userConfig, T defaultValue, const std::string& entryName)
88  {
89  if (userConfig.find(entryName) != userConfig.end())
90  {
91  return userConfig[entryName].get<T>();
92  }
93  else
94  {
95  return defaultValue;
96  }
97  }
98 
99  void
100  debugEigenPose(StringVariantBaseMap& datafields, const std::string& name, Eigen::Matrix4f pose);
101  void
102  debugEigenVec(StringVariantBaseMap& datafields, const std::string& name, Eigen::VectorXf vec);
103 
104  void
105  debugStdVec(StringVariantBaseMap& datafields, const std::string& name, std::vector<float> vec);
106 
107  void getError(Eigen::Matrix4f& currentPose,
108  Eigen::Vector3d& targetPosition,
109  Eigen::Quaterniond& targetQuaternion,
110  double& posiError,
111  double& oriError);
112 
113  void to_json(nlohmann::json& j, const Pose& fp);
114 
115  void from_json(const nlohmann::json& j, Pose& fp);
116 
117  void to_json(nlohmann::json& j, const PosePtr& fp);
118 
119  void from_json(const nlohmann::json& j, PosePtr& fp);
120 
121  void to_json(nlohmann::json& j, const PoseBasePtr& fp);
122 
123  void from_json(const nlohmann::json& j, PoseBasePtr& fp);
124 
125  Eigen::Block<Eigen::Matrix4f, 3, 3> getOri(Eigen::Matrix4f& matrix);
126  Eigen::Matrix3f getOri(const Eigen::Matrix4f& matrix);
127 
128  Eigen::Block<Eigen::Matrix4f, 3, 1> getPos(Eigen::Matrix4f& matrix);
129  Eigen::Vector3f getPos(const Eigen::Matrix4f& matrix);
130 
131 
132  PosePtr dVecToPose(const mplib::core::DVec& dvec);
133 
134  /**
135  * @brief create Eigen:Matrix4f from 7D double vector.
136  * @param dvec [x, y, z, qw, qx, qy, qz] format
137  * @return Eigen::Matrix4f \in SE(3)
138  *
139  * Note that, the order of the quaternion elements follows w, x, y, z format
140  */
142 
144 
145  /**
146  * @brief convert Eigen:Matrix4f to 7D double vector.
147  * @param mat Eigen::Matrix4f \in SE(3)
148  * @return vector of format [x, y, z, qw, qx, qy, qz]
149  *
150  * Note that, the order of the quaternion elements follows w, x, y, z format
151  */
153 
154  std::vector<float> poseToFVec(PosePtr& pose);
155 
156  std::vector<float> mat4ToFVec(const Eigen::Matrix4f& mat);
157 
158  std::string dVecToString(const mplib::core::DVec& dvec);
159 
160  Eigen::VectorXf vecToEigen(std::vector<float>& vec);
161 
162  std::string sVecToString(const std::vector<std::string>& vec,
163  const std::string& delimiter = ", ");
164 
165  armarx::control::common::mp::arondto::MPTraj
166  mpTrajFromFile(std::string filename, bool containsHeader = true, bool noTimeStamp = false);
167 
168  /**
169  * @brief create a dummy trajectory that only has zeros for every dimension.
170  * @param dimension the dimension of the trajectory, e.g. 7d for taskspace and
171  * Nd for N-joint kinematic chain.
172  * @param timesteps the total number of timesteps in the trajectory
173  * @return the MPTraj instance
174  *
175  * Note that, we you use this to configure MPs and execute it after, you'll have to set
176  * a proper start and goal for it, either set them in the MPListConfig for the corresponding
177  * MP and call startAll(), or use start("MPName", start, goal, duration_second).
178  * Otherwise, the MP will assume the first and last timestep being the start and goal, which
179  * are all zeros and therefore wrong for your task.
180  */
181  mp::arondto::MPTraj createDummyTraj(const int dimension, const int timesteps = 100);
182 
183  mp::arondto::MPConfig getDefaultMPConfig(
184  MPType mpType,
185  const std::string& name,
186  const std::string& nodeSetName,
187  const double& durationSec = -1.0,
188  const std::vector<mp::arondto::MPTraj>& mpTraj = std::vector<mp::arondto::MPTraj>(),
189  const std::vector<mp::arondto::ListViaPoint>& viaPointList =
190  std::vector<mp::arondto::ListViaPoint>());
191 
192  mp::arondto::MPConfig
193  getDefaultMPConfig(MPType mpType,
194  const std::string& name,
195  const std::string& nodeSetName,
196  const double& durationSec,
197  const std::vector<std::string>& mpTraj = std::vector<std::string>(),
198  const std::vector<mp::arondto::ListViaPoint>& viaPointList =
199  std::vector<mp::arondto::ListViaPoint>());
200 
201  void addViaPoint(mp::arondto::MPConfig& cfg,
202  const double& canValue,
203  const std::vector<double>& viaPoint);
204  void addViaPoint(mp::arondto::MPConfig& cfg,
205  const double& canValue,
206  const std::vector<float>& viaPoint);
207  void addViaPoint(mp::arondto::MPConfig& cfg,
208  const double& canValue,
209  const Eigen::VectorXf& viaPoint);
210 
211  void validateTSViaPoint(std::vector<double>& viaPoint, std::vector<double>& viaPointReference);
212 
213  void
214  addViaPoint(mp::arondto::MPConfig& cfg, const double& canValue, const Eigen::Matrix4f& viaPose);
215 
216  void appendToViapointList(std::vector<mp::arondto::ListViaPoint>& vpList,
217  const double canonicalValue,
218  const std::vector<double>& viaPoint);
219  void appendToViapointList(std::vector<mp::arondto::ListViaPoint>& vpList,
220  const double canonicalValue,
221  const std::vector<float>& viaPoint);
222  void appendToViapointList(std::vector<mp::arondto::ListViaPoint>& vpList,
223  const double canonicalValue,
224  const Eigen::VectorXf& viaPoint);
225  void appendToViapointList(std::vector<mp::arondto::ListViaPoint>& vpList,
226  const double canonicalValue,
227  const Eigen::Matrix4f& viaPose);
228 
229  float getDeltaAngleBetweenRotMat(const Eigen::Matrix3f& rotMat1,
230  const Eigen::Matrix3f& rotMat2);
231  float getDeltaAngleBetweenPose(const Eigen::Matrix4f& pose1, const Eigen::Matrix4f& pose2);
232 
233  bool poseDeviationTooLarge(const Eigen::Matrix4f& currentPose,
234  const Eigen::Matrix4f& desiredPose,
235  float positionThrMM,
236  float angleThrDeg);
237  void reportPoseDeviationWarning(const std::string& who,
238  const Eigen::Matrix4f& pose1,
239  const Eigen::Matrix4f& pose2,
240  const std::string& namePose1,
241  const std::string& namePose2);
243  const Eigen::Matrix4f& pose2,
244  const std::string& namePose1,
245  const std::string& namePose2,
246  float positionThrMM,
247  float angleThrDeg,
248  const std::string& who);
249 
253 } // namespace armarx::control::common
armarx::control::common::mirrorLeftRightPose
Eigen::Matrix4f mirrorLeftRightPose(Eigen::Matrix4f &sourcePose)
Definition: utils.cpp:669
armarx::control::common::debugEigenPose
void debugEigenPose(StringVariantBaseMap &datafields, const std::string &name, Eigen::Matrix4f pose)
Definition: utils.cpp:113
armarx::StringVariantBaseMap
std::map< std::string, VariantBasePtr > StringVariantBaseMap
Definition: ManagedIceObject.h:110
armarx::control::common::getOri
Eigen::Block< Eigen::Matrix4f, 3, 3 > getOri(Eigen::Matrix4f &matrix)
Definition: utils.cpp:200
armarx::control::common::createDummyTraj
mp::arondto::MPTraj createDummyTraj(const int dimension, const int timesteps)
create a dummy trajectory that only has zeros for every dimension.
Definition: utils.cpp:426
armarx::control::common::getValueWithDefault
T getValueWithDefault(nlohmann::json &userConfig, T defaultValue, const std::string &entryName)
Definition: utils.h:87
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:650
armarx::control::common::getEigenVecWithDefault
Eigen::VectorXf getEigenVecWithDefault(nlohmann::json &userConfig, Eigen::VectorXf defaultValue, const std::string &entryName)
Definition: utils.cpp:76
armarx::control::common::detectAndReportPoseDeviationWarning
bool detectAndReportPoseDeviationWarning(const Eigen::Matrix4f &pose1, const Eigen::Matrix4f &pose2, const std::string &namePose1, const std::string &namePose2, float positionThrMM, float angleThrDeg, const std::string &who)
Definition: utils.cpp:646
Pose.h
armarx::control::common::validateTSViaPoint
void validateTSViaPoint(std::vector< double > &viaPoint, std::vector< double > &viaPointReference)
Definition: utils.cpp:517
armarx::control::common::debugStdVec
void debugStdVec(StringVariantBaseMap &datafields, const std::string &name, std::vector< float > vec)
Definition: utils.cpp:134
armarx::control::common::mirrorLeftRightOri
Eigen::Matrix4f mirrorLeftRightOri(Eigen::Matrix4f &sourcePose)
Definition: utils.cpp:677
armarx::control::common::getError
void getError(Eigen::Matrix4f &currentPose, Eigen::Vector3d &targetPosition, Eigen::Quaterniond &targetQuaternion, double &posiError, double &oriError)
Definition: utils.cpp:94
armarx::control::common::from_json
void from_json(const nlohmann::json &j, Pose &fp)
Definition: utils.cpp:155
armarx::control::common::reportPoseDeviationWarning
void reportPoseDeviationWarning(const std::string &who, const Eigen::Matrix4f &pose1, const Eigen::Matrix4f &pose2, const std::string &namePose1, const std::string &namePose2)
Definition: utils.cpp:623
armarx::control::common
This file is part of ArmarX.
Definition: aron_conversions.cpp:29
type.h
IceInternal::Handle< Pose >
armarx::control::common::poseToFVec
std::vector< float > poseToFVec(PosePtr &pose)
Definition: utils.cpp:273
armarx::control::common::dVecToString
std::string dVecToString(const mplib::core::DVec &dvec)
Definition: utils.cpp:301
cxxopts::value
std::shared_ptr< Value > value()
Definition: cxxopts.hpp:855
ManagedIceObject.h
armarx::control::common::poseDeviationTooLarge
bool poseDeviationTooLarge(const Eigen::Matrix4f &currentPose, const Eigen::Matrix4f &desiredPose, float positionThrMM, float angleThrDeg)
Definition: utils.cpp:611
armarx::control::common::mp::DVec
Ice::DoubleSeq DVec
Definition: MP.h:47
armarx::control::common::MPType
MPType
Definition: type.h:60
armarx::control::common::getPos
Eigen::Block< Eigen::Matrix4f, 3, 1 > getPos(Eigen::Matrix4f &matrix)
Definition: utils.cpp:212
armarx::control::common::getDeltaAngleBetweenPose
float getDeltaAngleBetweenPose(const Eigen::Matrix4f &pose1, const Eigen::Matrix4f &pose2)
Definition: utils.cpp:605
armarx::control::common::poseToDVec
mplib::core::DVec poseToDVec(PosePtr &pose)
Definition: utils.cpp:245
armarx::control::common::dVecToMat4
Eigen::Matrix4f dVecToMat4(const mplib::core::DVec &dvec)
create Eigen:Matrix4f from 7D double vector.
Definition: utils.cpp:230
filename
std::string filename
Definition: VisualizationRobot.cpp:86
armarx::control::common::dVecToPose
PosePtr dVecToPose(const mplib::core::DVec &dvec)
Definition: utils.cpp:224
armarx::control::common::debugEigenVec
void debugEigenVec(StringVariantBaseMap &datafields, const std::string &name, Eigen::VectorXf vec)
Definition: utils.cpp:125
armarx::control::common::vecToEigen
Eigen::VectorXf vecToEigen(std::vector< float > &vec)
Definition: utils.cpp:312
armarx::control::common::getDeltaAngleBetweenRotMat
float getDeltaAngleBetweenRotMat(const Eigen::Matrix3f &rotMat1, const Eigen::Matrix3f &rotMat2)
Definition: utils.cpp:594
armarx::control::common::getValue
T getValue(nlohmann::json &userConfig, nlohmann::json &defaultConfig, const std::string &entryName)
Definition: utils.h:71
armarx::control::common::mpTrajFromFile
mp::arondto::MPTraj mpTrajFromFile(std::string filepath, bool containsHeader, bool noTimeStamp)
Definition: utils.cpp:333
armarx::Pose
The Pose class.
Definition: Pose.h:242
armarx::control::common::mat4ToFVec
std::vector< float > mat4ToFVec(const Eigen::Matrix4f &mat)
Definition: utils.cpp:280
armarx::control::common::appendToViapointList
void appendToViapointList(std::vector< mp::arondto::ListViaPoint > &vpList, const double canonicalValue, const std::vector< double > &viaPoint)
Definition: utils.cpp:548
armarx::control::common::mat4ToDVec
mplib::core::DVec mat4ToDVec(const Eigen::Matrix4f &mat)
convert Eigen:Matrix4f to 7D double vector.
Definition: utils.cpp:252
armarx::control::common::to_json
void to_json(nlohmann::json &j, const Pose &fp)
Definition: utils.cpp:143
Eigen::Quaterniond
Quaternion< double, 0 > Quaterniond
Definition: EigenForwardDeclarations.h:62
armarx::control::common::addViaPoint
void addViaPoint(mp::arondto::MPConfig &cfg, const double &canValue, const std::vector< double > &viaPoint)
Definition: utils.cpp:475
armarx::control::common::getEigenVec
Eigen::VectorXf getEigenVec(nlohmann::json &userConfig, nlohmann::json &defaultConfig, const std::string &entryName, int size, int value)
Definition: utils.cpp:23
Logging.h
GfxTL::Matrix3f
MatrixXX< 3, 3, float > Matrix3f
Definition: MatrixXX.h:649
T
float T
Definition: UnscentedKalmanFilterTest.cpp:38
armarx::control::common::sVecToString
std::string sVecToString(const std::vector< std::string > &vec, const std::string &delimiter)
Definition: utils.cpp:318
armarx::control::common::getEigenMatrix
void getEigenMatrix(nlohmann::json &userConfig, const std::string &entryName, Eigen::MatrixXf &mat)
Definition: utils.cpp:49
armarx::control::common::getDefaultMPConfig
mp::arondto::MPConfig getDefaultMPConfig(MPType mpType, const std::string &name, const std::string &nodeSetName, const double &durationSec, const std::vector< mp::arondto::MPTraj > &mpTraj, const std::vector< mp::arondto::ListViaPoint > &viaPointList)
Definition: utils.cpp:435