robManipulator.h

Go to the documentation of this file.

/* -*- Mode: C++; tab-width: 2; indent-tabs-mode: nil; c-basic-offset: 2 -*-    */
/* ex: set filetype=cpp softtabstop=2 shiftwidth=2 tabstop=2 cindent expandtab: */

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

  (C) Copyright 2008-2015 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 _robManipulator_h
#define _robManipulator_h

#include <string>
#include <vector>

#include <cisstVector/vctTransformationTypes.h>
#include <cisstRobot/robLink.h>

#if CISST_HAS_JSON
#include <json/json.h>
#endif

#include <cisstRobot/robExport.h>

class CISST_EXPORT robManipulator{

 protected:

  std::vector<robManipulator*> tools;

 public:

  enum Errno{ ESUCCESS, EFAILURE };


  vctFrame4x4<double> Rtw0;


  double** Jn;


  double** Js;

  std::vector<robLink> links;


  robManipulator::Errno LoadRobot( const std::string& linkfile );

#if CISST_HAS_JSON
  robManipulator::Errno LoadRobot(const Json::Value & config);
#endif

  robManipulator::Errno LoadRobot(std::vector<robKinematics *> KinParms);


  void JacobianBody( const vctDynamicVector<double>& q ) const;

  //  Returns true if successful; false otherwise (e.g., J is wrong size)
  bool JacobianBody(const vctDynamicVector<double>& q,
                    vctDynamicMatrix<double>& J) const;



  void JacobianSpatial( const vctDynamicVector<double>& q ) const;

  //  Returns true if successful; false otherwise (e.g., J is wrong size)
  bool JacobianSpatial(const vctDynamicVector<double>& q,
                       vctDynamicMatrix<double>& J) const;


  vctDynamicVector<double>
  RNE( const vctDynamicVector<double>& q,
       const vctDynamicVector<double>& qd,
       const vctDynamicVector<double>& qdd,
       const vctFixedSizeVector<double,6>& f,//=vctFixedSizeVector<double,6>(0.0),
       double g = 9.81 ) const;


  vctDynamicVector<double>
  CCG( const vctDynamicVector<double>& q,
       const vctDynamicVector<double>& qd ) const;


  /*
  vctFixedSizeVector<double,6>
  Acceleration( const vctDynamicVector<double>& q,
        const vctDynamicVector<double>& qd,
        const vctDynamicVector<double>& qdd ) const;
  */

  vctFixedSizeVector<double,6>
  BiasAcceleration( const vctDynamicVector<double>& q,
                    const vctDynamicVector<double>& qd ) const;



  void JSinertia(double** A, const vctDynamicVector<double>& q ) const;

  vctDynamicMatrix<double> JSinertia( const vctDynamicVector<double>& q ) const;



  void OSinertia(double Ac[6][6], const vctDynamicVector<double>& q) const;

  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;

public:

  enum LinkID{ L0, L1, L2, L3, L4, L5, L6, L7, L8, L9, LN };

  robManipulator( const vctFrame4x4<double>& Rtw0 = vctFrame4x4<double>() );


  robManipulator( const std::string& robotfilename,
          const vctFrame4x4<double>& Rtw0 = vctFrame4x4<double>() );

  robManipulator( const std::vector<robKinematics *> linkParms,
                  const vctFrame4x4<double>& Rtw0 = vctFrame4x4<double>() );

  virtual ~robManipulator();


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


  virtual
    robManipulator::Errno
    InverseKinematics( vctDynamicVector<double>& q,
                       const vctFrame4x4<double>& Rts,
                       double tolerance=1e-12,
                       size_t Niteration=1000,
                       double LAMBDA=0.001 );


  virtual
    robManipulator::Errno
    InverseKinematics( vctDynamicVector<double>& q,
                       const vctFrm3& Rts,
                       double tolerance=1e-12,
                       size_t Niteration=1000 );

  virtual void NormalizeAngles( vctDynamicVector<double>& q );


  virtual
    vctDynamicVector<double>
    InverseDynamics( const vctDynamicVector<double>& q,
                     const vctDynamicVector<double>& qd,
                     const vctDynamicVector<double>& qdd ) const;


  virtual
    vctDynamicVector<double>
    InverseDynamics( const vctDynamicVector<double>& q,
             const vctDynamicVector<double>& qd,
             const vctFixedSizeVector<double,6>& vdwd ) const;



  virtual
    vctDynamicMatrix<double>
    JacobianKinematicsIdentification( const vctDynamicVector<double>& q,
                                      double epsilon = 1e-6 ) const;

  void PrintKinematics( std::ostream& os ) const;

  virtual void Attach( robManipulator* tool );

};

#endif