SequencePlayer.h
Go to the documentation of this file.
00001 // -*- C++ -*-
00010 #ifndef SEQUENCEPLAYER_H
00011 #define SEQUENCEPLAYER_H
00012 
00013 #include <rtm/idl/BasicDataType.hh>
00014 #include <rtm/idl/ExtendedDataTypes.hh>
00015 #include <semaphore.h>
00016 #include <rtm/Manager.h>
00017 #include <rtm/DataFlowComponentBase.h>
00018 #include <rtm/CorbaPort.h>
00019 #include <rtm/DataInPort.h>
00020 #include <rtm/DataOutPort.h>
00021 #include <rtm/idl/BasicDataTypeSkel.h>
00022 #include <rtm/idl/ExtendedDataTypesSkel.h>
00023 #include <hrpModel/Body.h>
00024 #include <hrpModel/Sensor.h>
00025 #include "seqplay.h"
00026 
00027 // Service implementation headers
00028 // <rtc-template block="service_impl_h">
00029 #include "SequencePlayerService_impl.h"
00030 
00031 // </rtc-template>
00032 
00033 // Service Consumer stub headers
00034 // <rtc-template block="consumer_stub_h">
00035 
00036 // </rtc-template>
00037 
00038 using namespace RTC;
00039 
00040 class SequencePlayer
00041   : public RTC::DataFlowComponentBase
00042 {
00043  public:
00044   SequencePlayer(RTC::Manager* manager);
00045   virtual ~SequencePlayer();
00046 
00047   // The initialize action (on CREATED->ALIVE transition)
00048   // formaer rtc_init_entry()
00049  virtual RTC::ReturnCode_t onInitialize();
00050 
00051   // The finalize action (on ALIVE->END transition)
00052   // formaer rtc_exiting_entry()
00053   virtual RTC::ReturnCode_t onFinalize();
00054 
00055   // The startup action when ExecutionContext startup
00056   // former rtc_starting_entry()
00057   // virtual RTC::ReturnCode_t onStartup(RTC::UniqueId ec_id);
00058 
00059   // The shutdown action when ExecutionContext stop
00060   // former rtc_stopping_entry()
00061   // virtual RTC::ReturnCode_t onShutdown(RTC::UniqueId ec_id);
00062 
00063   // The activated action (Active state entry action)
00064   // former rtc_active_entry()
00065   virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id);
00066 
00067   // The deactivated action (Active state exit action)
00068   // former rtc_active_exit()
00069   // virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id);
00070 
00071   // The execution action that is invoked periodically
00072   // former rtc_active_do()
00073   virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id);
00074 
00075   // The aborting action when main logic error occurred.
00076   // former rtc_aborting_entry()
00077   // virtual RTC::ReturnCode_t onAborting(RTC::UniqueId ec_id);
00078 
00079   // The error action in ERROR state
00080   // former rtc_error_do()
00081   // virtual RTC::ReturnCode_t onError(RTC::UniqueId ec_id);
00082 
00083   // The reset action that is invoked resetting
00084   // This is same but different the former rtc_init_entry()
00085   // virtual RTC::ReturnCode_t onReset(RTC::UniqueId ec_id);
00086 
00087   // The state update action that is invoked after onExecute() action
00088   // no corresponding operation exists in OpenRTm-aist-0.2.0
00089   // virtual RTC::ReturnCode_t onStateUpdate(RTC::UniqueId ec_id);
00090 
00091   // The action that is invoked when execution context's rate is changed
00092   // no corresponding operation exists in OpenRTm-aist-0.2.0
00093   // virtual RTC::ReturnCode_t onRateChanged(RTC::UniqueId ec_id);
00094 
00095   double dt;
00096   seqplay *player() { return m_seq; }
00097   hrp::BodyPtr robot() { return m_robot;}  
00098   void setClearFlag();
00099   void waitInterpolation();
00100   bool waitInterpolationOfGroup(const char *gname);
00101   bool setJointAngle(short id, double angle, double tm);
00102   bool setJointAngles(const double *angles, double tm);
00103   bool setJointAngles(const double *angles, const bool *mask, double tm);
00104   bool setJointAnglesSequence(const OpenHRP::dSequenceSequence angless, const OpenHRP::bSequence& mask, const OpenHRP::dSequence& times);
00105   bool setJointAnglesSequenceFull(const OpenHRP::dSequenceSequence i_jvss, const OpenHRP::dSequenceSequence i_vels, const OpenHRP::dSequenceSequence i_torques, const OpenHRP::dSequenceSequence i_poss, const OpenHRP::dSequenceSequence i_rpys, const OpenHRP::dSequenceSequence i_accs, const OpenHRP::dSequenceSequence i_zmps, const OpenHRP::dSequenceSequence i_wrenches, const OpenHRP::dSequenceSequence i_optionals, const dSequence i_tms);
00106   bool clearJointAngles();
00107   bool setBasePos(const double *pos, double tm);
00108   bool setBaseRpy(const double *rpy, double tm);
00109   bool setZmp(const double *zmp, double tm);
00110   bool setTargetPose(const char* gname, const double *xyz, const double *rpy, double tm, const char* frame_name);
00111   bool setWrenches(const double *wrenches, double tm);
00112   void loadPattern(const char *basename, double time); 
00113   void playPattern(const OpenHRP::dSequenceSequence& pos, const OpenHRP::dSequenceSequence& rpy, const OpenHRP::dSequenceSequence& zmp, const OpenHRP::dSequence& tm);
00114   bool setInterpolationMode(OpenHRP::SequencePlayerService::interpolationMode i_mode_);
00115   bool setInitialState(double tm=0.0);
00116   bool addJointGroup(const char *gname, const OpenHRP::SequencePlayerService::StrSequence& jnames);
00117   bool removeJointGroup(const char *gname);
00118   bool setJointAnglesOfGroup(const char *gname, const OpenHRP::dSequence& jvs, double tm);
00119   bool setJointAnglesSequenceOfGroup(const char *gname, const OpenHRP::dSequenceSequence angless, const OpenHRP::dSequence& times);
00120     bool clearJointAnglesOfGroup(const char *gname);
00121   bool playPatternOfGroup(const char *gname, const OpenHRP::dSequenceSequence& pos, const OpenHRP::dSequence& tm);
00122 
00123   void setMaxIKError(double pos, double rot);
00124   void setMaxIKIteration(short iter);
00125  protected:
00126   // Configuration variable declaration
00127   // <rtc-template block="config_declare">
00128   
00129   // </rtc-template>
00130 
00131   // DataInPort declaration
00132   // <rtc-template block="inport_declare">
00133   TimedDoubleSeq m_qInit;
00134   InPort<TimedDoubleSeq> m_qInitIn;
00135   TimedPoint3D m_basePosInit;
00136   InPort<TimedPoint3D> m_basePosInitIn;
00137   TimedOrientation3D m_baseRpyInit;
00138   InPort<TimedOrientation3D> m_baseRpyInitIn;
00139   TimedPoint3D m_zmpRefInit;
00140   InPort<TimedPoint3D> m_zmpRefInitIn;
00141   
00142   // </rtc-template>
00143 
00144   // DataOutPort declaration
00145   // <rtc-template block="outport_declare">
00146   TimedDoubleSeq m_qRef;
00147   OutPort<TimedDoubleSeq> m_qRefOut;
00148   TimedDoubleSeq m_tqRef;
00149   OutPort<TimedDoubleSeq> m_tqRefOut;
00150   TimedPoint3D m_zmpRef;
00151   OutPort<TimedPoint3D> m_zmpRefOut;
00152   TimedAcceleration3D m_accRef;
00153   OutPort<TimedAcceleration3D> m_accRefOut;
00154   TimedPoint3D m_basePos;
00155   OutPort<TimedPoint3D> m_basePosOut;
00156   TimedOrientation3D m_baseRpy;
00157   OutPort<TimedOrientation3D> m_baseRpyOut;
00158   std::vector<TimedDoubleSeq> m_wrenches;
00159   std::vector<OutPort<TimedDoubleSeq> *> m_wrenchesOut;
00160   TimedDoubleSeq m_optionalData;
00161   OutPort<TimedDoubleSeq> m_optionalDataOut;
00162 
00163   
00164   // </rtc-template>
00165 
00166   // CORBA Port declaration
00167   // <rtc-template block="corbaport_declare">
00168   RTC::CorbaPort m_SequencePlayerServicePort;
00169 
00170   // </rtc-template>
00171 
00172   // Service declaration
00173   // <rtc-template block="service_declare">
00174   SequencePlayerService_impl m_service0;
00175 
00176   // </rtc-template>
00177 
00178   // Consumer declaration
00179   // <rtc-template block="consumer_declare">
00180   
00181   // </rtc-template>
00182 
00183  private:
00184   seqplay *m_seq;
00185   bool m_clearFlag, m_waitFlag;
00186   sem_t m_waitSem;
00187   hrp::BodyPtr m_robot;
00188   std::string m_gname;
00189   unsigned int m_debugLevel;
00190   size_t optional_data_dim;
00191   coil::Mutex m_mutex;
00192   double m_error_pos, m_error_rot;
00193   short m_iteration;
00194   std::string m_fixedLink;
00195   hrp::Vector3 m_offsetP, m_fixedP;
00196   hrp::Matrix33 m_offsetR, m_fixedR;
00197   double m_timeToStartPlaying;
00198   int dummy;
00199 };
00200 
00201 
00202 extern "C"
00203 {
00204   void SequencePlayerInit(RTC::Manager* manager);
00205 };
00206 
00207 #endif // SEQUENCEPLAYER_H


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Wed Sep 6 2017 02:35:56