This file is part of ArmarX. More...

Namespaces
namespace	control_law
	This file is part of ArmarX.

namespace	coordination
	This file is part of ArmarX.

namespace	ft
	This file is part of ArmarX.

namespace	mp
	This file is part of ArmarX.

Classes
class	MultivariateNormal

struct	SensorDevicesForNJointTorqueController

Typedefs
using	DVec = std::vector<double>

Enumerations
enum class	ControllerType { TSAdm , TSImp , TSVel , TSMixImpVel , TSImpCol , TSAdmCol , TSVelCol , TSVeloCol , TSMixImpVelCol , TSMPAdm , TSMPImp , TSMPVel , TSMPMixImpVel , TSMPImpCol , TSMPVelCol , TSMPVeloCol , TSMPMixImpVelCol , TSMPVelWiping , TSMPImpWiping , TSMPMixImpVelColWiping , QPWholeBodyImpedance , QPWholeBodyVelocity , WholeBodyTrajectoryController , ZMQTSVel , ZMQTSImp , ZMQTSImpCol , ZMQTSMixImpVel , ZMQTSMixImpVelCol , ZenohTSVel , ZenohTSImp , ZenohTSImpCol , ZenohTSMixImpVel , ZenohTSMixImpVelCol , SharedMemoryTSVel , SharedMemoryTSVelCol , SharedMemoryTSVeloCol , SharedMemoryTSImp , SharedMemoryTSImpCol , SharedMemoryTSMixImpVel , SharedMemoryTSMixImpVelCol }

enum class	MPStatus { reset , trained , running , paused , finished }

enum class	MPType { taskspace , nullsapce , hand }

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)

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)

Eigen::VectorXf	pdf_gradient (const Eigen::VectorXf &mean, float std, const Eigen::VectorXf &x)

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)

Variables
const simox::meta::EnumNames< ControllerType >	ControllerTypeNames

const simox::meta::EnumNames< ControllerType >	ControllerTypeShort

const simox::meta::EnumNames< MPType >	mpTypeToString

Detailed Description

This file is part of ArmarX.

ArmarX is free software; you can redistribute it and/or modify it under the terms of the GNU General Public License version 2 as published by the Free Software Foundation.

ArmarX is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.

You should have received a copy of the GNU General Public License along with this program. If not, see http://www.gnu.org/licenses/.

Author: Jianfeng Gao ( jianfeng dot gao at kit dot edu )

Date: 2022

Copyright: http://www.gnu.org/licenses/gpl-2.0.txt GNU General Public License

Typedef Documentation

◆ DVec

using DVec = std::vector<double>

Enumeration Type Documentation

◆ ControllerType

enum class ControllerType

strong

Enumerator
TSAdm
TSImp
TSVel
TSMixImpVel
TSImpCol
TSAdmCol
TSVelCol
TSVeloCol
TSMixImpVelCol
TSMPAdm
TSMPImp
TSMPVel
TSMPMixImpVel
TSMPImpCol
TSMPVelCol
TSMPVeloCol
TSMPMixImpVelCol
TSMPVelWiping
TSMPImpWiping
TSMPMixImpVelColWiping
QPWholeBodyImpedance
QPWholeBodyVelocity
WholeBodyTrajectoryController
ZMQTSVel
ZMQTSImp
ZMQTSImpCol
ZMQTSMixImpVel
ZMQTSMixImpVelCol
ZenohTSVel
ZenohTSImp
ZenohTSImpCol
ZenohTSMixImpVel
ZenohTSMixImpVelCol
SharedMemoryTSVel
SharedMemoryTSVelCol
SharedMemoryTSVeloCol
SharedMemoryTSImp
SharedMemoryTSImpCol
SharedMemoryTSMixImpVel
SharedMemoryTSMixImpVelCol

◆ MPStatus

enum class MPStatus

strong

Enumerator
reset
trained
running
paused
finished

◆ MPType

enum class MPType

strong

Enumerator
taskspace
nullsapce
hand

Function Documentation

◆ addViaPoint() [1/4]

void addViaPoint	(	mp::arondto::MPConfig &	cfg,
		const double &	canValue,
		const Eigen::Matrix4f &	viaPose )

◆ addViaPoint() [2/4]

void addViaPoint	(	mp::arondto::MPConfig &	cfg,
		const double &	canValue,
		const Eigen::VectorXf &	viaPoint )

◆ addViaPoint() [3/4]

void addViaPoint	(	mp::arondto::MPConfig &	cfg,
		const double &	canValue,
		const std::vector< double > &	viaPoint )

◆ addViaPoint() [4/4]

void addViaPoint	(	mp::arondto::MPConfig &	cfg,
		const double &	canValue,
		const std::vector< float > &	viaPoint )

◆ appendToViapointList() [1/4]

void appendToViapointList	(	std::vector< mp::arondto::ListViaPoint > &	vpList,
		const double	canonicalValue,
		const Eigen::Matrix4f &	viaPose )

◆ appendToViapointList() [2/4]

void appendToViapointList	(	std::vector< mp::arondto::ListViaPoint > &	vpList,
		const double	canonicalValue,
		const Eigen::VectorXf &	viaPoint )

◆ appendToViapointList() [3/4]

void appendToViapointList	(	std::vector< mp::arondto::ListViaPoint > &	vpList,
		const double	canonicalValue,
		const std::vector< double > &	viaPoint )

◆ appendToViapointList() [4/4]

void appendToViapointList	(	std::vector< mp::arondto::ListViaPoint > &	vpList,
		const double	canonicalValue,
		const std::vector< float > &	viaPoint )

◆ createDummyTraj()

mp::arondto::MPTraj createDummyTraj	(	const int	dimension,
		const int	timesteps = 100 )

create a dummy trajectory that only has zeros for every dimension.

Parameters

dimension	the dimension of the trajectory, e.g. 7d for taskspace and Nd for N-joint kinematic chain.
timesteps	the total number of timesteps in the trajectory

Returns: the MPTraj instance

Note that, we you use this to configure MPs and execute it after, you'll have to set a proper start and goal for it, either set them in the MPListConfig for the corresponding MP and call startAll(), or use start("MPName", start, goal, duration_second). Otherwise, the MP will assume the first and last timestep being the start and goal, which are all zeros and therefore wrong for your task.

◆ debugEigenMat() [1/2]

void debugEigenMat	(	StringVariantBaseMap &	datafields,
		const std::string &	name,
		const Eigen::MatrixXd &	mat )

◆ debugEigenMat() [2/2]

void debugEigenMat	(	StringVariantBaseMap &	datafields,
		const std::string &	name,
		const Eigen::MatrixXf &	mat )

◆ debugEigenPose()

void debugEigenPose	(	StringVariantBaseMap &	datafields,
		const std::string &	name,
		Eigen::Matrix4f	pose )

◆ debugEigenVec()

void debugEigenVec	(	StringVariantBaseMap &	datafields,
		const std::string &	name,
		Eigen::VectorXf	vec )

◆ debugStdVec()

void debugStdVec	(	StringVariantBaseMap &	datafields,
		const std::string &	name,
		std::vector< float >	vec )

◆ 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 )

◆ dvecToEigenf()

void dvecToEigenf	(	std::vector< double > &	vec,
		Eigen::VectorXf &	result )

◆ dVecToMat4()

Eigen::Matrix4f dVecToMat4 ( const DVec & dvec )

create Eigen:Matrix4f from 7D double vector.

Parameters

dvec	[x, y, z, qw, qx, qy, qz] format

Returns: Eigen::Matrix4f \in SE(3)

Note that, the order of the quaternion elements follows w, x, y, z format

◆ dVecToPose()

PosePtr dVecToPose ( const DVec & dvec )

◆ dVecToString()

std::string dVecToString ( const DVec & dvec )

◆ from_json() [1/3]

void from_json	(	const nlohmann::json &	j,
		Pose &	fp )

◆ from_json() [2/3]

void from_json	(	const nlohmann::json &	j,
		PoseBasePtr &	fp )

◆ from_json() [3/3]

void from_json	(	const nlohmann::json &	j,
		PosePtr &	fp )

◆ getDefaultMPConfig() [1/2]

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 )

◆ getDefaultMPConfig() [2/2]

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 )

◆ getDeltaAngleBetweenPose()

float getDeltaAngleBetweenPose	(	const Eigen::Matrix4f &	pose1,
		const Eigen::Matrix4f &	pose2 )

◆ getDeltaAngleBetweenRotMat()

float getDeltaAngleBetweenRotMat	(	const Eigen::Matrix3f &	rotMat1,
		const Eigen::Matrix3f &	rotMat2 )

◆ getEigenMatrix()

void getEigenMatrix	(	nlohmann::json &	userConfig,
		const std::string &	entryName,
		Eigen::MatrixXf &	mat )

◆ getEigenVec() [1/2]

Eigen::VectorXf getEigenVec	(	nlohmann::json &	userConfig,
		nlohmann::json &	defaultConfig,
		const std::string &	entryName,
		int	size,
		int	value )

◆ getEigenVec() [2/2]

template<typename T>

void getEigenVec	(	nlohmann::json &	userConfig,
		nlohmann::json &	defaultConfig,
		const std::string &	entryName,
		T &	vec )

◆ getEigenVecWithDefault()

Eigen::VectorXf getEigenVecWithDefault	(	nlohmann::json &	userConfig,
		Eigen::VectorXf	defaultValue,
		const std::string &	entryName )

◆ getError()

void getError	(	Eigen::Matrix4f &	currentPose,
		Eigen::Vector3d &	targetPosition,
		Eigen::Quaterniond &	targetQuaternion,
		double &	posiError,
		double &	oriError )

◆ getOri() [1/2]

Eigen::Matrix3f getOri ( const Eigen::Matrix4f & matrix )

◆ getOri() [2/2]

Eigen::Block< Eigen::Matrix4f, 3, 3 > getOri ( Eigen::Matrix4f & matrix )

◆ getOriT()

Eigen::Matrix3f getOriT ( const Eigen::Matrix4f & matrix )

◆ getPos() [1/2]

Eigen::Vector3f getPos ( const Eigen::Matrix4f & matrix )

◆ getPos() [2/2]

Eigen::Block< Eigen::Matrix4f, 3, 1 > getPos ( Eigen::Matrix4f & matrix )

◆ getValue()

template<typename T>

T getValue	(	nlohmann::json &	userConfig,
		nlohmann::json &	defaultConfig,
		const std::string &	entryName )

◆ getValueWithDefault()

template<typename T>

T getValueWithDefault	(	nlohmann::json &	userConfig,
		T	defaultValue,
		const std::string &	entryName )

◆ mat4ToDVec()

DVec mat4ToDVec ( const Eigen::Matrix4f & mat )

convert Eigen:Matrix4f to 7D double vector.

Parameters

mat	Eigen::Matrix4f \in SE(3)

Returns: vector of format [x, y, z, qw, qx, qy, qz]

Note that, the order of the quaternion elements follows w, x, y, z format

◆ mat4ToFVec()

std::vector< float > mat4ToFVec ( const Eigen::Matrix4f & mat )

◆ mirrorLeftRightOri() [1/2]

Eigen::Matrix3f mirrorLeftRightOri ( Eigen::Matrix3f & sourcePose )

◆ mirrorLeftRightOri() [2/2]

Eigen::Matrix4f mirrorLeftRightOri ( Eigen::Matrix4f & sourcePose )

◆ mirrorLeftRightPose()

Eigen::Matrix4f mirrorLeftRightPose ( Eigen::Matrix4f & sourcePose )

◆ mpTrajFromFile()

armarx::control::common::mp::arondto::MPTraj mpTrajFromFile	(	std::string	filepath,
		bool	containsHeader,
		bool	noTimeStamp )

◆ pdf_gradient()

Eigen::VectorXf pdf_gradient	(	const Eigen::VectorXf &	mean,
		float	std,
		const Eigen::VectorXf &	x )

◆ poseDeviationTooLarge()

bool poseDeviationTooLarge	(	const Eigen::Matrix4f &	currentPose,
		const Eigen::Matrix4f &	desiredPose,
		float	positionThrMM,
		float	angleThrDeg )

◆ poseToDVec()

DVec poseToDVec ( PosePtr & pose )

◆ poseToFVec()

std::vector< float > poseToFVec ( PosePtr & pose )

◆ reportPoseDeviationWarning()

void reportPoseDeviationWarning	(	const std::string &	who,
		const Eigen::Matrix4f &	pose1,
		const Eigen::Matrix4f &	pose2,
		const std::string &	namePose1,
		const std::string &	namePose2 )

◆ setOri()

void setOri	(	Eigen::Matrix4f &	matrix,
		const Eigen::Ref< const Eigen::Matrix3f > &	ori )

◆ setPos()

void setPos	(	Eigen::Matrix4f &	matrix,
		const Eigen::Ref< const Eigen::Vector3f > &	pos )

◆ skew()

void skew	(	const Eigen::Vector3f &	vec,
		Eigen::Ref< Eigen::Matrix3f >	result )

◆ sVecToString()

std::string sVecToString	(	const std::vector< std::string > &	vec,
		const std::string &	delimiter )

◆ to_json() [1/3]

void to_json	(	nlohmann::json &	j,
		const Pose &	fp )

◆ to_json() [2/3]

void to_json	(	nlohmann::json &	j,
		const PoseBasePtr &	fp )

◆ to_json() [3/3]

void to_json	(	nlohmann::json &	j,
		const PosePtr &	fp )

◆ validateTSViaPoint()

void validateTSViaPoint	(	std::vector< double > &	viaPoint,
		std::vector< double > &	viaPointReference )

◆ vec7fToMat4f()

Eigen::Matrix4f vec7fToMat4f ( const Eigen::VectorXf & vec )

◆ vecToEigen()

Eigen::VectorXf vecToEigen ( std::vector< float > & vec )

Variable Documentation

◆ ControllerTypeNames

const simox::meta::EnumNames<ControllerType> ControllerTypeNames

inline

◆ ControllerTypeShort

const simox::meta::EnumNames<ControllerType> ControllerTypeShort

inline

◆ mpTypeToString

const simox::meta::EnumNames<MPType> mpTypeToString

inline

Initial value:

= {{MPType::taskspace, "taskspace"},
                                                                  {MPType::nullsapce, "nullspace"},
                                                                  {MPType::hand, "hand"}}

Namespaces

Classes

Typedefs

Enumerations

Functions

Variables

Detailed Description

Typedef Documentation

◆ DVec

Enumeration Type Documentation

◆ ControllerType

◆ MPStatus

◆ MPType

Function Documentation

◆ addViaPoint() [1/4]

◆ addViaPoint() [2/4]

◆ addViaPoint() [3/4]

◆ addViaPoint() [4/4]

◆ appendToViapointList() [1/4]

◆ appendToViapointList() [2/4]

◆ appendToViapointList() [3/4]

◆ appendToViapointList() [4/4]

◆ createDummyTraj()

◆ debugEigenMat() [1/2]

◆ debugEigenMat() [2/2]

◆ debugEigenPose()

◆ debugEigenVec()

◆ debugStdVec()

◆ detectAndReportPoseDeviationWarning()

◆ dvecToEigenf()

◆ dVecToMat4()

◆ dVecToPose()

◆ dVecToString()

◆ from_json() [1/3]

◆ from_json() [2/3]

◆ from_json() [3/3]

◆ getDefaultMPConfig() [1/2]

◆ getDefaultMPConfig() [2/2]

◆ getDeltaAngleBetweenPose()

◆ getDeltaAngleBetweenRotMat()

◆ getEigenMatrix()

◆ getEigenVec() [1/2]

◆ getEigenVec() [2/2]

◆ getEigenVecWithDefault()

◆ getError()

◆ getOri() [1/2]

◆ getOri() [2/2]

◆ getOriT()

◆ getPos() [1/2]

◆ getPos() [2/2]

◆ getValue()

◆ getValueWithDefault()

◆ mat4ToDVec()

◆ mat4ToFVec()

◆ mirrorLeftRightOri() [1/2]

◆ mirrorLeftRightOri() [2/2]

◆ mirrorLeftRightPose()

◆ mpTrajFromFile()

◆ pdf_gradient()

◆ poseDeviationTooLarge()

◆ poseToDVec()

◆ poseToFVec()

◆ reportPoseDeviationWarning()

◆ setOri()

◆ setPos()

◆ skew()

◆ sVecToString()

◆ to_json() [1/3]

◆ to_json() [2/3]

◆ to_json() [3/3]

◆ validateTSViaPoint()

◆ vec7fToMat4f()

◆ vecToEigen()

Variable Documentation

◆ ControllerTypeNames

◆ ControllerTypeShort

◆ mpTypeToString