PowerCubeSim.h

Go to the documentation of this file.

/****************************************************************
 *
 * Copyright (c) 2010
 *
 * Fraunhofer Institute for Manufacturing Engineering   
 * and Automation (IPA)
 *
 * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 *
 * Project name: care-o-bot
 * ROS stack name: cob_driver
 * ROS package name: cob_powercube_chain
 * Description: This class simulates the PowerCubes in a very rough and simple way.
 *                                                              
 * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 *                      
 * Author: Felix Geibel, email:Felix.Geibel@gmx.de
 * Supervised by: Alexander Bubeck, email:alexander.bubeck@ipa.fhg.de
 *
 * Date of creation: Aug 2007
 * ToDo:
 *
 * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Fraunhofer Institute for Manufacturing 
 *       Engineering and Automation (IPA) nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License LGPL as 
 * published by the Free Software Foundation, either version 3 of the 
 * License, or (at your option) any later version.
 * 
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License LGPL for more details.
 * 
 * You should have received a copy of the GNU Lesser General Public 
 * License LGPL along with this program. 
 * If not, see <http://www.gnu.org/licenses/>.
 *
 ****************************************************************/

#ifndef __POWER_CUBE_SIM_H_
#define __POWER_CUBE_SIM_H_

#include <cob_powercube_chain/PowerCubeCtrl.h>
#include <cob_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

---

cob_powercube_chain
Author(s): Florian Weisshardt
autogenerated on Fri Jan 11 09:14:20 2013