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: 2009-11-11
 
  (C) Copyright 2008-2024 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:

  typedef std::vector<robManipulator*> ToolsType;
  ToolsType tools;
 
 public:
 
  enum Errno{ ESUCCESS, EFAILURE };
 
  std::string mLastError;


  vctFrame4x4<double> Rtw0;


  double** Jn;


  double** Js;

  std::vector<robLink> links;
 


  virtual robManipulator::Errno LoadRobot(const std::string & linkfile);
 
  virtual robManipulator::Errno LoadRobot(std::istream & ifs);
 
#if CISST_HAS_JSON
  virtual 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>
  RNE_MDH( 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>
  RNE_MDH( const vctDynamicVector<double>& q,
           const vctDynamicVector<double>& qd,
           const vctDynamicVector<double>& qdd,
           const vctFixedSizeVector<double,6>& f,//=vctFixedSizeVector<double,6>(0.0),
           const vct3 & g) const;


  vctDynamicVector<double>
  CCG( const vctDynamicVector<double>& q,
       const vctDynamicVector<double>& qd,
       double g = 9.81 ) const;
 
  vctDynamicVector<double>
  CCG_MDH( const vctDynamicVector<double>& q,
           const vctDynamicVector<double>& qd,
           double g = 9.81 ) const;
 
   vctDynamicVector<double>
  CCG_MDH( const vctDynamicVector<double>& q,
           const vctDynamicVector<double>& qd,
           const vct3 & g) 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 void
    SetJointLimits(const vctDynamicVector<double> & lowerLimits,
                   const vctDynamicVector<double> & upperLimits);

  virtual void
    GetJointLimits(vctDynamicVectorRef<double> lowerLimits,
                   vctDynamicVectorRef<double> upperLimits) const;

  virtual void
    GetFTMaximums(vctDynamicVectorRef<double> ftMaximums) const;

  virtual void
    GetJointNames(std::vector<std::string> & names) const;

  virtual void
    GetJointTypes(std::vector<cmnJointType> & types) const;


  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;

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

  virtual void Attach( robManipulator* tool );
 
  void DeleteTools(void);


  virtual
    robManipulator::Errno
    Truncate(const size_t linksToKeep);

  inline const std::string & LastError(void) const {
    return mLastError;
  }

  bool ClampJointValueAndUpdateError(const size_t jointIndex,
                                     double & value,
                                     const double & tolerance = 1e-6);
};
 
#endif // _robManipulator_h