RemoveForceSensorLinkOffset.h
Go to the documentation of this file.
00001 // -*- C++ -*-
00010 #ifndef REMOVEFORCESENSORLINKOFFSET_H
00011 #define REMOVEFORCESENSORLINKOFFSET_H
00012 
00013 #include <rtm/idl/BasicDataType.hh>
00014 #include <rtm/idl/ExtendedDataTypes.hh>
00015 #include <rtm/Manager.h>
00016 #include <rtm/DataFlowComponentBase.h>
00017 #include <rtm/CorbaPort.h>
00018 #include <rtm/DataInPort.h>
00019 #include <rtm/DataOutPort.h>
00020 #include <rtm/idl/BasicDataTypeSkel.h>
00021 #include <rtm/idl/ExtendedDataTypesSkel.h>
00022 #include <hrpModel/Body.h>
00023 #include <hrpModel/Link.h>
00024 #include <hrpModel/JointPath.h>
00025 #include <hrpUtil/EigenTypes.h>
00026 
00027 #include "RemoveForceSensorLinkOffsetService_impl.h"
00028 #include "../ImpedanceController/RatsMatrix.h"
00029 #include <semaphore.h>
00030 
00031 // Service implementation headers
00032 // <rtc-template block="service_impl_h">
00033 
00034 // </rtc-template>
00035 
00036 // Service Consumer stub headers
00037 // <rtc-template block="consumer_stub_h">
00038 
00039 // </rtc-template>
00040 
00041 using namespace RTC;
00042 
00046 class RemoveForceSensorLinkOffset
00047   : public RTC::DataFlowComponentBase
00048 {
00049  public:
00054   RemoveForceSensorLinkOffset(RTC::Manager* manager);
00058   virtual ~RemoveForceSensorLinkOffset();
00059 
00060   // The initialize action (on CREATED->ALIVE transition)
00061   // formaer rtc_init_entry()
00062   virtual RTC::ReturnCode_t onInitialize();
00063 
00064   // The finalize action (on ALIVE->END transition)
00065   // formaer rtc_exiting_entry()
00066   // virtual RTC::ReturnCode_t onFinalize();
00067 
00068   // The startup action when ExecutionContext startup
00069   // former rtc_starting_entry()
00070   // virtual RTC::ReturnCode_t onStartup(RTC::UniqueId ec_id);
00071 
00072   // The shutdown action when ExecutionContext stop
00073   // former rtc_stopping_entry()
00074   // virtual RTC::ReturnCode_t onShutdown(RTC::UniqueId ec_id);
00075 
00076   // The activated action (Active state entry action)
00077   // former rtc_active_entry()
00078   virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id);
00079 
00080   // The deactivated action (Active state exit action)
00081   // former rtc_active_exit()
00082   virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id);
00083 
00084   // The execution action that is invoked periodically
00085   // former rtc_active_do()
00086   virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id);
00087 
00088   // The aborting action when main logic error occurred.
00089   // former rtc_aborting_entry()
00090   // virtual RTC::ReturnCode_t onAborting(RTC::UniqueId ec_id);
00091 
00092   // The error action in ERROR state
00093   // former rtc_error_do()
00094   // virtual RTC::ReturnCode_t onError(RTC::UniqueId ec_id);
00095 
00096   // The reset action that is invoked resetting
00097   // This is same but different the former rtc_init_entry()
00098   // virtual RTC::ReturnCode_t onReset(RTC::UniqueId ec_id);
00099 
00100   // The state update action that is invoked after onExecute() action
00101   // no corresponding operation exists in OpenRTm-aist-0.2.0
00102   // virtual RTC::ReturnCode_t onStateUpdate(RTC::UniqueId ec_id);
00103 
00104   // The action that is invoked when execution context's rate is changed
00105   // no corresponding operation exists in OpenRTm-aist-0.2.0
00106   // virtual RTC::ReturnCode_t onRateChanged(RTC::UniqueId ec_id);
00107   bool setForceMomentOffsetParam(const std::string& i_name_, const OpenHRP::RemoveForceSensorLinkOffsetService::forcemomentOffsetParam &i_param_);
00108   bool getForceMomentOffsetParam(const std::string& i_name_, OpenHRP::RemoveForceSensorLinkOffsetService::forcemomentOffsetParam& i_param_);
00109   bool loadForceMomentOffsetParams(const std::string& filename);
00110   bool dumpForceMomentOffsetParams(const std::string& filename);
00111   bool removeForceSensorOffset (const ::OpenHRP::RemoveForceSensorLinkOffsetService::StrSequence& names, const double tm);
00112 
00113  protected:
00114   // Configuration variable declaration
00115   // <rtc-template block="config_declare">
00116   
00117   // </rtc-template>
00118   // TimedDoubleSeq m_qRef;
00119   TimedDoubleSeq m_qCurrent;
00120   TimedOrientation3D m_rpy;
00121   
00122   // DataInPort declaration
00123   // <rtc-template block="inport_declare">
00124   InPort<TimedDoubleSeq> m_qCurrentIn;
00125   InPort<TimedOrientation3D> m_rpyIn;
00126 
00127   
00128   // </rtc-template>
00129 
00130   // DataOutPort declaration
00131   // <rtc-template block="outport_declare">
00132   std::vector<TimedDoubleSeq> m_force;
00133   std::vector<InPort<TimedDoubleSeq> *> m_forceIn;
00134   std::vector<OutPort<TimedDoubleSeq> *> m_forceOut;
00135   
00136   // </rtc-template>
00137 
00138   // DataOutPort declaration
00139   // <rtc-template block="outport_declare">
00140   
00141   // </rtc-template>
00142 
00143   // CORBA Port declaration
00144   // <rtc-template block="corbaport_declare">
00145   
00146   // </rtc-template>
00147 
00148   // Service declaration
00149   // <rtc-template block="service_declare">
00150   RTC::CorbaPort m_RemoveForceSensorLinkOffsetServicePort;
00151   
00152   // </rtc-template>
00153 
00154   // Consumer declaration
00155   // <rtc-template block="consumer_declare">
00156   RemoveForceSensorLinkOffsetService_impl m_service0;
00157   
00158   // </rtc-template>
00159 
00160  private:
00161   struct ForceMomentOffsetParam {
00162     hrp::Vector3 force_offset, moment_offset, link_offset_centroid;
00163     hrp::Vector3 off_force, off_moment;
00164     double link_offset_mass;
00165     // Members for sensor offset calib
00166     hrp::Vector3 force_offset_sum, moment_offset_sum;
00167     int sensor_offset_calib_counter;
00168     sem_t wait_sem;
00169 
00170     ForceMomentOffsetParam ()
00171       : force_offset(hrp::Vector3::Zero()), moment_offset(hrp::Vector3::Zero()),
00172         off_force(hrp::Vector3::Zero()), off_moment(hrp::Vector3::Zero()),
00173         link_offset_centroid(hrp::Vector3::Zero()), link_offset_mass(0),
00174         force_offset_sum(hrp::Vector3::Zero()), moment_offset_sum(hrp::Vector3::Zero()),
00175         sensor_offset_calib_counter(0), wait_sem()
00176     {
00177         sem_init(&wait_sem, 0, 0);
00178     };
00179   };
00180   void updateRootLinkPosRot (const hrp::Vector3& rpy);
00181   void printForceMomentOffsetParam(const std::string& i_name_);
00182 
00183   std::map<std::string, ForceMomentOffsetParam> m_forcemoment_offset_param;
00184   static const double grav = 9.80665; /* [m/s^2] */
00185   double m_dt;
00186   hrp::BodyPtr m_robot;
00187   unsigned int m_debugLevel;
00188   int max_sensor_offset_calib_counter;
00189   coil::Mutex m_mutex;
00190 };
00191 
00192 
00193 extern "C"
00194 {
00195   void RemoveForceSensorLinkOffsetInit(RTC::Manager* manager);
00196 };
00197 
00198 #endif // REMOVEFORCESENSORLINKOFFSET_H


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