cisst-saw
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
Public Member Functions | Protected Member Functions | Protected Attributes | List of all members
osaOSGManipulator Class Reference

#include <osaOSGManipulator.h>

Inheritance diagram for osaOSGManipulator:
robManipulator osaODEManipulator osaOSGBH osaODEBarrettHand

Public Member Functions

 osaOSGManipulator (const std::vector< std::string > &models, osaOSGWorld *world, const vctFrame4x4< double > &Rtw0, const std::string &robfilename, const std::string &basemodel="")
 OSG Manipulator generic constructor. More...
 
 osaOSGManipulator (const std::vector< std::string > &models, osaOSGWorld *world, const vctFrm3 &Rtw0, const std::string &robfilename, const std::string &basemodel="")
 OSG Manipulator generic constructor. More...
 
 ~osaOSGManipulator ()
 
void Initialize ()
 
virtual osaOSGManipulator::Errno GetPositions (vctDynamicVector< double > &q) const
 Return the joints positions. More...
 
virtual osaOSGManipulator::Errno SetPositions (const vctDynamicVector< double > &q)
 Set the joint position. More...
 
unsigned int GetNumLinks ()
 
osaOSGBodyGetLink (size_t i)
 
osaOSGBodyGetBase ()
 
virtual void Attach (osaOSGManipulator *CMN_UNUSED(tool))
 
- Public Member Functions inherited from robManipulator
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...
 

Protected Member Functions

 osaOSGManipulator (const vctFrame4x4< double > &Rtw0, const std::string &robotfile)
 
 osaOSGManipulator (const vctFrm3 &Rtw0, const std::string &robotfile)
 

Protected Attributes

vctDynamicVector< double > q
 Store the current joints values. More...
 
osg::ref_ptr< osaOSGBodybase
 
osg::ref_ptr< osg::Group > osglinks
 
- Protected Attributes inherited from robManipulator
std::vector< robManipulator * > tools
 A vector of tools. More...
 

Additional Inherited Members

- Public Types inherited from robManipulator
enum  Errno { ESUCCESS, EFAILURE }
 
enum  LinkID {
  L0, L1, L2, L3,
  L4, L5, L6, L7,
  L8, L9, LN
}
 
- Public Attributes inherited from robManipulator
vctFrame4x4< double > Rtw0
 Position and orientation of the first link. More...
 
double ** Jn
 Body Jacobian. More...
 
double ** Js
 Spatial Jacobian. More...
 
std::vector< robLinklinks
 A vector of links. More...
 

Constructor & Destructor Documentation

osaOSGManipulator::osaOSGManipulator ( const vctFrame4x4< double > &  Rtw0,
const std::string &  robotfile 
)
inlineprotected
osaOSGManipulator::osaOSGManipulator ( const vctFrm3 Rtw0,
const std::string &  robotfile 
)
inlineprotected
osaOSGManipulator::osaOSGManipulator ( const std::vector< std::string > &  models,
osaOSGWorld world,
const vctFrame4x4< double > &  Rtw0,
const std::string &  robfilename,
const std::string &  basemodel = "" 
)

OSG Manipulator generic constructor.

This constructor initializes an OSG manipulator with the kinematics and dynamics contained in a file. Plus it initializes the OSG elements of the manipulators (bodies and joints) for the engine.

Parameters
modelsA vector of 3D model file names
world[in]The OSG world
Rtw0The offset transformation of the robot base
robotfileThe file with the kinematics and dynamics parameters
basemodelThe file name of the base 3D model
osaOSGManipulator::osaOSGManipulator ( const std::vector< std::string > &  models,
osaOSGWorld world,
const vctFrm3 Rtw0,
const std::string &  robfilename,
const std::string &  basemodel = "" 
)

OSG Manipulator generic constructor.

This constructor initializes an OSG manipulator with the kinematics and dynamics contained in a file. Plus it initializes the OSG elements of the manipulators (bodies and joints) for the engine.

Parameters
modelsA vector of 3D model file names
worldThe OSG world
Rtw0The offset transformation of the robot base
robotfileThe file with the kinematics and dynamics parameters
basemodelThe file name of the base 3D model
osaOSGManipulator::~osaOSGManipulator ( )

Member Function Documentation

virtual void osaOSGManipulator::Attach ( osaOSGManipulator CMN_UNUSEDtool)
inlinevirtual
osaOSGBody* osaOSGManipulator::GetBase ( )
inline
osaOSGBody* osaOSGManipulator::GetLink ( size_t  i)
unsigned int osaOSGManipulator::GetNumLinks ( )
virtual osaOSGManipulator::Errno osaOSGManipulator::GetPositions ( vctDynamicVector< double > &  q) const
virtual

Return the joints positions.

Query each joint and return the joint positions

Parameters
q[in]A vector of joints positions
Returns
ESUCCESS if successfull. EFAILURE otherwise.

Reimplemented in osaODEManipulator, and osaOSGBH.

void osaOSGManipulator::Initialize ( )
virtual osaOSGManipulator::Errno osaOSGManipulator::SetPositions ( const vctDynamicVector< double > &  q)
virtual

Set the joint position.

This sets the position command and stores a local copy

Parameters
qA vector of joint positions
Returns
ESUCCESS if successfull. EFAILURE otherwise.

Reimplemented in osaODEManipulator, and osaOSGBH.

Member Data Documentation

osg::ref_ptr<osaOSGBody> osaOSGManipulator::base
protected
osg::ref_ptr<osg::Group> osaOSGManipulator::osglinks
protected
vctDynamicVector<double> osaOSGManipulator::q
protected

Store the current joints values.


The documentation for this class was generated from the following file: