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 171 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.
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
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.

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

References a, Quaternion::dot_prod(), Robot_basic::get_dof(), Robot_basic::get_q(), Robot_basic::get_qp(), Robot_basic::jacobian_DLS_inv(), Robot::jacobian_dot(), Robot::kine_pd(), Kpo, Kpp, Kvo, Kvp, p, pp, qp, qpp, quat, quat_v_error, robot, Rot, Quaternion::s(), Robot::torque(), Quaternion::v(), Robot_basic::w, x_prod_matrix(), and zero3.

Referenced by Dynamics::xdot().

Generated on Thu Dec 14 08:52:19 2006 for ROBOOP, A Robotics Object Oriented Package in C++ by  doxygen 1.5.1