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_Kpo (const Real Kpo) |
Assign the gain . | |
void | set_Kpp (const Real Kpp) |
Assign the gain . | |
void | set_Kvo (const Real Kvo) |
Assign the gain . | |
void | set_Kvp (const Real Kvp) |
Assign the gain . | |
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 |
zero vector. |
Resolved rate acceleration controller class.
The dynamic model of a robot manipulator can be expressed in joint space as
According to the concept of inverse dynamics, the driving torques can be chosen as
where is the a new control input defined by
where and is the vector par of the quaternion representing the orientation error between the desired and end effector frame. , , and 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.
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.
void Resolved_acc::set_Kpo | ( | const Real | Kpo | ) |
Assign the gain .
Definition at line 465 of file controller.cpp.
void Resolved_acc::set_Kpp | ( | const Real | Kpp | ) |
Assign the gain .
Definition at line 453 of file controller.cpp.
void Resolved_acc::set_Kvo | ( | const Real | Kvo | ) |
Assign the gain .
Definition at line 459 of file controller.cpp.
void Resolved_acc::set_Kvp | ( | const Real | Kvp | ) |
Assign the gain .
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.
ColumnVector Resolved_acc::a [private] |
Control input.
Definition at line 193 of file controller.h.
double Resolved_acc::Kpo [private] |
Definition at line 188 of file controller.h.
double Resolved_acc::Kpp [private] |
Definition at line 188 of file controller.h.
double Resolved_acc::Kvo [private] |
Definition at line 188 of file controller.h.
double Resolved_acc::Kvp [private] |
Controller gains.
Definition at line 188 of file controller.h.
ColumnVector Resolved_acc::p [private] |
End effector position.
Definition at line 193 of file controller.h.
ColumnVector Resolved_acc::pp [private] |
End effector velocity.
Definition at line 193 of file controller.h.
ColumnVector Resolved_acc::qp [private] |
Robot joints velocity.
Definition at line 193 of file controller.h.
ColumnVector Resolved_acc::qpp [private] |
Robot joints acceleration.
Definition at line 193 of file controller.h.
Quaternion Resolved_acc::quat [private] |
Temporary quaternion.
Definition at line 200 of file controller.h.
ColumnVector Resolved_acc::quat_v_error [private] |
Vector part of error quaternion.
Definition at line 193 of file controller.h.
Matrix Resolved_acc::Rot [private] |
Temporay rotation matrix.
Definition at line 192 of file controller.h.
ColumnVector Resolved_acc::zero3 [private] |
zero vector.
Definition at line 193 of file controller.h.