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