arl::math::Clik Class Reference

Calculates joint velocities given desired task velocities by implementing Closed Loop Inverse Kinematics. More...

#include <autharl_core/math/kinematics/clik.h>

Public Types
enum	PinvMethod { ARMA, EIGEN }
	Enumerates the available methods for calculating the pseudo inverse jacobian matrix. More...

Public Member Functions
	Clik (double dt, double singularity_avoid=0.1, Clik::PinvMethod pinv_method=Clik::PinvMethod::EIGEN, const arma::vec &k={4.0, 4.0, 4.0, 0.4, 0.4, 0.4})
	The default constructor. More...
	~Clik ()
arma::vec	getJointVelocity (const arma::mat &jac, const arma::vec &vd, const arma::vec &x, const arma::vec &xd)
	Returns the joint velocities given the jacobian matrix, the desired Cartesian velocity, the current pose and the desired pose. More...
arma::vec	getJointVelocity (const arma::mat &jac, const arma::vec &vd, const arma::vec &x)
	Returns the joint velocities given the jacobian matrix, the desired Cartesian velocity, the current pose. Here the desired pose it is not necessary as it is calculated from integration of the desired velocity. This is a wrapper function which calculates the desired pose by integrating the desired velocity and calls Clik::getJointVelocity(mat, vec, vec, vec) for calculating the joint velocities. More...
void	setInitialPose (const arma::vec &position, const arma::vec &orientation)
	Sets the initial position and orientation of the arm. Call this function the first time that the control loop will run. More...
void	enableLogging (const std::string &log_path="/home/user/autharl_logfiles")
	Call it if you desire to enable logging. More...

Detailed Description

Calculates joint velocities given desired task velocities by implementing Closed Loop Inverse Kinematics.

You can use it in a control loop for commanding Cartesian velocities.

The joint velocities is computed as:

where the joint velocities of a robot with joints, the pseudo-inverse of the jacobian matrix, the desired cartesian velocity, diagonal gain matrix and the pose error (position and orientation error), as the difference between the current pose and the desired pose (the desired pose can by calculated also by integrating the desired Cartesian velocity).

Example of use:

using arl::math::Clik;
Clik clik(1e-3);

// Read the position and orientation of the end-effector of the robot
// (orientation in quaternion)
p = getPositionFromRobot();
q = getOrientationFromRobot();

// Initialize clik
clik.setInitialPose(p, q);

// Run the control loop
while (true)
{
  // Read the current pose of the robot (orientation in quaternion)
  vec x = getPoseFromRobot();

  // Read the jacobian from the robot
  vec jac = getJacobianFromRobot();

  // Calculate an arbitrary desired Cartesian velocity.
  vec vd = {0.1, 0.1, 0, 0.01, 0.01, 0};

  // Calculate the joint velocities for the desired Cartesian position.
  vec qdot = clik.getJointVelocity(jac, vd, x);

  // Command the robot joint velocities
  sendJointVelocitiesToRobot(qdot);
}

Member Enumeration Documentation

enum arl::math::Clik::PinvMethod

Enumerates the available methods for calculating the pseudo inverse jacobian matrix.

Enumerator
ARMA	It will use the arma::pinv() function. From experience it can be proven quite slow for control loops under 5ms.
EIGEN	It will use the Eigen::colPivHouseholderQr() solver which has been proven quite fast.

Constructor & Destructor Documentation

arl::math::Clik::Clik	(	double	dt,
		double	singularity_avoid = 0.1,
		Clik::PinvMethod	pinv_method = Clik::PinvMethod::EIGEN,
		const arma::vec &	k = {4.0, 4.0, 4.0, 0.4, 0.4, 0.4}
	)

The default constructor.

Parameters

dt	The cycle time of the control loop needed if no desired pose is provided and the desired pose is calculated by integrating the desired velocity.
singularity_avoid	Tolerance under which any singular values are treated as zero. Use it for singularity avoidance. This parameter is used only if the pinv_method is choosen to be ARMA.
pinv_method	The method to use for calculating the inverse Jacobian. It is recommended to use the default value of EIGEN as it has been tested to be much faster. Calculating with ARMA may cause delays in a real time loop.
k	The gain K of CLIK as a vector of 6 gains (translational and rotational). It is recommended to use the default values.

arl::math::Clik::~Clik

(

)

Member Function Documentation

void arl::math::Clik::enableLogging

(

const std::string &

log_path = "/home/user/autharl_logfiles"

)

Call it if you desire to enable logging.

The logfiles will located in path with a name with a timestamp and the name clik.

Parameters

log_path

An already existing path for the logfiles.

Here is the call graph for this function:

arma::vec arl::math::Clik::getJointVelocity	(	const arma::mat &	jac,
		const arma::vec &	vd,
		const arma::vec &	x,
		const arma::vec &	xd
	)

Returns the joint velocities given the jacobian matrix, the desired Cartesian velocity, the current pose and the desired pose.

Parameters

jac	The jacobian matrix of the arm
vd	The desired velocity of the end-effector
x	The current pose of the the arm as a 7D vector (position/quaternion: [x, y, z, qw, qx, qy, qz]
xd	The desired pose of the the arm as a 7D vector (position/quaternion: [x, y, z, qw, qx, qy, qz]

Returns

The velocities of the joints

arma::vec arl::math::Clik::getJointVelocity	(	const arma::mat &	jac,
		const arma::vec &	vd,
		const arma::vec &	x
	)

Returns the joint velocities given the jacobian matrix, the desired Cartesian velocity, the current pose. Here the desired pose it is not necessary as it is calculated from integration of the desired velocity. This is a wrapper function which calculates the desired pose by integrating the desired velocity and calls Clik::getJointVelocity(mat, vec, vec, vec) for calculating the joint velocities.

Parameters

jac	The jacobian matrix of the arm
vd	The desired velocity of the end-effector
x	The current pose of the the arm as a 7D vector (position/quaternion: [x, y, z, qw, qx, qy, qz]

Returns

The velocities of the joints

void arl::math::Clik::setInitialPose	(	const arma::vec &	position,
		const arma::vec &	orientation
	)

Sets the initial position and orientation of the arm. Call this function the first time that the control loop will run.

Parameters

position	The initial position (x, y ,z)
orientation	The initial orientation in quaternion w, x, y, z

Here is the call graph for this function: