osaODEManipulator.h

Go to the documentation of this file.

/*

  Author(s): Simon Leonard
  Created on: Nov 11 2009

  (C) Copyright 2008 Johns Hopkins University (JHU), All Rights
  Reserved.

--- begin cisst license - do not edit ---

This software is provided "as is" under an open source license, with
no warranty.  The complete license can be found in license.txt and
http://www.cisst.org/cisst/license.txt.

--- end cisst license ---
*/

#ifndef _osaODEManipulator_h
#define _osaODEManipulator_h

#include <vector>

#include <sawOpenSceneGraph/osaOSGManipulator.h>

#include <sawOpenDynamicsEngine/osaODEBody.h>
#include <sawOpenDynamicsEngine/osaODEJoint.h>
#include <sawOpenDynamicsEngine/osaODEServoMotor.h>
#include <sawOpenDynamicsEngine/osaODEWorld.h>

#include <sawOpenDynamicsEngine/sawOpenDynamicsEngineExport.h>

class CISST_EXPORT osaODEManipulator : public osaOSGManipulator {

 private:

  osaODEWorld* odeworld;

  std::vector< osg::ref_ptr<osaODEBody> >  odebodies;

  std::vector<osaODEJoint*> odejoints;

  std::vector<osaODEServoMotor*> odeservos;

  vctDynamicVector<double> qinit;

  void CreateManipulator( const std::vector<std::string>& models,
              const std::string& basemodel );

  void CreateManipulator( const std::vector<std::string>& models );

 protected:

  osaODEWorld* GetWorld() const{ return odeworld; }
  dWorldID GetWorldID() const { return odeworld->GetWorldID(); }

  void Insert( osaODEBody* body );

  void Insert( osaODEJoint* joint );

  void Insert( osaODEServoMotor* servo );

 public:
  /*
  osaODEManipulator( const std::string& devname, 
               double period,
               devManipulator::State state,
               osaCPUMask mask,
               osaODEWorld* world,
               devManipulator::Mode mode );
  */

  osaODEManipulator( const std::vector<std::string>& models,
               osaODEWorld* world,
               const vctFrame4x4<double>& Rtw0,
               const std::string& robotfilename,
               const std::string& basemodel,
               const vctDynamicVector<double>& qinit );


  osaODEManipulator( const std::vector<std::string>& models,
               osaODEWorld* world,
               const vctFrm3& Rtw0,
               const std::string& robotfilename,
               const std::string& basemodel,
               const vctDynamicVector<double>& qinit );


  osaODEManipulator( const std::vector<std::string>& models,
               osaODEWorld* world,
               const vctFrame4x4<double>& Rtw0,
               const std::string& robotfilename,
               osaODEBody* base,
               const vctDynamicVector<double>& qinit );

  osaODEManipulator( const std::vector<std::string>& models,
               osaODEWorld* world,
               const vctFrm3& Rtw0,
               const std::string& robotfilename,
               osaODEBody* base,
               const vctDynamicVector<double>& qinit );

  ~osaODEManipulator(){}


  virtual 
    osaODEManipulator::Errno 
    GetPositions( vctDynamicVector<double>& q ) const ;


  virtual 
    osaODEManipulator::Errno
    GetVelocities( vctDynamicVector<double>& qd ) const ;


  virtual 
    osaODEManipulator::Errno
    SetPositions( const vctDynamicVector<double>& qs );


  virtual 
    osaODEManipulator::Errno 
    SetVelocities( const vctDynamicVector<double>& qds );
  

  virtual 
    osaODEManipulator::Errno 
    SetForcesTorques( const vctDynamicVector<double>& ft);

#ifndef SWIG

  typedef std::vector< osaODEBody::State > State;


  virtual osaODEManipulator::State GetState( ) const;


  virtual void SetState( const osaODEManipulator::State& state );

#endif


  dBodyID GetBaseID() const;

  virtual
    vctFrame4x4<double> 
    ForwardKinematics( const vctDynamicVector<double>& q, int N=-1 ) const;

  void 
    ForwardKinematics( const vctDynamicVector<double>& q, 
               vctFrm3& Rt, int N=-1 ) const;


  virtual void Attach( osaOSGManipulator* osgtool );

  void Disable();

  void Enable();
  
};

#endif