Resolved rate acceleration controller class. More...

#include <controller.h>

Public Member Functions
	Resolved_acc (const short dof=1)
	Constructor. More...

	Resolved_acc (const Robot_basic &robot, const Real Kvp, const Real Kpp, const Real Kvo, const Real Kpo)
	Constructor. More...

void	set_Kpo (const Real Kpo)
	Assign the gain $k_{po}$ . More...

void	set_Kpp (const Real Kpp)
	Assign the gain $k_{pp}$ . More...

void	set_Kvo (const Real Kvo)
	Assign the gain $k_{vo}$ . More...

void	set_Kvp (const Real Kvp)
	Assign the gain $k_{vp}$ . More...

ReturnMatrix	torque_cmd (Robot_basic &robot, const ColumnVector &pdpp, const ColumnVector &pdp, const ColumnVector &pd, const ColumnVector &wdp, const ColumnVector &wd, const Quaternion &qd, const short link_pc, const Real dt)
	Output torque. More...

Private Attributes
ColumnVector	a
	Control input. More...

double	Kpo

double	Kpp

double	Kvo

double	Kvp
	Controller gains. More...

ColumnVector	p
	End effector position. More...

ColumnVector	pp
	End effector velocity. More...

ColumnVector	qp
	Robot joints velocity. More...

ColumnVector	qpp
	Robot joints acceleration. More...

Quaternion	quat
	Temporary quaternion. More...

ColumnVector	quat_v_error
	Vector part of error quaternion. More...

Matrix	Rot
	Temporay rotation matrix. More...

ColumnVector	zero3
	$3\times 1$ zero vector. More...

Detailed Description

Resolved rate acceleration controller class.

The dynamic model of a robot manipulator can be expressed in joint space as

$B(q)\ddot{q} + C(q,\dot{q})\dot{q} + D\dot{q} + g(q) = \tau - J^T(q)f$

According to the concept of inverse dynamics, the driving torques can be chosen as

$\tau = B(q)J^{-1}(q)\big(a - \dot{J}(q,\dot{q})\dot{q}\big) + C(q,\dot{q})\dot{q} + D\dot{q} + g(q) - J^T(q)f$

where $a$ is the a new control input defined by

$a_p = \ddot{p}_d + k_{vp}\dot{\tilde{p}} + k_{pp}\tilde{p}$

$a_o = \dot{\omega}_d + k_{vo}\dot{\tilde{\omega}} + k_{po}\tilde{v}$

where $\tilde{x} = x_c - x_d$ and $ v$ is the vector par of the quaternion representing the orientation error between the desired and end effector frame. $k_{vp}$ , $k_{pp}$ , $k_{vo}$ and $k_{po}$ are positive gains.

Up to now this class has been tested only with a 6 dof robot.

Definition at line 171 of file controller.h.

Constructor & Destructor Documentation

Resolved_acc::Resolved_acc ( const short dof = 1 )

Constructor.

Definition at line 407 of file controller.cpp.

Resolved_acc::Resolved_acc	(	const Robot_basic &	robot,
		const Real	Kvp,
		const Real	Kpp,
		const Real	Kvo,
		const Real	Kpo
	)

Constructor.

Definition at line 426 of file controller.cpp.

Member Function Documentation

void Resolved_acc::set_Kpo ( const Real Kpo )

Assign the gain $k_{po}$ .

Definition at line 465 of file controller.cpp.

void Resolved_acc::set_Kpp ( const Real Kpp )

Assign the gain $k_{pp}$ .

Definition at line 453 of file controller.cpp.

void Resolved_acc::set_Kvo ( const Real Kvo )

Assign the gain $k_{vo}$ .

Definition at line 459 of file controller.cpp.

void Resolved_acc::set_Kvp ( const Real Kvp )

Assign the gain $k_{vp}$ .

Definition at line 447 of file controller.cpp.

ReturnMatrix Resolved_acc::torque_cmd	(	Robot_basic &	robot,
		const ColumnVector &	pdpp,
		const ColumnVector &	pdp,
		const ColumnVector &	pd,
		const ColumnVector &	wdp,
		const ColumnVector &	wd,
		const Quaternion &	quatd,
		const short	link_pc,
		const Real	dt
	)

Output torque.

For more robustess the damped least squares inverse Jacobian is used instead of the inverse Jacobian.

The quaternion -q represents exactly the same rotation as the quaternion q. The temporay quaternion, quat, is quatd plus a sign correction. It is customary to choose the sign G on q1 so that q0.Gq1 >=0 (the angle between q0 ang Gq1 is acute). This choice avoids extra spinning caused by the interpolated rotations.

Definition at line 471 of file controller.cpp.

Member Data Documentation

ColumnVector Resolved_acc::a

private

Control input.

Definition at line 193 of file controller.h.

double Resolved_acc::Kpo

private

Definition at line 188 of file controller.h.

double Resolved_acc::Kpp

private

Definition at line 188 of file controller.h.

double Resolved_acc::Kvo

private

Definition at line 188 of file controller.h.

double Resolved_acc::Kvp

private

Controller gains.

Definition at line 188 of file controller.h.

ColumnVector Resolved_acc::p

private

End effector position.

Definition at line 193 of file controller.h.

ColumnVector Resolved_acc::pp

private

End effector velocity.

Definition at line 193 of file controller.h.

ColumnVector Resolved_acc::qp

private

Robot joints velocity.

Definition at line 193 of file controller.h.

ColumnVector Resolved_acc::qpp

private

Robot joints acceleration.

Definition at line 193 of file controller.h.

Quaternion Resolved_acc::quat

private

Temporary quaternion.

Definition at line 200 of file controller.h.

ColumnVector Resolved_acc::quat_v_error

private

Vector part of error quaternion.

Definition at line 193 of file controller.h.

Matrix Resolved_acc::Rot

private

Temporay rotation matrix.

Definition at line 192 of file controller.h.

ColumnVector Resolved_acc::zero3

private

$3\times 1$ zero vector.

Definition at line 193 of file controller.h.

The documentation for this class was generated from the following files:

Public Member Functions

Private Attributes

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation