vpRobotFrankaSim.h

Go to the documentation of this file.

/*
 * This file is part of the ViSP software.
 * Copyright (C) 2005 - 2021 by INRIA. All rights reserved.
 *
 * This software is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License
 * ("GPL") version 2 as published by the Free Software Foundation.
 * See the file LICENSE.txt at the root directory of this source
 * distribution for additional information about the GNU GPL.
 *
 * For using ViSP with software that can not be combined with the GNU
 * GPL, please contact INRIA about acquiring a ViSP Professional
 * Edition License.
 *
 * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
 *
 * This software was developed at:
 * INRIA Rennes - Bretagne Atlantique
 * Campus Universitaire de Beaulieu
 * 35042 Rennes Cedex
 * France
 * http://www.irisa.fr/lagadic
 *
 * If you have questions regarding the use of this file, please contact
 * INRIA at visp@inria.fr
 *
 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
 *
 * Description:
 * Franka robot simulator.
 *
 * Authors:
 * Alexander Oliva
 * Fabien Spindler
 */

#ifndef vpRobotFrankaSim_h
#define vpRobotFrankaSim_h

#include <atomic>
#include <mutex>
#include <thread>

#include <visp3/core/vpConfig.h>
#include <visp3/core/vpPoseVector.h>
#include <visp3/core/vpThetaUVector.h>
#include <visp3/robot/vpRobot.h>

#if defined( VISP_HAVE_OROCOS_KDL )

#ifdef VISP_HAVE_OROCOS_KDL
#include <kdl/chain.hpp>
#include <kdl/chainfksolver.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>
#include <kdl/chainiksolverpos_nr.hpp>
#include <kdl/chainiksolverpos_nr_jl.hpp>
#include <kdl/chainiksolvervel_pinv.hpp>
#include <kdl/chainjnttojacsolver.hpp>
#include <kdl/frames_io.hpp>
#include <kdl/solveri.hpp>
#endif

class VISP_EXPORT vpRobotFrankaSim
{
public:
  vpRobotFrankaSim();
  virtual ~vpRobotFrankaSim();

  vpRobot::vpRobotStateType getRobotState( void );

  void get_fJe( vpMatrix &fJe );
  void get_fJe( const vpColVector &q, vpMatrix &fJe );
  void get_eJe( vpMatrix &eJe_ );
  void get_eJe( const vpColVector &q, vpMatrix &fJe );
  vpHomogeneousMatrix get_eMc() const;
  vpHomogeneousMatrix get_fMe( const vpColVector &q );
  vpHomogeneousMatrix get_fMe();
  vpHomogeneousMatrix get_flMe() const;
  vpHomogeneousMatrix get_flMcom() const;
  double get_tool_mass() const;

  void getGravity( vpColVector &gravity );
  void getMass( vpMatrix &mass );
  void getCoriolis( vpColVector &coriolis );
  void getCoriolisMatrix( vpMatrix &coriolis );
  void getFriction( vpColVector &friction );

  virtual void getForceTorque( const vpRobot::vpControlFrameType frame, vpColVector &force );
  virtual void getPosition( const vpRobot::vpControlFrameType frame, vpColVector &position );
  virtual void getPosition( const vpRobot::vpControlFrameType frame, vpPoseVector &position );
  virtual void getVelocity( const vpRobot::vpControlFrameType frame, vpColVector &d_position );

  virtual void set_eMc( const vpHomogeneousMatrix &eMc );
  virtual void setForceTorque( const vpRobot::vpControlFrameType frame, const vpColVector &force );
  virtual void setPosition( const vpRobot::vpControlFrameType frame, const vpColVector &position );
  virtual void setVelocity( const vpRobot::vpControlFrameType frame, const vpColVector &vel );

  virtual void add_tool( const vpHomogeneousMatrix &flMe, const double mL, const vpHomogeneousMatrix &flMcom,
                         const vpMatrix &I_L );
  virtual void set_flMe( const vpHomogeneousMatrix &flMe );
  virtual void set_g0( const vpColVector &g0 );

  inline void setVerbose( bool verbose ) { m_verbose = verbose; }

protected:
  vpColVector m_q;     // Joint Positions
  vpColVector m_dq;    // Joint Velocities
  vpColVector m_tau_J; // Joint efforts

  double m_mL;                  // payload mass
  vpHomogeneousMatrix m_flMcom; // payload Center of Mass pose in flange frame
  vpMatrix m_Il;                // payload inertia tensor
  vpHomogeneousMatrix m_flMe;   // End-effector pose in flange frame
  bool m_toolMounted;           // flag to indicate the presence of a tool
  bool m_camMounted;            // flag to indicate the presence of a camera

  vpColVector m_g0; // Absolute gravitational acceleration vector in base frame

  std::mutex m_mutex;

  vpColVector solveIK( const vpHomogeneousMatrix &edMw );
  vpColVector getVelDes();

#ifdef VISP_HAVE_OROCOS_KDL
  KDL::JntArray m_q_kdl;
  KDL::JntArray m_dq_des_kdl;
  KDL::Chain m_chain_kdl;
  KDL::JntArray m_q_min_kdl;
  KDL::JntArray m_q_max_kdl;
  KDL::ChainFkSolverPos_recursive *m_fksolver_kdl;
  KDL::ChainJntToJacSolver *m_jacobianSolver_kdl;
  KDL::ChainIkSolverVel_pinv *m_diffIkSolver_kdl;
  KDL::ChainIkSolverPos_NR_JL *m_iksolver_JL_kdl;
#endif

  vpRobot::vpRobotStateType m_stateRobot;

  vpColVector m_q_des; // Desired joint position.

  vpColVector m_dq_des;      // Desired joint velocity.
  vpColVector m_dq_des_filt; // Desired joint velocity filtered.
  vpColVector m_v_cart_des;  // Desired Cartesian velocity in end-effector frame.

  vpColVector m_tau_J_des;      // Desired joint torques.
  vpColVector m_tau_J_des_filt; // Desired joint torques filtered.

  vpHomogeneousMatrix m_eMc; // Transformation of the camera w.r.t. End-Effector frame
  vpVelocityTwistMatrix m_eVc;

  bool m_verbose;
};

#endif
#endif