Resolved_acc Class Reference

Resolved rate acceleration controller class. More...

#include <controller.h>

Public Member Functions
	Resolved_acc (const Robot_basic &robot, const Real Kvp, const Real Kpp, const Real Kvo, const Real Kpo)
	Constructor.
	Resolved_acc (const short dof=1)
	Constructor.
void	set_Kpo (const Real Kpo)
	Assign the gain $k_{po}$ .
void	set_Kpp (const Real Kpp)
	Assign the gain $k_{pp}$ .
void	set_Kvo (const Real Kvo)
	Assign the gain $k_{vo}$ .
void	set_Kvp (const Real Kvp)
	Assign the gain $k_{vp}$ .
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.
Private Attributes
ColumnVector	a
	Control input.
double	Kpo
double	Kpp
double	Kvo
double	Kvp
	Controller gains.
ColumnVector	p
	End effector position.
ColumnVector	pp
	End effector velocity.
ColumnVector	qp
	Robot joints velocity.
ColumnVector	qpp
	Robot joints acceleration.
Quaternion	quat
	Temporary quaternion.
ColumnVector	quat_v_error
	Vector part of error quaternion.
Matrix	Rot
	Temporay rotation matrix.
ColumnVector	zero3
	$3\times 1$ zero vector.

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.