Namespaces | Typedefs | Functions
utils.h File Reference
#include <Eigen/Geometry>
#include <SimoxUtility/json.h>
#include <ArmarXCore/core/ManagedIceObject.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <RobotAPI/libraries/core/Pose.h>
#include <armarx/control/common/mp/aron/MPConfig.aron.generated.h>
#include <armarx/control/common/type.h>
+ Include dependency graph for utils.h:

Go to the source code of this file.

Namespaces

namespace  armarx
 This file offers overloads of toIce() and fromIce() functions for STL container types.
 
namespace  armarx::control
 
namespace  armarx::control::common
 This file is part of ArmarX.
 
namespace  armarx::control::common::control_law
 This file is part of ArmarX.
 
namespace  armarx::control::common::control_law::helper
 

Typedefs

using DVec = std::vector<double>
 

Functions

void addViaPoint (mp::arondto::MPConfig &cfg, const double &canValue, const Eigen::Matrix4f &viaPose)
 
void addViaPoint (mp::arondto::MPConfig &cfg, const double &canValue, const Eigen::VectorXf &viaPoint)
 
void addViaPoint (mp::arondto::MPConfig &cfg, const double &canValue, const std::vector< double > &viaPoint)
 
void addViaPoint (mp::arondto::MPConfig &cfg, const double &canValue, const std::vector< float > &viaPoint)
 
void appendToViapointList (std::vector< mp::arondto::ListViaPoint > &vpList, const double canonicalValue, const Eigen::Matrix4f &viaPose)
 
void appendToViapointList (std::vector< mp::arondto::ListViaPoint > &vpList, const double canonicalValue, const Eigen::VectorXf &viaPoint)
 
void appendToViapointList (std::vector< mp::arondto::ListViaPoint > &vpList, const double canonicalValue, const std::vector< double > &viaPoint)
 
void appendToViapointList (std::vector< mp::arondto::ListViaPoint > &vpList, const double canonicalValue, const std::vector< float > &viaPoint)
 
Eigen::Vector4f calculateTransitionWeights (float z1, float z2)
 helper functions
 
mp::arondto::MPTraj createDummyTraj (const int dimension, const int timesteps=100)
 create a dummy trajectory that only has zeros for every dimension.
 
void debugEigenMat (StringVariantBaseMap &datafields, const std::string &name, const Eigen::MatrixXd &mat)
 
void debugEigenMat (StringVariantBaseMap &datafields, const std::string &name, const Eigen::MatrixXf &mat)
 
void debugEigenPose (StringVariantBaseMap &datafields, const std::string &name, Eigen::Matrix4f pose)
 
void debugEigenVec (StringVariantBaseMap &datafields, const std::string &name, Eigen::VectorXf vec)
 
void debugStdVec (StringVariantBaseMap &datafields, const std::string &name, std::vector< float > vec)
 
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 dvecToEigenf (std::vector< double > &vec, Eigen::VectorXf &result)
 
Eigen::Matrix4f dVecToMat4 (const DVec &dvec)
 create Eigen:Matrix4f from 7D double vector.
 
PosePtr dVecToPose (const DVec &dvec)
 
std::string dVecToString (const DVec &dvec)
 
void from_json (const nlohmann::json &j, Pose &fp)
 
void from_json (const nlohmann::json &j, PoseBasePtr &fp)
 
void from_json (const nlohmann::json &j, PosePtr &fp)
 
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)
 
mp::arondto::MPConfig getDefaultMPConfig (MPType mpType, const std::string &name, const std::string &nodeSetName, const double &durationSec, const std::vector< std::string > &mpTraj, const std::vector< mp::arondto::ListViaPoint > &viaPointList)
 
float getDeltaAngleBetweenPose (const Eigen::Matrix4f &pose1, const Eigen::Matrix4f &pose2)
 
float getDeltaAngleBetweenRotMat (const Eigen::Matrix3f &rotMat1, const Eigen::Matrix3f &rotMat2)
 
void getEigenMatrix (nlohmann::json &userConfig, const std::string &entryName, Eigen::MatrixXf &mat)
 
Eigen::VectorXf getEigenVec (nlohmann::json &userConfig, nlohmann::json &defaultConfig, const std::string &entryName, int size, int value)
 
template<typename T>
void getEigenVec (nlohmann::json &userConfig, nlohmann::json &defaultConfig, const std::string &entryName, T &vec)
 
Eigen::VectorXf getEigenVecWithDefault (nlohmann::json &userConfig, Eigen::VectorXf defaultValue, const std::string &entryName)
 
void getError (Eigen::Matrix4f &currentPose, Eigen::Vector3d &targetPosition, Eigen::Quaterniond &targetQuaternion, double &posiError, double &oriError)
 
Eigen::Matrix3f getOri (const Eigen::Matrix4f &matrix)
 
Eigen::Block< Eigen::Matrix4f, 3, 3 > getOri (Eigen::Matrix4f &matrix)
 
Eigen::Matrix3f getOriT (const Eigen::Matrix4f &matrix)
 
Eigen::Vector3f getPos (const Eigen::Matrix4f &matrix)
 
Eigen::Block< Eigen::Matrix4f, 3, 1 > getPos (Eigen::Matrix4f &matrix)
 
template<typename T>
T getValue (nlohmann::json &userConfig, nlohmann::json &defaultConfig, const std::string &entryName)
 
template<typename T>
T getValueWithDefault (nlohmann::json &userConfig, T defaultValue, const std::string &entryName)
 
DVec mat4ToDVec (const Eigen::Matrix4f &mat)
 convert Eigen:Matrix4f to 7D double vector.
 
std::vector< float > mat4ToFVec (const Eigen::Matrix4f &mat)
 
Eigen::Matrix3f mirrorLeftRightOri (Eigen::Matrix3f &sourcePose)
 
Eigen::Matrix4f mirrorLeftRightOri (Eigen::Matrix4f &sourcePose)
 
Eigen::Matrix4f mirrorLeftRightPose (Eigen::Matrix4f &sourcePose)
 
mp::arondto::MPTraj mpTrajFromFile (std::string filepath, bool containsHeader, bool noTimeStamp)
 
bool poseDeviationTooLarge (const Eigen::Matrix4f &currentPose, const Eigen::Matrix4f &desiredPose, float positionThrMM, float angleThrDeg)
 
DVec poseToDVec (PosePtr &pose)
 
std::vector< float > poseToFVec (PosePtr &pose)
 
void reportPoseDeviationWarning (const std::string &who, const Eigen::Matrix4f &pose1, const Eigen::Matrix4f &pose2, const std::string &namePose1, const std::string &namePose2)
 
void setOri (Eigen::Matrix4f &matrix, const Eigen::Ref< const Eigen::Matrix3f > &ori)
 
void setPos (Eigen::Matrix4f &matrix, const Eigen::Ref< const Eigen::Vector3f > &pos)
 
void skew (const Eigen::Vector3f &vec, Eigen::Ref< Eigen::Matrix3f > result)
 
std::string sVecToString (const std::vector< std::string > &vec, const std::string &delimiter)
 
void to_json (nlohmann::json &j, const Pose &fp)
 
void to_json (nlohmann::json &j, const PoseBasePtr &fp)
 
void to_json (nlohmann::json &j, const PosePtr &fp)
 
void validateTSViaPoint (std::vector< double > &viaPoint, std::vector< double > &viaPointReference)
 
Eigen::Matrix4f vec7fToMat4f (const Eigen::VectorXf &vec)
 
Eigen::VectorXf vecToEigen (std::vector< float > &vec)