Classes
class	Args

class	Atomic

class	AtomicQueue

class	AtomicBuffer

struct	ExpMapJacobianReturnType

struct	ExpMapJacobianReturnType< 1 >

class	RedisClient

class	ThreadPool

class	Timer

class	Tree

Functions
	PYBIND11_MODULE (ctrlutils, m)

template<typename Derived >
std::optional< Derived >	ParseArgs (int argc, char *argv[])

std::ostream &	operator<< (std::ostream &os, const Args &args)

template<typename Derived1 , typename Derived2 , typename Derived3 , typename Derived5 >
Derived1::PlainObject	PdControl (const ::Eigen::MatrixBase< Derived1 > &x, const ::Eigen::MatrixBase< Derived2 > &x_des, const ::Eigen::MatrixBase< Derived3 > &dx, const ::Eigen::MatrixBase< Derived5 > &kp_kv, double ddx_max=0., typename Derived1::PlainObject *x_err_out=nullptr)

template<typename Derived1 , typename Derived2 , typename Derived3 , typename Derived4 , typename Derived5 >
Derived1::PlainObject	PdControl (const ::Eigen::MatrixBase< Derived1 > &x, const ::Eigen::MatrixBase< Derived2 > &x_des, const ::Eigen::MatrixBase< Derived3 > &dx, const ::Eigen::MatrixBase< Derived4 > &dx_des, const ::Eigen::MatrixBase< Derived5 > &kp_kv, double ddx_max=0., typename Derived1::PlainObject x_err_out=nullptr, typename Derived3::PlainObject dx_err_out=nullptr)

template<typename Derived1 , typename Derived3 >
Derived1::PlainObject	PdControl (const ::Eigen::Quaterniond &quat, const ::Eigen::Quaterniond &quat_des, const ::Eigen::MatrixBase< Derived1 > &w, const ::Eigen::MatrixBase< Derived3 > &kp_kv, double dw_max=0., typename Derived1::PlainObject *ori_err_out=nullptr)

template<typename Derived1 , typename Derived2 , typename Derived3 >
Derived1::PlainObject	PdControl (const ::Eigen::Quaterniond &quat, const ::Eigen::Quaterniond &quat_des, const ::Eigen::MatrixBase< Derived1 > &w, const ::Eigen::MatrixBase< Derived2 > &w_des, const ::Eigen::MatrixBase< Derived3 > &kp_kv, double dw_max=0., typename Derived1::PlainObject ori_err_out=nullptr, typename Derived2::PlainObject w_err_out=nullptr)

template<typename Derived1 , typename Derived2 , typename Derived3 , typename Derived4 >
Derived1::PlainObject	LookatPdControl (const ::Eigen::MatrixBase< Derived1 > &vec, const ::Eigen::MatrixBase< Derived2 > &vec_des, const ::Eigen::MatrixBase< Derived3 > &w, const ::Eigen::MatrixBase< Derived4 > &kp_kv, double dw_max=0., typename Derived1::PlainObject *ori_err_out=nullptr)

template<typename Derived >
Eigen::MatrixBase< Derived >::PlainObject	PseudoInverse (const Eigen::MatrixBase< Derived > &A, double svd_epsilon=0, bool *is_singular=nullptr)

double	Cross (Eigen::Ref< const Eigen::Vector2d > a, Eigen::Ref< const Eigen::Vector2d > b)

template<typename Scalar >
Eigen::Matrix< Scalar, 3, 3 >	RotationX (Scalar angle)

template<typename Scalar >
Eigen::Matrix< Scalar, 3, 3 >	RotationY (Scalar angle)

template<typename Scalar >
Eigen::Matrix< Scalar, 3, 3 >	RotationZ (Scalar angle)

template<typename Derived >
Eigen::Matrix< typename Derived::Scalar, 3, 3 >	CrossMatrix (const Eigen::DenseBase< Derived > &x)

template<typename Derived >
Eigen::Matrix< typename Derived::Scalar, 3, 3 >	DoubleCrossMatrix (const Eigen::DenseBase< Derived > &x)

template<typename Derived >
Eigen::Matrix< typename Derived::Scalar, 4, 4 >	LeftProductMatrix (const Eigen::QuaternionBase< Derived > &quat)

template<typename Derived >
Eigen::Matrix< typename Derived::Scalar, 4, 4 >	RightProductMatrix (const Eigen::QuaternionBase< Derived > &quat)

template<typename Derived1 , typename Derived2 >
Eigen::MatrixBase< Derived1 >::PlainObject	Projection (const Eigen::MatrixBase< Derived1 > &v1, const Eigen::MatrixBase< Derived2 > &v2)

template<typename Derived1 , typename Derived2 >
Eigen::MatrixBase< Derived1 >::PlainObject	OrthogonalProjection (const Eigen::MatrixBase< Derived1 > &v1, const Eigen::MatrixBase< Derived2 > &v2)

template<typename Derived >
bool	IsNan (const Eigen::ArrayBase< Derived > &a)

template<typename Derived >
bool	IsNan (const Eigen::MatrixBase< Derived > &m)

template<typename Derived >
bool	IsNan (const Eigen::QuaternionBase< Derived > &q)

template<typename Derived >
Derived	DecodeMatlab (const std::string &str)

template<typename Derived >
std::string	EncodeMatlab (const Eigen::DenseBase< Derived > &matrix)

template<typename Derived >
Derived	DecodeJson (const std::string &str)

template<typename Derived >
std::string	EncodeJson (const Eigen::DenseBase< Derived > &matrix)

Eigen::Vector3d	OrientationError (const Eigen::Quaterniond &quat, const Eigen::Quaterniond &quat_des)

Eigen::Vector3d	LookatError (const Eigen::Vector3d &vec, const Eigen::Vector3d &vec_des)

Eigen::Quaterniond	NearQuaternion (const Eigen::Quaterniond &quat, const Eigen::Quaterniond &quat_reference)

Eigen::Quaterniond	NearQuaternion (Eigen::Ref< const Eigen::Matrix3d > ori, const Eigen::Quaterniond &quat_reference)

Eigen::Quaterniond	FarQuaternion (const Eigen::Quaterniond &quat, const Eigen::Quaterniond &quat_reference)

Eigen::Quaterniond	FarQuaternion (const Eigen::Ref< const Eigen::Matrix3d > ori, const Eigen::Quaterniond &quat_reference)

template<typename Derived >
Eigen::Vector3d	Log (const Eigen::RotationBase< Derived, 3 > &ori)

Eigen::Matrix3d	Exp (const Eigen::Vector3d &w)

Eigen::Matrix3d	ExpMapDerivative (const Eigen::AngleAxisd &aa)

Eigen::Matrix3d	LogMapDerivative (const Eigen::AngleAxisd &aa)

template<typename Derived >
ExpMapJacobianReturnType< Eigen::MatrixBase< Derived >::ColsAtCompileTime >::type	ExpMapJacobian (const Eigen::AngleAxisd &aa, const Eigen::MatrixBase< Derived > &X)

ExpMapJacobianReturnType< 3 >::type	ExpMapJacobian (const Eigen::AngleAxisd &aa)

Eigen::Vector6d	Log (const Eigen::Isometry3d &T)

Eigen::Matrix6d	ExpMapDerivative (const Eigen::Isometry3d &T)

Eigen::Matrix6d	LogMapDerivative (const Eigen::Isometry3d &T)

Eigen::Matrix< double, 9, 3 >	LogMapHessian (const Eigen::AngleAxisd &aa)

template<int Cols>
ExpMapJacobianReturnType< Cols >::type	ExpCoordsJacobianImpl (const Eigen::Matrix3d &R, const Eigen::AngleAxisd &aa, const Eigen::Matrix< double, 3, Cols > &X)

template<>
Eigen::Matrix3d	ExpCoordsJacobianImpl< 1 > (const Eigen::Matrix3d &R, const Eigen::AngleAxisd &aa, const Eigen::Vector3d &p)

template<>
Eigen::Matrix< double, 9, 3 >	ExpCoordsJacobianImpl< 3 > (const Eigen::Matrix3d &R, const Eigen::AngleAxisd &aa, const Eigen::Matrix3d &I)

template<typename Derived >
ExpMapJacobianReturnType< Eigen::MatrixBase< Derived >::ColsAtCompileTime >::type	ExpMapJacobianOld (const Eigen::AngleAxisd &aa, const Eigen::MatrixBase< Derived > &X)

ExpMapJacobianReturnType< 3 >::type	ExpMapJacobianOld (const Eigen::AngleAxisd &aa)

Eigen::Matrix3d	ExpMapDerivativeCrossTerm (const Eigen::AngleAxisd &aa, Eigen::Ref< const Eigen::Vector3d > x)

template<>
std::string	ToString (const nlohmann::json &value)

template<>
nlohmann::json	FromString (const std::string &str)

template<typename T >
T	Signum (T x, T epsilon=T(0))

template<typename T >
T	Clip (T x, T max)

template<typename T >
T	Power (T x, size_t exp)

template<>
void	FromString (const std::string &str, cv::Mat &image)

std::string	ToString (const cv::Mat &image)

template<typename T >
std::string	ToString (const T &value)

template<>
std::string	ToString (const std::string &value)

template<typename T >
void	ToString (std::string &str, const T &value)

template<typename T >
void	FromString (const std::string &str, T &value)

template<>
void	FromString (const std::string &str, std::string &value)

std::ostream &	bold (std::ostream &os)

std::ostream &	underline (std::ostream &os)

std::ostream &	bold_underline (std::ostream &os)

std::ostream &	normal (std::ostream &os)

Detailed Description

eigen.cc

Created: September 8, 2018 Authors: Toki Migimatsu

argparse.h

Created: May 27, 2021 Authors: Toki Migimatsu

atomic.h

Created: August 10, 2019 Authors: Toki Migimatsu

atomic_queue.h

Created: November 18, 2018 Authors: Toki Migimatsu

control.h

Created: February 07, 2019 Authors: Toki Migimatsu

eigen_string.h

Created: July 1, 2018 Authors: Toki Migimatsu

euclidian.h

Created: May 16, 2019 Authors: Toki Migimatsu

json.h

Created: January 31, 2019 Authors: Toki Migimatsu

math.h

Created: October 7, 2018 Authors: Toki Migimatsu

opencv.h

Created: May 9, 2019 Authors: Toki Migimatsu

redis_client.h

Created: July 1, 2018 Authors: Toki Migimatsu

string.h

Created: January 22, 2019 Authors: Toki Migimatsu

thread_pool.h

Created: May 25, 2021 Authors: Toki Migimatsu

timer.h

Created: July 2, 2018 Authors: Toki Migimatsu

tree.h

Created: November 12, 2018 Authors: Toki Migimatsu

yaml.h

Created: February 27, 2019 Authors: Toki Migimatsu

Function Documentation

◆ DecodeJson()

template<typename Derived >

Derived ctrl_utils::DecodeJson ( const std::string & str )

Decode an Eigen matrix from Json format:

Usage: Eigen::Vector3d x = DecodeJson<Eigen::Vector3d>("[1, 2, 3]"); Eigen::MatrixXd A = DecodeJson<Eigen::MatrixXd>("[[1, 2, 3], [4, 5, 6]]");

◆ DecodeMatlab()

template<typename Derived >

Derived ctrl_utils::DecodeMatlab ( const std::string & str )

Decode an Eigen matrix from Matlab format:

Usage: Eigen::Vector3d x = DecodeMatlab<Eigen::Vector3d>("1 2 3"); Eigen::MatrixXd A = DecodeMatlab<Eigen::MatrixXd>("1 2 3; 4 5 6");

◆ EncodeJson()

template<typename Derived >

std::string ctrl_utils::EncodeJson ( const Eigen::DenseBase< Derived > & matrix )

Encode an Eigen matrix to Json format:

Usage: std::string x = EncodeMatlab(Eigen::Vector3d(1, 2, 3)); // "[1, 2, 3]" std::string A = EncodeMatlab(Eigen::Matrix2d(1, 2, 3, 4)); // "[[1, 2], [3, 4]]"

◆ EncodeMatlab()

template<typename Derived >

std::string ctrl_utils::EncodeMatlab ( const Eigen::DenseBase< Derived > & matrix )

Encode an Eigen matrix to Matlab format:

Usage: std::string x = EncodeMatlab(Eigen::Vector3d(1, 2, 3)); // "1 2 3" std::string A = EncodeMatlab(Eigen::Matrix2d(1, 2, 3, 4)); // "1 2; 3 4"

◆ ExpMapDerivative()

Eigen::Matrix3d ctrl_utils::ExpMapDerivative ( const Eigen::AngleAxisd & aa )

inline

d/dx exp(x) = d/de|e=0 log(f(x+e) f(x)^{-1})

◆ ExpMapJacobian()

template<typename Derived >

ExpMapJacobianReturnType< Eigen::MatrixBase< Derived >::ColsAtCompileTime >::type ctrl_utils::ExpMapJacobian	(	const Eigen::AngleAxisd &	aa,
		const Eigen::MatrixBase< Derived > &	X
	)

inline

Compute the Jacobian of a rotated matrix R * X with respect to the rotation's exponential coordinates (angle axis representation).

If X is a vector, the Jacobian returned is d(Rx)/dw. If X is a matrix, the Jacobian tensor is stacked into a matrix [d(RX)/dw_x; d(RX)/dw_y; d(RX)/dw_z].

d/dx exp(x) p = d/dexp(x) exp(x) p * d/dx exp(x) d/dx exp(x) p = d/dk (I + ad_k) Rp * d/de log(f(x+e) f(-x)) = d/dk Rp + [k]x Rp = d/dk Rp - [Rp]x k = -[Rp]x d/dx exp(x) p = d/dexp(x) Ad_exp(x) p * d/dx exp(x) = d/dexp(x) exp(ad_x) p * d/dx exp(x)

◆ FarQuaternion() [1/2]

Eigen::Quaterniond ctrl_utils::FarQuaternion	(	const Eigen::Quaterniond &	quat,
		const Eigen::Quaterniond &	quat_reference
	)

inline

Computes the quaternion for the given orientation that is farther from the reference quaternion.

Parameters

quat	Quaternion representing the desired orientation.
quat_ref	Reference quaternion.

Returns: The quaternion that represents the same orientation as quat but has a negative dot product with quat_ref.

See also: Python: ctrlutils.far_quaternion()

◆ FarQuaternion() [2/2]

Eigen::Quaterniond ctrl_utils::FarQuaternion	(	const Eigen::Ref< const Eigen::Matrix3d >	ori,
		const Eigen::Quaterniond &	quat_reference
	)

inline

Computes the quaternion for the given orientation that is farther from the reference quaternion.

Parameters

ori	3D matrix representing the desired orientation.
quat_ref	Reference quaternion.

Returns: The quaternion that represents the same orientation as ori but has a negative dot product with quat_ref.

See also: Python: ctrlutils.far_quaternion()

◆ FromString() [1/4]

template<>

YAML::Node ctrl_utils::FromString ( const std::string & str )

inline

Converts the string to a value using stringstream.

◆ FromString() [2/4]

template<>

void ctrl_utils::FromString	(	const std::string &	str,
		cv::Mat &	image
	)

inline

Decode a cv::Mat from a string for Redis.

Format: "nrows ncols cvtype bytedata"

◆ FromString() [3/4]

template<>

void ctrl_utils::FromString	(	const std::string &	str,
		std::string &	value
	)

inline

Template specialization for strings.

◆ FromString() [4/4]

template<typename T >

void ctrl_utils::FromString	(	const std::string &	str,
		T &	value
	)

inline

Converts the string to a value using stringstream.

◆ LookatError()

Eigen::Vector3d ctrl_utils::LookatError	(	const Eigen::Vector3d &	vec,
		const Eigen::Vector3d &	vec_des
	)

inline

Computes the orientation error between two lookat vectors.

Unlike ctrl_utils::OrientationError(), this error ignores rotations about the lookat vectors, producing the shortest rotation between the two.

Parameters

quat	Current orientation.
quat_des	Desired orientation.

Returns: Quaternion error.

See also: Python: ctrlutils.lookat_error()

◆ NearQuaternion() [1/2]

Eigen::Quaterniond ctrl_utils::NearQuaternion	(	const Eigen::Quaterniond &	quat,
		const Eigen::Quaterniond &	quat_reference
	)

inline

Computes the quaternion for the given orientation that is closer to the reference quaternion.

Each 3D orientation can be represented by two quaternions, where one quaternion has the same 4 elements as the other but negated. Of these two quaternions, the one that is "closer" to the reference is the one that has a positive dot product with the reference.

This function may be used in orientation control, where quat is the goal orientation, and quat_reference is the current orientation of the controlled body. The effect of controlling the body to the closer quaternion is that it will rotate the shortest way to the goal orientation. Using the farther quarternion will cause it to rotate the long way around, which may be desired to avoid singularities, for example.

Parameters

quat	Quaternion representing the desired orientation.
quat_ref	Reference quaternion.

Returns: The quaternion that represents the same orientation as quat but has a positive dot product with quat_ref.

See also: Python: ctrlutils.near_quaternion()

◆ NearQuaternion() [2/2]

Eigen::Quaterniond ctrl_utils::NearQuaternion	(	Eigen::Ref< const Eigen::Matrix3d >	ori,
		const Eigen::Quaterniond &	quat_reference
	)

inline

Computes the quaternion for the given orientation that is closer to the reference quaternion.

Parameters

ori	3D matrix representing the desired orientation.
quat_ref	Reference quaternion.

Returns: The quaternion that represents the same orientation as ori but has a positive dot product with quat_ref.

See also: Python: ctrlutils.near_quaternion()

◆ operator<<()

std::ostream& ctrl_utils::operator<<	(	std::ostream &	os,
		const Args &	args
	)

inline

Prints the parsed fields of the Args object.

◆ OrientationError()

Eigen::Vector3d ctrl_utils::OrientationError	(	const Eigen::Quaterniond &	quat,
		const Eigen::Quaterniond &	quat_des
	)

inline

Computes the quaternion error between the given orientations.

This quaternion error is equivalent to the angular velocity required to go from quat_des to quat in one second. Following this angular velocity also produces the SLERP interpolation between the two orientations.

For orientation control, a control law might look like the following:

Eigen::Matrix3d J_w = AngularJacobian(ab);
Eigen::Vector3d w_err = opspace::OrientationError(Orientation(ab), quat_des);
Eigen::Vector3d dw = -kp_ori * w_err - kv_ori * J_w * ab.dq;
Eigen::VectorXd tau = opspace::InverseDynamics(ab, J_w, dw);

Parameters

quat	Current orientation.
quat_des	Desired orientation.

Returns: Quaternion error.

See also: Python: ctrlutils.orientation_error()

◆ ParseArgs()

template<typename Derived >

std::optional< Derived > ctrl_utils::ParseArgs	(	int	argc,
		char *	argv[]
	)

Parses the arguments.

Example:

struct Args : ctrl_utils::Args {
  // Mandatory constructor boilerplate.
  explicit Args(ctrl_utils::Args&& args)
    : ctrl_utils::Args(std::move(args)) {}
 
  // Create a positional string argument.
  // E.g. `./hello joe`
  std::string name = Arg<std::string>("name", "Your name.");
 
  // Create an optional int argument.
  // E.g. `./hello joe --num-repeat 5` or `./hello joe -n 5`.
  int num_repeat = Kwarg<int>("n,num-repeat", 1,
      "Number of times to repeat the greeting");
 
  // Create a flag argument.
  // E.g. `./bin joe -n 5 --print-name` or `./bin joe -n 5 --no-print-name`.
  bool print_name = Flag<bool>("print-name", true, "Print your name.");
};
 
int main(int argc, char* argv[]) {
  // Parse the arguments into an Args object.
  std::optional<Args> args = ctrl_utils::ParseArgs<Args>(argc, argv);
 
  // If the optional is empty, the args could not be parsed.
  if (!args) return 1;
 
  // Access the parsed args as normal fields in the struct.
  const std::string entity = args->print_name ? args->name : "world";
  for (int i = 0; i < args->num_repeat; i++) {
    std::cout << "Hello " << entity << "!" << std::endl;
  }
  return 0;
}

Returns: A child class of ctrl_utils::Args with its fields populated with the command line arguments, or an empty optional if the parsing fails.

◆ PdControl() [1/2]

template<typename Derived1 , typename Derived2 , typename Derived3 , typename Derived5 >

Derived1::PlainObject ctrl_utils::PdControl	(	const ::Eigen::MatrixBase< Derived1 > &	x,
		const ::Eigen::MatrixBase< Derived2 > &	x_des,
		const ::Eigen::MatrixBase< Derived3 > &	dx,
		const ::Eigen::MatrixBase< Derived5 > &	kp_kv,
		double	ddx_max = `0.`,
		typename Derived1::PlainObject *	x_err_out = `nullptr`
	)

inline

General PD control law for reaching a goal position with velocity damping.

Outputs a desired acceleration given a desired position, as well as the current position and velocity. This behaves like a damped spring-mass system.

Control law: ddx = -kp (x - x_des) - kv dx

Parameters

x	Current position as an N-D vector.
x_des	Desired position as an N-D vector.
dx	Current velocity as an N-D vector.
kp_kv	Gains matrix as a 2D vector [kp, kv] or an N x 2 matrix, where the first and second columns correspond to kp and kv, respectively, and the N rows represent per-dimension gains.
ddx_max	Optional maximum acceleration due to the position error. This value clips the acceleration when the distance to the goal is large and is ignored when less than or equal to 0.
x_err_out	Optional output of the position error.

Returns: Desired acceleration.

See also: Python: ctrlutils.pd_control()

◆ PdControl() [2/2]

template<typename Derived1 , typename Derived3 >

Derived1::PlainObject ctrl_utils::PdControl	(	const ::Eigen::Quaterniond &	quat,
		const ::Eigen::Quaterniond &	quat_des,
		const ::Eigen::MatrixBase< Derived1 > &	w,
		const ::Eigen::MatrixBase< Derived3 > &	kp_kv,
		double	dw_max = `0.`,
		typename Derived1::PlainObject *	ori_err_out = `nullptr`
	)

inline

Special PD control law for 3D orientations.

Outputs a desired acceleration given a desired orientation, as well as the current orientation and angular velocity. This behaves like a damped spring-mass system.

Control law: dw = -kp (quat - quat_des) - kv w

Parameters

quat	Current orientation as a quaternion.
quat_des	Desired orientation as a quaternion.
w	Current angular velocity as a 4D vector.
kp_kv	Gains matrix as a 2D vector [kp, kv] or a 3 x 2 matrix, where the first and second columns correspond to kp and kv, respectively, and the 3 rows represent per-dimension (x, y, z) gains.
dw_max	Optional maximum acceleration due to the orientation error. This value clips the acceleration when the distance to the goal is large and is ignored when less than or equal to 0.
ori_err_out	Optional output of the orientation error.

Returns: Desired angular acceleration.

See also: Python: ctrlutils.pd_control()

◆ ToString() [1/4]

std::string ctrl_utils::ToString ( const cv::Mat & image )

inline

Encode a cv::Mat into a string for Redis.

Format: "nrows ncols cvtype bytedata"

◆ ToString() [2/4]

template<>

std::string ctrl_utils::ToString ( const std::string & value )

inline

Template specialization for strings.

◆ ToString() [3/4]

template<typename T >

std::string ctrl_utils::ToString ( const T & value )

inline

Converts the value to a string using stringstream.

◆ ToString() [4/4]

template<typename T >

void ctrl_utils::ToString	(	std::string &	str,
		const T &	value
	)

inline

Converts the value to a preallocated string.

Classes

Functions

Detailed Description

Function Documentation

◆ DecodeJson()

◆ DecodeMatlab()

◆ EncodeJson()

◆ EncodeMatlab()

◆ ExpMapDerivative()

◆ ExpMapJacobian()

◆ FarQuaternion() [1/2]

◆ FarQuaternion() [2/2]

◆ FromString() [1/4]

◆ FromString() [2/4]

◆ FromString() [3/4]

◆ FromString() [4/4]

◆ LookatError()

◆ NearQuaternion() [1/2]

◆ NearQuaternion() [2/2]

◆ operator<<()

◆ OrientationError()

◆ ParseArgs()

◆ PdControl() [1/2]

◆ PdControl() [2/2]

◆ ToString() [1/4]

◆ ToString() [2/4]

◆ ToString() [3/4]

◆ ToString() [4/4]