Homepage Demos Overview Downloads Tutorials Reference
Credits

ROBOOP::Resolved_acc Class Reference

#include <controller.h>

List of all members.


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 164 of file 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.
 Resolved_acc (const Resolved_acc &x)
 Copy constructor.
Resolved_accoperator= (const Resolved_acc &x)
 Overload = operator.
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.

Private Attributes

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


Constructor & Destructor Documentation

ROBOOP::Resolved_acc::Resolved_acc const short  dof = 1  ) 
 

Constructor.

Definition at line 444 of file controller.cpp.

ROBOOP::Resolved_acc::Resolved_acc const Robot_basic robot,
const Real  Kvp,
const Real  Kpp,
const Real  Kvo,
const Real  Kpo
 

Constructor.

Definition at line 463 of file controller.cpp.

ROBOOP::Resolved_acc::Resolved_acc const Resolved_acc x  ) 
 

Copy constructor.

Definition at line 484 of file controller.cpp.


Member Function Documentation

Resolved_acc & ROBOOP::Resolved_acc::operator= const Resolved_acc x  ) 
 

Overload = operator.

Definition at line 501 of file controller.cpp.

void ROBOOP::Resolved_acc::set_Kpo const Real  Kpo  ) 
 

Assign the gain $k_{po}$.

Definition at line 538 of file controller.cpp.

Referenced by Resolved_acc(), and ROBOOP::Control_Select::set_control().

void ROBOOP::Resolved_acc::set_Kpp const Real  Kpp  ) 
 

Assign the gain $k_{pp}$.

Definition at line 526 of file controller.cpp.

Referenced by Resolved_acc(), and ROBOOP::Control_Select::set_control().

void ROBOOP::Resolved_acc::set_Kvo const Real  Kvo  ) 
 

Assign the gain $k_{vo}$.

Definition at line 532 of file controller.cpp.

Referenced by Resolved_acc(), and ROBOOP::Control_Select::set_control().

void ROBOOP::Resolved_acc::set_Kvp const Real  Kvp  ) 
 

Assign the gain $k_{vp}$.

Definition at line 520 of file controller.cpp.

Referenced by Resolved_acc(), and ROBOOP::Control_Select::set_control().

ReturnMatrix ROBOOP::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 544 of file controller.cpp.

Referenced by ROBOOP::Dynamics::xdot().


Member Data Documentation

ColumnVector ROBOOP::Resolved_acc::a [private]
 

Control input.

Definition at line 189 of file controller.h.

Referenced by operator=(), Resolved_acc(), and torque_cmd().

double ROBOOP::Resolved_acc::Kpo [private]
 

Controller gains.

Definition at line 184 of file controller.h.

Referenced by operator=(), Resolved_acc(), set_Kpo(), and torque_cmd().

double ROBOOP::Resolved_acc::Kpp [private]
 

Controller gains.

Definition at line 184 of file controller.h.

Referenced by operator=(), Resolved_acc(), set_Kpp(), and torque_cmd().

double ROBOOP::Resolved_acc::Kvo [private]
 

Controller gains.

Definition at line 184 of file controller.h.

Referenced by operator=(), Resolved_acc(), set_Kvo(), and torque_cmd().

double ROBOOP::Resolved_acc::Kvp [private]
 

Controller gains.

Definition at line 184 of file controller.h.

Referenced by operator=(), Resolved_acc(), set_Kvp(), and torque_cmd().

ColumnVector ROBOOP::Resolved_acc::p [private]
 

End effector position.

Definition at line 189 of file controller.h.

Referenced by operator=(), Resolved_acc(), and torque_cmd().

ColumnVector ROBOOP::Resolved_acc::pp [private]
 

End effector velocity.

Definition at line 189 of file controller.h.

Referenced by operator=(), Resolved_acc(), and torque_cmd().

ColumnVector ROBOOP::Resolved_acc::qp [private]
 

Robot joints velocity.

Definition at line 189 of file controller.h.

Referenced by operator=(), Resolved_acc(), and torque_cmd().

ColumnVector ROBOOP::Resolved_acc::qpp [private]
 

Robot joints acceleration.

Definition at line 189 of file controller.h.

Referenced by operator=(), Resolved_acc(), and torque_cmd().

Quaternion ROBOOP::Resolved_acc::quat [private]
 

Temporary quaternion.

Definition at line 195 of file controller.h.

Referenced by operator=(), Resolved_acc(), and torque_cmd().

ColumnVector ROBOOP::Resolved_acc::quat_v_error [private]
 

Vector part of error quaternion.

Definition at line 189 of file controller.h.

Referenced by operator=(), Resolved_acc(), and torque_cmd().

Matrix ROBOOP::Resolved_acc::Rot [private]
 

Temporay rotation matrix.

Definition at line 187 of file controller.h.

Referenced by torque_cmd().

ColumnVector ROBOOP::Resolved_acc::zero3 [private]
 

$3\times 1$ zero vector.

Definition at line 189 of file controller.h.

Referenced by operator=(), and Resolved_acc().


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

ROBOOP v1.21a
Generated Tue Nov 23 16:35:55 2004 by Doxygen 1.3.9.1