Impedance controller class. More...

#include <controller.h>

Public Member Functions
	Impedance ()
	Constructor.

	Impedance (const Robot_basic &robot, const DiagonalMatrix &Mp_, const DiagonalMatrix &Dp_, const DiagonalMatrix &Kp_, const DiagonalMatrix &Mo_, const DiagonalMatrix &Do_, const DiagonalMatrix &Ko_)
	Constructor.

short	set_Mp (const DiagonalMatrix &Mp_)
	Assign the translational impedance inertia matrix . More...

short	set_Mp (const Real MP_i, const short i)
	Assign the translational impedance inertia term . More...

short	set_Dp (const DiagonalMatrix &Dp_)
	Assign the translational impedance damping matrix . More...

short	set_Dp (const Real Dp_i, const short i)
	Assign the translational impedance damping term . More...

short	set_Kp (const DiagonalMatrix &Kp_)
	Assign the translational impedance stifness matrix . More...

short	set_Kp (const Real Kp_i, const short i)
	Assign the translational impedance stifness term . More...

short	set_Mo (const DiagonalMatrix &Mo_)
	Assign the rotational impedance inertia matrix . More...

short	set_Mo (const Real Mo_i, const short i)
	Assign the rotational impedance inertia term . More...

short	set_Do (const DiagonalMatrix &Do_)
	Assign the rotational impedance damping matrix . More...

short	set_Do (const Real Do_i, const short i)
	Assign the rotational impedance damping term . More...

short	set_Ko (const DiagonalMatrix &Ko_)
	Assign the rotational impedance stifness matrix . More...

short	set_Ko (const Real Ko_i, const short i)
	Assign the rotational impedance stifness term . More...

short	control (const ColumnVector &pdpp, const ColumnVector &pdp, const ColumnVector &pd, const ColumnVector &wdp, const ColumnVector &wd, const Quaternion &qd, const ColumnVector &f, const ColumnVector &n, const Real dt)
	Generation of a compliance trajectory. More...

Public Attributes
Quaternion	qc
	Compliant frame quaternion.

Quaternion	qcp
	Compliant frame quaternion derivative.

Quaternion	qcp_prev
	Previous value of qcp.

Quaternion	qcd
	Orientation error (betweem compliant and desired frame) quaternion.

Quaternion	quat
	Temporary quaternion.

ColumnVector	pc
	Compliant position.

ColumnVector	pcp
	Compliant velocity.

ColumnVector	pcpp
	Compliant acceleration.

ColumnVector	pcp_prev
	Previous value of pcp.

ColumnVector	pcpp_prev
	Previous value of pcpp.

ColumnVector	pcd
	Difference between pc and desired position.

ColumnVector	pcdp
	Difference between pcp and desired velocity.

ColumnVector	wc
	Compliant angular velocity.

ColumnVector	wcp
	Compliant angular acceleration.

ColumnVector	wcp_prev
	Previous value of wcp.

ColumnVector	wcd
	Difference between wc and desired angular velocity.

Private Attributes
DiagonalMatrix	Mp
	Translational impedance inertia matrix.

DiagonalMatrix	Dp
	Translational impedance damping matrix.

DiagonalMatrix	Kp
	Translational impedance stifness matrix.

DiagonalMatrix	Mo
	Rotational impedance inertia matrix.

DiagonalMatrix	Do
	Rotational impedance damping matrix.

DiagonalMatrix	Ko
	Rotational impedance stifness matrix.

Matrix	Ko_prime
	Modified rotational impedance stifness matrix.

Detailed Description

Impedance controller class.

The implemantation of the impedance controller is made of two section: the first one is the generation of a compliance trajectory and the second one used a position controller to ensure the end effector follow the compliance trajectory (We recommended to used the resolve acceleration controller scheme, implemented in the class Resolved_acc).

This class generate a compliance path given by the translational and the rotational impedance.

$M_p\ddot{\tilde{p}} + D_p\dot{\tilde{p}} + K_p\tilde{p} = f$

$M_o\dot{\tilde{\omega}} + D_o\tilde{\omega} + K_o'\tilde{v} = n$

where $\tilde{x} = x_c - x_d$ and $ v$ is the vector par of the quaternion representing the orientation error between the compliant and desired frame. The orientation error can also be express by rotation matrix, $\tilde{R} = R_d^TR_c$ . The quaternion mathematics are implemented in the Quaternion class. The index $_c$ and $_d$ denote the compliance and the desired respectively.

The impedance parameters $M_p$ , $D_p$ , $K_p$ , $M_o$ , $D_o$ and $K_o$ are $3\times 3$ diagonal positive definite matrix

Definition at line 87 of file controller.h.

Member Function Documentation

short Impedance::control	(	const ColumnVector &	pdpp,
		const ColumnVector &	pdp,
		const ColumnVector &	pd,
		const ColumnVector &	wdp,
		const ColumnVector &	wd,
		const Quaternion &	qd,
		const ColumnVector &	f,
		const ColumnVector &	n,
		const Real	dt
	)

Generation of a compliance trajectory.

Parameters

pdpp,:	desired end effector acceleration.
pdp,:	desired end effector velocity.
pd,:	desired end effector position.
wdp,:	desired end effector angular acceleration.
wd,:	desired end effector angular velocity.
qd,:	desired quaternion.
f,:	end effector contact force.
n,:	end effector contact moment.
dt,:	time frame.

Returns: short: 0 or WRONG_SIZE if one of the vector input is not $3\times 1$ .

The translational and rotational impedance equations are integrated, with input $f$ and $n$ to computed $\ddot{p}_c$ and $\dot{\omega}_c$ , $\dot{p}_c$ and $\omega_c$ , and then $p_c$ and $q_c$ . The compliant quaternion $q_c$ is obtained with the quaternion propagation equations (see Quaternion class).

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

References Quaternion::dot(), Quaternion::i(), Integ_quat(), Integ_Trap(), and WRONG_SIZE.

short Impedance::set_Do ( const DiagonalMatrix & Do_ )

Assign the rotational impedance damping matrix $D_o$ .

Returns: short: 0 or WRONG_SIZE if the matrix is not $3\times 3$ .

Definition at line 228 of file controller.cpp.

References WRONG_SIZE.

short Impedance::set_Do	(	const Real	Do_i,
		const short	i
	)

Assign the rotational impedance damping term $D_o(i,i)$ .

Returns: short: 0 or WRONG_SIZE if the matrix is not $3\times 3$ .

Definition at line 245 of file controller.cpp.

References WRONG_SIZE.

short Impedance::set_Dp ( const DiagonalMatrix & Dp_ )

Assign the translational impedance damping matrix $D_p$ .

Returns: short: 0 or WRONG_SIZE if the matrix is not $3\times 3$ .

Definition at line 129 of file controller.cpp.

References WRONG_SIZE.

short Impedance::set_Dp	(	const Real	Dp_i,
		const short	i
	)

Assign the translational impedance damping term $D_p(i,i)$ .

Returns: short: 0 or WRONG_SIZE if the matrix is not $3\times 3$ .

Definition at line 146 of file controller.cpp.

References WRONG_SIZE.

short Impedance::set_Ko ( const DiagonalMatrix & Ko_ )

Assign the rotational impedance stifness matrix $K_o$ .

Returns: short: 0 or WRONG_SIZE if the matrix is not $3\times 3$ .

Definition at line 261 of file controller.cpp.

References WRONG_SIZE.

short Impedance::set_Ko	(	const Real	Ko_i,
		const short	i
	)

Assign the rotational impedance stifness term $K_o(i,i)$ .

Returns: short: 0 or WRONG_SIZE if the matrix is not $3\times 3$ .

Definition at line 278 of file controller.cpp.

References WRONG_SIZE.

short Impedance::set_Kp ( const DiagonalMatrix & Kp_ )

Assign the translational impedance stifness matrix $K_p$ .

Returns: short: 0 or WRONG_SIZE if the matrix is not $3\times 3$ .

Definition at line 162 of file controller.cpp.

References WRONG_SIZE.

short Impedance::set_Kp	(	const Real	Kp_i,
		const short	i
	)

Assign the translational impedance stifness term $K_p(i,i)$ .

Returns: short: 0 or WRONG_SIZE if the matrix is not $3\times 3$ .

Definition at line 179 of file controller.cpp.

References WRONG_SIZE.

short Impedance::set_Mo ( const DiagonalMatrix & Mo_ )

Assign the rotational impedance inertia matrix $M_o$ .

Returns: short: 0 or WRONG_SIZE if the matrix is not $3\times 3$ .

Definition at line 195 of file controller.cpp.

References WRONG_SIZE.

short Impedance::set_Mo	(	const Real	Mo_i,
		const short	i
	)

Assign the rotational impedance inertia term $M_o(i,i)$ .

Returns: short: 0 or WRONG_SIZE if the matrix is not $3\times 3$ .

Definition at line 212 of file controller.cpp.

References WRONG_SIZE.

short Impedance::set_Mp ( const DiagonalMatrix & Mp_ )

Assign the translational impedance inertia matrix $M_p$ .

Returns: short: 0 or WRONG_SIZE if the matrix is not $3\times 3$ .

Definition at line 96 of file controller.cpp.

References WRONG_SIZE.

short Impedance::set_Mp	(	const Real	Mp_i,
		const short	i
	)

Assign the translational impedance inertia term $M_p(i,i)$ .

Returns: short: 0 or WRONG_SIZE if the matrix is not $3\times 3$ .

Definition at line 113 of file controller.cpp.

References WRONG_SIZE.

Public Member Functions

Public Attributes

Private Attributes

Detailed Description

Member Function Documentation