ROBOOP::Impedance Class Reference#include <controller.h>
List of all members.
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.
where and 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, . The quaternion mathematics are implemented in the Quaternion class. The index and denote the compliance and the desired respectively.
The impedance parameters , , , , and are diagonal positive definite matrix
Definition at line 82 of file 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.
|
| Impedance (const Impedance &x) |
| Copy constructor.
|
Impedance & | operator= (const Impedance &x) |
| Overload = operator.
|
short | set_Mp (const DiagonalMatrix &Mp_) |
| Assign the translational impedance inertia matrix .
|
short | set_Mp (const Real MP_i, const short i) |
| Assign the translational impedance inertia term .
|
short | set_Dp (const DiagonalMatrix &Dp_) |
| Assign the translational impedance damping matrix .
|
short | set_Dp (const Real Dp_i, const short i) |
| Assign the translational impedance damping term .
|
short | set_Kp (const DiagonalMatrix &Kp_) |
| Assign the translational impedance stifness matrix .
|
short | set_Kp (const Real Kp_i, const short i) |
| Assign the translational impedance stifness term .
|
short | set_Mo (const DiagonalMatrix &Mo_) |
| Assign the rotational impedance inertia matrix .
|
short | set_Mo (const Real Mo_i, const short i) |
| Assign the rotational impedance inertia term .
|
short | set_Do (const DiagonalMatrix &Do_) |
| Assign the rotational impedance damping matrix .
|
short | set_Do (const Real Do_i, const short i) |
| Assign the rotational impedance damping term .
|
short | set_Ko (const DiagonalMatrix &Ko_) |
| Assign the rotational impedance stifness matrix .
|
short | set_Ko (const Real Ko_i, const short i) |
| Assign the rotational impedance stifness term .
|
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.
|
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.
|
Constructor & Destructor Documentation
ROBOOP::Impedance::Impedance |
( |
|
) |
|
|
ROBOOP::Impedance::Impedance |
( |
const Impedance & |
x |
) |
|
|
Member Function Documentation
short ROBOOP::Impedance::set_Do |
( |
const Real |
Do_i, |
|
|
const short |
i |
|
) |
|
|
|
Assign the rotational impedance damping term .
- Returns:
- short: 0 or WRONG_SIZE if the matrix is not
.
Definition at line 286 of file controller.cpp. |
|
Assign the rotational impedance damping matrix .
- Returns:
- short: 0 or WRONG_SIZE if the matrix is not
.
Definition at line 269 of file controller.cpp.
Referenced by Impedance(). |
short ROBOOP::Impedance::set_Dp |
( |
const Real |
Dp_i, |
|
|
const short |
i |
|
) |
|
|
|
Assign the translational impedance damping term .
- Returns:
- short: 0 or WRONG_SIZE if the matrix is not
.
Definition at line 187 of file controller.cpp. |
|
Assign the translational impedance damping matrix .
- Returns:
- short: 0 or WRONG_SIZE if the matrix is not
.
Definition at line 170 of file controller.cpp.
Referenced by Impedance(). |
short ROBOOP::Impedance::set_Ko |
( |
const Real |
Ko_i, |
|
|
const short |
i |
|
) |
|
|
|
Assign the rotational impedance stifness term .
- Returns:
- short: 0 or WRONG_SIZE if the matrix is not
.
Definition at line 319 of file controller.cpp. |
|
Assign the rotational impedance stifness matrix .
- Returns:
- short: 0 or WRONG_SIZE if the matrix is not
.
Definition at line 302 of file controller.cpp.
Referenced by Impedance(). |
short ROBOOP::Impedance::set_Kp |
( |
const Real |
Kp_i, |
|
|
const short |
i |
|
) |
|
|
|
Assign the translational impedance stifness term .
- Returns:
- short: 0 or WRONG_SIZE if the matrix is not
.
Definition at line 220 of file controller.cpp. |
|
Assign the translational impedance stifness matrix .
- Returns:
- short: 0 or WRONG_SIZE if the matrix is not
.
Definition at line 203 of file controller.cpp.
Referenced by Impedance(). |
short ROBOOP::Impedance::set_Mo |
( |
const Real |
Mo_i, |
|
|
const short |
i |
|
) |
|
|
|
Assign the rotational impedance inertia term .
- Returns:
- short: 0 or WRONG_SIZE if the matrix is not
.
Definition at line 253 of file controller.cpp. |
|
Assign the rotational impedance inertia matrix .
- Returns:
- short: 0 or WRONG_SIZE if the matrix is not
.
Definition at line 236 of file controller.cpp.
Referenced by Impedance(). |
short ROBOOP::Impedance::set_Mp |
( |
const Real |
Mp_i, |
|
|
const short |
i |
|
) |
|
|
|
Assign the translational impedance inertia term .
- Returns:
- short: 0 or WRONG_SIZE if the matrix is not
.
Definition at line 154 of file controller.cpp. |
|
Assign the translational impedance inertia matrix .
- Returns:
- short: 0 or WRONG_SIZE if the matrix is not
.
Definition at line 137 of file controller.cpp.
Referenced by Impedance(). |
Member Data Documentation
|
Orientation error (betweem compliant and desired frame) quaternion.
Definition at line 110 of file controller.h.
Referenced by control(). |
|
Difference between wc and desired angular velocity.
Definition at line 115 of file controller.h.
Referenced by control(). |
The documentation for this class was generated from the following files:
|