cisst-saw
|
#include <osaGravityCompensation.h>
Public Types | |
enum | Errno { ESUCCESS, EFAILURE } |
![]() | |
enum | Errno { ESUCCESS, EFAILURE } |
enum | LinkID { L0, L1, L2, L3, L4, L5, L6, L7, L8, L9, LN } |
Public Member Functions | |
osaGravityCompensation (const std::string &robfile, const vctFrame4x4< double > &Rtwb) | |
Main constructor. More... | |
osaGravityCompensation::Errno | Evaluate (const vctDynamicVector< double > &q, vctDynamicVector< double > &tau) |
Evaluate the control law. More... | |
![]() | |
robManipulator::Errno | LoadRobot (const std::string &linkfile) |
Load the kinematics and the dynamics of the robot. More... | |
robManipulator::Errno | LoadRobot (std::vector< robKinematics * > KinParms) |
void | JacobianBody (const vctDynamicVector< double > &q) const |
Evaluate the body Jacobian. More... | |
bool | JacobianBody (const vctDynamicVector< double > &q, vctDynamicMatrix< double > &J) const |
Evaluate the body Jacobian and return it in the dynamic matrix J. More... | |
void | JacobianSpatial (const vctDynamicVector< double > &q) const |
Evaluate the spatial Jacobian. More... | |
bool | JacobianSpatial (const vctDynamicVector< double > &q, vctDynamicMatrix< double > &J) const |
Evaluate the spatial Jacobian and return it in the dynamic matrix J. More... | |
vctDynamicVector< double > | RNE (const vctDynamicVector< double > &q, const vctDynamicVector< double > &qd, const vctDynamicVector< double > &qdd, const vctFixedSizeVector< double, 6 > &f, double g=9.81) const |
Recursive Newton-Euler altorithm. More... | |
vctDynamicVector< double > | CCG (const vctDynamicVector< double > &q, const vctDynamicVector< double > &qd) const |
Coriolis/centrifugal and gravity. More... | |
vctFixedSizeVector< double, 6 > | BiasAcceleration (const vctDynamicVector< double > &q, const vctDynamicVector< double > &qd) const |
End-effector accelerations. More... | |
void | JSinertia (double **A, const vctDynamicVector< double > &q) const |
Compute the NxN manipulator inertia matrix. More... | |
vctDynamicMatrix< double > | JSinertia (const vctDynamicVector< double > &q) const |
void | OSinertia (double Ac[6][6], const vctDynamicVector< double > &q) const |
Compute the 6x6 manipulator inertia matrix in operation space. More... | |
vctFixedSizeMatrix< double, 4, 4 > | SE3Difference (const vctFrame4x4< double > &Rt1, const vctFrame4x4< double > &Rt2) const |
void | AddIdentificationColumn (vctDynamicMatrix< double > &J, vctFixedSizeMatrix< double, 4, 4 > &delRt) const |
robManipulator (const vctFrame4x4< double > &Rtw0=vctFrame4x4< double >()) | |
robManipulator (const std::string &robotfilename, const vctFrame4x4< double > &Rtw0=vctFrame4x4< double >()) | |
Manipulator generic constructor. More... | |
robManipulator (const std::vector< robKinematics * > linkParms, const vctFrame4x4< double > &Rtw0=vctFrame4x4< double >()) | |
virtual | ~robManipulator () |
Manipulator destructor. More... | |
virtual vctFrame4x4< double > | ForwardKinematics (const vctDynamicVector< double > &q, int N=-1) const |
Evaluate the forward kinematics. More... | |
virtual robManipulator::Errno | InverseKinematics (vctDynamicVector< double > &q, const vctFrame4x4< double > &Rts, double tolerance=1e-12, size_t Niteration=1000, double LAMBDA=0.001) |
Evaluate the inverse kinematics. More... | |
virtual robManipulator::Errno | InverseKinematics (vctDynamicVector< double > &q, const vctFrm3 &Rts, double tolerance=1e-12, size_t Niteration=1000) |
virtual void | NormalizeAngles (vctDynamicVector< double > &q) |
Normalize angles to -pi to pi. More... | |
virtual vctDynamicVector< double > | InverseDynamics (const vctDynamicVector< double > &q, const vctDynamicVector< double > &qd, const vctDynamicVector< double > &qdd) const |
Inverse dynamics in joint space. More... | |
virtual vctDynamicVector< double > | InverseDynamics (const vctDynamicVector< double > &q, const vctDynamicVector< double > &qd, const vctFixedSizeVector< double, 6 > &vdwd) const |
Inverse dynamics in operation space. More... | |
virtual vctDynamicMatrix< double > | JacobianKinematicsIdentification (const vctDynamicVector< double > &q, double epsilon=1e-6) const |
Compute Jacobian for kinematics identification. More... | |
void | PrintKinematics (std::ostream &os) const |
Print the kinematics parameters to the specified output stream. More... | |
virtual void | Attach (robManipulator *tool) |
Attach a tool. More... | |
Additional Inherited Members | |
![]() | |
vctFrame4x4< double > | Rtw0 |
Position and orientation of the first link. More... | |
double ** | Jn |
Body Jacobian. More... | |
double ** | Js |
Spatial Jacobian. More... | |
std::vector< robLink > | links |
A vector of links. More... | |
![]() | |
std::vector< robManipulator * > | tools |
A vector of tools. More... | |
osaGravityCompensation::osaGravityCompensation | ( | const std::string & | robfile, |
const vctFrame4x4< double > & | Rtwb | ||
) |
Main constructor.
[in] | robfilename | File containing the kinematics and dynamics parameters |
[in] | Rtwb | Position and orientation of the robot with resepct to world frame |
osaGravityCompensation::Errno osaGravityCompensation::Evaluate | ( | const vctDynamicVector< double > & | q, |
vctDynamicVector< double > & | tau | ||
) |
Evaluate the control law.
[in] | qs | Desired joint positions |
[out] | tau | Joint forces/torques |