Resolved rate acceleration controller class. More...

#include <controller.h>

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

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

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

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

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

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

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
double	Kvp
	Controller gains.

double	Kpp

double	Kvo

double	Kpo

Matrix	Rot
	Temporay rotation matrix.

ColumnVector	zero3
	$3\times 1$ zero vector.

ColumnVector	qp
	Robot joints velocity.

ColumnVector	qpp
	Robot joints acceleration.

ColumnVector	a
	Control input.

ColumnVector	p
	End effector position.

ColumnVector	pp
	End effector velocity.

ColumnVector	quat_v_error
	Vector part of error quaternion.

Quaternion	quat
	Temporary quaternion.

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 167 of file controller.h.

Member Function Documentation

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 467 of file controller.cpp.

References Quaternion::dot_prod(), Robot_basic::get_dof(), Robot_basic::get_q(), Robot_basic::get_qp(), Robot_basic::jacobian_DLS_inv(), Robot_basic::kine_pd(), Quaternion::s(), Quaternion::v(), and x_prod_matrix().

Referenced by Dynamics::xdot().

Public Member Functions

Private Attributes

Detailed Description

Member Function Documentation