Go to the documentation of this file.
22 #include <Eigen/Geometry>
24 #include <SimoxUtility/json.h>
31 #include <armarx/control/common/mp/aron/MPConfig.aron.generated.h>
34 #include <mplib/core/types.h>
39 Eigen::VectorXf
getEigenVec(nlohmann::json& userConfig,
40 nlohmann::json& defaultConfig,
41 const std::string& entryName,
48 nlohmann::json& defaultConfig,
49 const std::string& entryName,
52 if (userConfig.find(entryName) != userConfig.end())
54 vec = userConfig[entryName];
58 vec = defaultConfig[entryName];
63 getEigenMatrix(nlohmann::json& userConfig,
const std::string& entryName, Eigen::MatrixXf& mat);
66 Eigen::VectorXf defaultValue,
67 const std::string& entryName);
72 nlohmann::json& defaultConfig,
73 const std::string& entryName)
75 if (userConfig.find(entryName) != userConfig.end())
77 return userConfig[entryName].get<
T>();
81 return defaultConfig[entryName].get<
T>();
89 if (userConfig.find(entryName) != userConfig.end())
91 return userConfig[entryName].get<
T>();
108 Eigen::Vector3d& targetPosition,
121 void to_json(nlohmann::json& j,
const PoseBasePtr& fp);
123 void from_json(
const nlohmann::json& j, PoseBasePtr& fp);
160 Eigen::VectorXf
vecToEigen(std::vector<float>& vec);
162 std::string
sVecToString(
const std::vector<std::string>& vec,
163 const std::string& delimiter =
", ");
165 armarx::control::common::mp::arondto::MPTraj
181 mp::arondto::MPTraj
createDummyTraj(
const int dimension,
const int timesteps = 100);
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>());
192 mp::arondto::MPConfig
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>());
202 const double& canValue,
203 const std::vector<double>& viaPoint);
205 const double& canValue,
206 const std::vector<float>& viaPoint);
208 const double& canValue,
209 const Eigen::VectorXf& viaPoint);
211 void validateTSViaPoint(std::vector<double>& viaPoint, std::vector<double>& viaPointReference);
217 const double canonicalValue,
218 const std::vector<double>& viaPoint);
220 const double canonicalValue,
221 const std::vector<float>& viaPoint);
223 const double canonicalValue,
224 const Eigen::VectorXf& viaPoint);
226 const double canonicalValue,
240 const std::string& namePose1,
241 const std::string& namePose2);
244 const std::string& namePose1,
245 const std::string& namePose2,
248 const std::string& who);
Eigen::Matrix4f mirrorLeftRightPose(Eigen::Matrix4f &sourcePose)
void debugEigenPose(StringVariantBaseMap &datafields, const std::string &name, Eigen::Matrix4f pose)
std::map< std::string, VariantBasePtr > StringVariantBaseMap
Eigen::Block< Eigen::Matrix4f, 3, 3 > getOri(Eigen::Matrix4f &matrix)
mp::arondto::MPTraj createDummyTraj(const int dimension, const int timesteps)
create a dummy trajectory that only has zeros for every dimension.
T getValueWithDefault(nlohmann::json &userConfig, T defaultValue, const std::string &entryName)
MatrixXX< 4, 4, float > Matrix4f
Eigen::VectorXf getEigenVecWithDefault(nlohmann::json &userConfig, Eigen::VectorXf defaultValue, const std::string &entryName)
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)
void validateTSViaPoint(std::vector< double > &viaPoint, std::vector< double > &viaPointReference)
void debugStdVec(StringVariantBaseMap &datafields, const std::string &name, std::vector< float > vec)
Eigen::Matrix4f mirrorLeftRightOri(Eigen::Matrix4f &sourcePose)
void getError(Eigen::Matrix4f ¤tPose, Eigen::Vector3d &targetPosition, Eigen::Quaterniond &targetQuaternion, double &posiError, double &oriError)
void from_json(const nlohmann::json &j, Pose &fp)
void reportPoseDeviationWarning(const std::string &who, const Eigen::Matrix4f &pose1, const Eigen::Matrix4f &pose2, const std::string &namePose1, const std::string &namePose2)
This file is part of ArmarX.
std::vector< float > poseToFVec(PosePtr &pose)
std::string dVecToString(const mplib::core::DVec &dvec)
std::shared_ptr< Value > value()
bool poseDeviationTooLarge(const Eigen::Matrix4f ¤tPose, const Eigen::Matrix4f &desiredPose, float positionThrMM, float angleThrDeg)
Eigen::Block< Eigen::Matrix4f, 3, 1 > getPos(Eigen::Matrix4f &matrix)
float getDeltaAngleBetweenPose(const Eigen::Matrix4f &pose1, const Eigen::Matrix4f &pose2)
mplib::core::DVec poseToDVec(PosePtr &pose)
Eigen::Matrix4f dVecToMat4(const mplib::core::DVec &dvec)
create Eigen:Matrix4f from 7D double vector.
PosePtr dVecToPose(const mplib::core::DVec &dvec)
void debugEigenVec(StringVariantBaseMap &datafields, const std::string &name, Eigen::VectorXf vec)
Eigen::VectorXf vecToEigen(std::vector< float > &vec)
float getDeltaAngleBetweenRotMat(const Eigen::Matrix3f &rotMat1, const Eigen::Matrix3f &rotMat2)
T getValue(nlohmann::json &userConfig, nlohmann::json &defaultConfig, const std::string &entryName)
mp::arondto::MPTraj mpTrajFromFile(std::string filepath, bool containsHeader, bool noTimeStamp)
std::vector< float > mat4ToFVec(const Eigen::Matrix4f &mat)
void appendToViapointList(std::vector< mp::arondto::ListViaPoint > &vpList, const double canonicalValue, const std::vector< double > &viaPoint)
mplib::core::DVec mat4ToDVec(const Eigen::Matrix4f &mat)
convert Eigen:Matrix4f to 7D double vector.
void to_json(nlohmann::json &j, const Pose &fp)
Quaternion< double, 0 > Quaterniond
void addViaPoint(mp::arondto::MPConfig &cfg, const double &canValue, const std::vector< double > &viaPoint)
Eigen::VectorXf getEigenVec(nlohmann::json &userConfig, nlohmann::json &defaultConfig, const std::string &entryName, int size, int value)
MatrixXX< 3, 3, float > Matrix3f
std::string sVecToString(const std::vector< std::string > &vec, const std::string &delimiter)
void getEigenMatrix(nlohmann::json &userConfig, const std::string &entryName, Eigen::MatrixXf &mat)
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)