Impedance controller class. More...
#include <controller.h>
Public Member Functions | |
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. | |
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. | |
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_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_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 | 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_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 . | |
Public Attributes | |
ColumnVector | pc |
Compliant position. | |
ColumnVector | pcd |
Difference between pc and desired position. | |
ColumnVector | pcdp |
Difference between pcp and desired velocity. | |
ColumnVector | pcp |
Compliant velocity. | |
ColumnVector | pcp_prev |
Previous value of pcp. | |
ColumnVector | pcpp |
Compliant acceleration. | |
ColumnVector | pcpp_prev |
Previous value of pcpp. | |
Quaternion | qc |
Compliant frame quaternion. | |
Quaternion | qcd |
Orientation error (betweem compliant and desired frame) quaternion. | |
Quaternion | qcp |
Compliant frame quaternion derivative. | |
Quaternion | qcp_prev |
Previous value of qcp. | |
Quaternion | quat |
Temporary quaternion. | |
ColumnVector | wc |
Compliant angular velocity. | |
ColumnVector | wcd |
Difference between wc and desired angular velocity. | |
ColumnVector | wcp |
Compliant angular acceleration. | |
ColumnVector | wcp_prev |
Previous value of wcp. | |
Private Attributes | |
DiagonalMatrix | Do |
Rotational impedance damping matrix. | |
DiagonalMatrix | Dp |
Translational impedance damping matrix. | |
DiagonalMatrix | Ko |
Rotational impedance stifness matrix. | |
Matrix | Ko_prime |
Modified rotational impedance stifness matrix. | |
DiagonalMatrix | Kp |
Translational impedance stifness matrix. | |
DiagonalMatrix | Mo |
Rotational impedance inertia matrix. | |
DiagonalMatrix | Mp |
Translational impedance inertia matrix. |
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 91 of file controller.h.
Constructor.
Definition at line 54 of file controller.cpp.
Impedance::Impedance | ( | const Robot_basic & | robot, |
const DiagonalMatrix & | Mp_, | ||
const DiagonalMatrix & | Dp_, | ||
const DiagonalMatrix & | Kp_, | ||
const DiagonalMatrix & | Mo_, | ||
const DiagonalMatrix & | Do_, | ||
const DiagonalMatrix & | Ko_ | ||
) |
Constructor.
Definition at line 69 of file controller.cpp.
short Impedance::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.
pdpp,: | desired end effector acceleration. |
pdp,: | desired end effector velocity. |
pd,: | desired end effector position. |
wdp,: | desired end effector angular acceleration. |
wd,: | desired end effector angular velocity. |
qd,: | desired quaternion. |
f,: | end effector contact force. |
n,: | end effector contact moment. |
dt,: | time frame. |
The translational and rotational impedance equations are integrated, with input and to computed and , and , and then and . The compliant quaternion is obtained with the quaternion propagation equations (see Quaternion class).
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 298 of file controller.cpp.
short Impedance::set_Do | ( | const DiagonalMatrix & | Do_ | ) |
Assign the rotational impedance damping matrix .
Definition at line 232 of file controller.cpp.
short Impedance::set_Do | ( | const Real | Do_i, |
const short | i | ||
) |
Assign the rotational impedance damping term .
Definition at line 249 of file controller.cpp.
short Impedance::set_Dp | ( | const DiagonalMatrix & | Dp_ | ) |
Assign the translational impedance damping matrix .
Definition at line 133 of file controller.cpp.
short Impedance::set_Dp | ( | const Real | Dp_i, |
const short | i | ||
) |
Assign the translational impedance damping term .
Definition at line 150 of file controller.cpp.
short Impedance::set_Ko | ( | const DiagonalMatrix & | Ko_ | ) |
Assign the rotational impedance stifness matrix .
Definition at line 265 of file controller.cpp.
short Impedance::set_Ko | ( | const Real | Ko_i, |
const short | i | ||
) |
Assign the rotational impedance stifness term .
Definition at line 282 of file controller.cpp.
short Impedance::set_Kp | ( | const DiagonalMatrix & | Kp_ | ) |
Assign the translational impedance stifness matrix .
Definition at line 166 of file controller.cpp.
short Impedance::set_Kp | ( | const Real | Kp_i, |
const short | i | ||
) |
Assign the translational impedance stifness term .
Definition at line 183 of file controller.cpp.
short Impedance::set_Mo | ( | const DiagonalMatrix & | Mo_ | ) |
Assign the rotational impedance inertia matrix .
Definition at line 199 of file controller.cpp.
short Impedance::set_Mo | ( | const Real | Mo_i, |
const short | i | ||
) |
Assign the rotational impedance inertia term .
Definition at line 216 of file controller.cpp.
short Impedance::set_Mp | ( | const DiagonalMatrix & | Mp_ | ) |
Assign the translational impedance inertia matrix .
Definition at line 100 of file controller.cpp.
short Impedance::set_Mp | ( | const Real | Mp_i, |
const short | i | ||
) |
Assign the translational impedance inertia term .
Definition at line 117 of file controller.cpp.
DiagonalMatrix Impedance::Do [private] |
Rotational impedance damping matrix.
Definition at line 133 of file controller.h.
DiagonalMatrix Impedance::Dp [private] |
Translational impedance damping matrix.
Definition at line 133 of file controller.h.
DiagonalMatrix Impedance::Ko [private] |
Rotational impedance stifness matrix.
Definition at line 133 of file controller.h.
Matrix Impedance::Ko_prime [private] |
Modified rotational impedance stifness matrix.
Definition at line 139 of file controller.h.
DiagonalMatrix Impedance::Kp [private] |
Translational impedance stifness matrix.
Definition at line 133 of file controller.h.
DiagonalMatrix Impedance::Mo [private] |
Rotational impedance inertia matrix.
Definition at line 133 of file controller.h.
DiagonalMatrix Impedance::Mp [private] |
Translational impedance inertia matrix.
Definition at line 133 of file controller.h.
ColumnVector Impedance::pc |
Compliant position.
Definition at line 121 of file controller.h.
ColumnVector Impedance::pcd |
Difference between pc and desired position.
Definition at line 121 of file controller.h.
ColumnVector Impedance::pcdp |
Difference between pcp and desired velocity.
Definition at line 121 of file controller.h.
ColumnVector Impedance::pcp |
Compliant velocity.
Definition at line 121 of file controller.h.
ColumnVector Impedance::pcp_prev |
Previous value of pcp.
Definition at line 121 of file controller.h.
ColumnVector Impedance::pcpp |
Compliant acceleration.
Definition at line 121 of file controller.h.
ColumnVector Impedance::pcpp_prev |
Previous value of pcpp.
Definition at line 121 of file controller.h.
Compliant frame quaternion.
Definition at line 116 of file controller.h.
Orientation error (betweem compliant and desired frame) quaternion.
Definition at line 116 of file controller.h.
Compliant frame quaternion derivative.
Definition at line 116 of file controller.h.
Previous value of qcp.
Definition at line 116 of file controller.h.
Temporary quaternion.
Definition at line 116 of file controller.h.
ColumnVector Impedance::wc |
Compliant angular velocity.
Definition at line 121 of file controller.h.
ColumnVector Impedance::wcd |
Difference between wc and desired angular velocity.
Definition at line 121 of file controller.h.
ColumnVector Impedance::wcp |
Compliant angular acceleration.
Definition at line 121 of file controller.h.
ColumnVector Impedance::wcp_prev |
Previous value of wcp.
Definition at line 121 of file controller.h.