PowerCubeSim.h

Go to the documentation of this file.

/*
 * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0

 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */


#ifndef __POWER_CUBE_SIM_H_
#define __POWER_CUBE_SIM_H_

#include <schunk_powercube_chain/PowerCubeCtrl.h>
#include <schunk_powercube_chain/Joint.h>
#include <iostream>
#include <stdio.h>
#include <stdlib.h>
#include <vector>

using namespace std;
#include <pthread.h>

//-------------------------------------------------------------------------
//                              Defines
// -------------------------------------------------------------------------

#define MAX_VEL 0.5
#define MAX_ACC 0.5
#define K13 0.5
#define K14 0.5
#define K15 3
#define DAEMPFUNG 0.5

//#define __LINUX__

#ifdef SWIG
%module PowerCubeSim
%include "Source/Manipulation/Interfaces/armInterface.h"
%{
  #include "PowerCubeSim.h"
%}
#endif

class PowerCubeSim;

/* Thread arguments for simulation threads*/
typedef struct
{
  PowerCubeSim* cubeSimPtr;
  int cubeID;
  double targetAngle;
} SimThreadArgs;

class PowerCubeSim
{
public:
  PowerCubeSim();
  ~PowerCubeSim();

  bool Init(PowerCubeCtrlParams* params);

  bool isInitialized() const
  {
    return m_Initialized;
  }

  std::string getErrorMessage() const
  {
    return m_ErrorMessage;
  }

  int Close()
  {
    return true;
  }
  // Arm-Ansteuerung:

  bool MoveJointSpaceSync(const std::vector<double>& Angle);

  bool MoveVel(const std::vector<double>& vel);

  bool Stop();

  // Funktionen zum setzen von Parametern: //

  bool setMaxVelocity(double radpersec);

  bool setMaxVelocity(const std::vector<double>& radpersec);

  bool setMaxAcceleration(double radPerSecSquared);
  bool setMaxAcceleration(const std::vector<double>& radPerSecSquared);

  // hier die Funktionen zur Statusabfrage: //

  bool getConfig(std::vector<double>& result);

  bool getJointVelocities(std::vector<double>& result);

  void setCurrentAngles(std::vector<double> Angles);

  void setCurrentJointVelocities(std::vector<double> Angles);

  bool statusMoving();
  bool statusMoving(int cubeNo);

  bool statusDec();

  bool statusAcc();

  // vector<int> getModuleMap(int dev);

  // void HomingDone();

  typedef enum
  {
    PC_CTRL_OK = 0,
    PC_CTRL_NOT_REFERENCED = -1,
    PC_CTRL_ERR = -2,
    PC_CTRL_POW_VOLT_ERR = -3
  } PC_CTRL_STATE;
  // int getStatus(){return PC_CTRL_OK;}
  double maxVel;

  void setStatusMoving(int cubeNo, bool moving);
  bool getStatusMoving(int cubeNo) const
  {
    return m_MovementInProgress[cubeNo];
  }

  vector<int> getModuleMap() const
  {
    return m_IdModules;
  }
  std::vector<double> getCurrentAngularMaxVel()
  {
    return m_CurrentAngularMaxVel;
  }
  std::vector<double> getCurrentAngularMaxAccel()
  {
    return m_CurrentAngularMaxAccel;
  }
  void millisleep(unsigned int milliseconds) const;

protected:
  // void waitForSync();
  // void dontWaitForSync();
  double timeRampMove(double dtheta, double vnow, double v, double a);

  int startSimulatedMovement(std::vector<double> target);

// void* SimThreadRoutine (void*);

#ifdef COB3
  Manipulator* m_Obj_Manipulator;
#endif

  int m_DOF;
  int m_Initialized;
  int m_NumOfModules;
  int m_Dev;
  vector<int> m_IdModules;

  std::vector<double> m_maxVel;
  std::vector<double> m_maxAcc;

  Jointd m_AngleOffsets;

  std::vector<bool> m_MovementInProgress;

  std::vector<double> m_CurrentAngles;
  std::vector<double> m_CurrentAngularVel;
  std::vector<double> m_CurrentAngularMaxVel;
  std::vector<double> m_CurrentAngularMaxAccel;

  // vector<unsigned long> startConf;

  std::string m_ErrorMessage;

  float maxAcc;

  pthread_mutex_t m_Angles_Mutex;
  pthread_mutex_t m_AngularVel_Mutex;
  pthread_mutex_t m_Movement_Mutex;
  pthread_t* m_SimThreadID;
  SimThreadArgs** m_SimThreadArgs;
};

#endif