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
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 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_acc & | operator= (const Resolved_acc &x) |
| Overload = operator.
|
void | set_Kvp (const Real Kvp) |
| Assign the gain .
|
void | set_Kpp (const Real Kpp) |
| Assign the gain .
|
void | set_Kvo (const Real Kvo) |
| Assign the gain .
|
void | set_Kpo (const Real Kpo) |
| 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 |
double | Kvp |
| Controller gains.
|
double | Kpp |
| Controller gains.
|
double | Kvo |
| Controller gains.
|
double | Kpo |
| Controller gains.
|
Matrix | Rot |
| Temporay rotation matrix.
|
ColumnVector | zero3 |
| 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 |
) |
|
|
ROBOOP::Resolved_acc::Resolved_acc |
( |
const Resolved_acc & |
x |
) |
|
|
Member Function Documentation
void ROBOOP::Resolved_acc::set_Kpo |
( |
const Real |
Kpo |
) |
|
|
void ROBOOP::Resolved_acc::set_Kpp |
( |
const Real |
Kpp |
) |
|
|
void ROBOOP::Resolved_acc::set_Kvo |
( |
const Real |
Kvo |
) |
|
|
void ROBOOP::Resolved_acc::set_Kvp |
( |
const Real |
Kvp |
) |
|
|
|
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
The documentation for this class was generated from the following files:
|