CollisionDetector.h
Go to the documentation of this file.
1 // -*- C++ -*-
10 #ifndef COLLISION_DETECTOR_H
11 #define COLLISION_DETECTOR_H
12 
13 #include <rtm/idl/BasicDataType.hh>
14 #include "hrpsys/idl/HRPDataTypes.hh"
15 #include <rtm/Manager.h>
16 #include <rtm/DataFlowComponentBase.h>
17 #include <rtm/CorbaPort.h>
18 #include <rtm/DataInPort.h>
19 #include <rtm/DataOutPort.h>
20 #include <rtm/idl/BasicDataTypeSkel.h>
21 #include <hrpModel/Body.h>
23 #include <hrpModel/ModelLoaderUtil.h>
24 #ifdef USE_HRPSYSUTIL
25 #include "GLscene.h"
26 #include "hrpsys/util/SDLUtil.h"
27 #include "hrpsys/util/LogManager.h"
28 #endif // USE_HRPSYSUTIL
29 #include "TimedPosture.h"
30 #include "interpolator.h"
31 
32 #include "VclipLinkPair.h"
34 #include "../SoftErrorLimiter/beep.h"
35 
36 // Service implementation headers
37 // <rtc-template block="service_impl_h">
38 
39 // </rtc-template>
40 
41 // Service Consumer stub headers
42 // <rtc-template block="consumer_stub_h">
43 
44 // </rtc-template>
45 
46 using namespace RTC;
47 
53 {
54  public:
63  virtual ~CollisionDetector();
64 
65  // The initialize action (on CREATED->ALIVE transition)
66  // formaer rtc_init_entry()
67  virtual RTC::ReturnCode_t onInitialize();
68 
69  // The finalize action (on ALIVE->END transition)
70  // formaer rtc_exiting_entry()
71  virtual RTC::ReturnCode_t onFinalize();
72 
73  // The startup action when ExecutionContext startup
74  // former rtc_starting_entry()
75  // virtual RTC::ReturnCode_t onStartup(RTC::UniqueId ec_id);
76 
77  // The shutdown action when ExecutionContext stop
78  // former rtc_stopping_entry()
79  // virtual RTC::ReturnCode_t onShutdown(RTC::UniqueId ec_id);
80 
81  // The activated action (Active state entry action)
82  // former rtc_active_entry()
83  virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id);
84 
85  // The deactivated action (Active state exit action)
86  // former rtc_active_exit()
87  virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id);
88 
89  // The execution action that is invoked periodically
90  // former rtc_active_do()
91  virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id);
92 
93  // The aborting action when main logic error occurred.
94  // former rtc_aborting_entry()
95  // virtual RTC::ReturnCode_t onAborting(RTC::UniqueId ec_id);
96 
97  // The error action in ERROR state
98  // former rtc_error_do()
99  // virtual RTC::ReturnCode_t onError(RTC::UniqueId ec_id);
100 
101  // The reset action that is invoked resetting
102  // This is same but different the former rtc_init_entry()
103  // virtual RTC::ReturnCode_t onReset(RTC::UniqueId ec_id);
104 
105  // The state update action that is invoked after onExecute() action
106  // no corresponding operation exists in OpenRTm-aist-0.2.0
107  // virtual RTC::ReturnCode_t onStateUpdate(RTC::UniqueId ec_id);
108 
109  // The action that is invoked when execution context's rate is changed
110  // no corresponding operation exists in OpenRTm-aist-0.2.0
111  // virtual RTC::ReturnCode_t onRateChanged(RTC::UniqueId ec_id);
112 
113  bool setTolerance(const char *i_link_pair_name, double i_tolerance);
114  bool setCollisionLoop(int input_loop);
115  bool getCollisionStatus(OpenHRP::CollisionDetectorService::CollisionState &state);
116 
117  bool checkIsSafeTransition(void);
118  bool enable(void);
119  bool disable(void);
120 
121  protected:
122  // Configuration variable declaration
123  // <rtc-template block="config_declare">
124 
125  // </rtc-template>
126 
127  // DataInPort declaration
128  // <rtc-template block="inport_declare">
129  TimedDoubleSeq m_qRef;
131  TimedDoubleSeq m_qCurrent;
133  OpenHRP::TimedLongSeqSeq m_servoState;
135 
136  // </rtc-template>
137 
138  // DataOutPort declaration
139  // <rtc-template block="outport_declare">
140  TimedDoubleSeq m_q;
142  TimedLongSeq m_beepCommand;
144 
145  // </rtc-template>
146 
147  // CORBA Port declaration
148  // <rtc-template block="corbaport_declare">
150 
151  // </rtc-template>
152 
153  // Service declaration
154  // <rtc-template block="service_declare">
156 
157  // </rtc-template>
158 
159  // Consumer declaration
160  // <rtc-template block="consumer_declare">
161 
162 
163  // </rtc-template>
164  void setupVClipModel(hrp::BodyPtr i_body);
165  void setupVClipModel(hrp::Link *i_link);
166 
167  private:
169  public:
170  CollisionLinkPair(VclipLinkPairPtr i_pair) : point0(hrp::Vector3(0,0,0)), point1(hrp::Vector3(0,0,0)), distance(0) {
171  pair = i_pair;
172  }
175  double distance;
176  };
177 #ifdef USE_HRPSYSUTIL
180  SDLwindow m_window;
181  GLbody *m_glbody;
182 #endif // USE_HRPSYSUTIL
183  std::vector<Vclip::Polyhedron *> m_VclipLinks;
184  std::vector<int> m_curr_collision_mask, m_init_collision_mask;
188  std::map<std::string, CollisionLinkPair *> m_pair;
189  int m_loop_for_check, m_collision_loop;
192  double m_dt;
193  //
194  double *m_recover_jointdata, *m_lastsafe_jointdata;
197  double i_dt;
199  unsigned int m_debugLevel;
200  bool m_enable;
201  int collision_beep_freq, collision_beep_count;
203  OpenHRP::CollisionDetectorService::CollisionState m_state;
205  // Since this RTC is stable RTC, we support both direct beeping from this RTC and beepring through BeeperRTC.
206  // If m_beepCommand is connected to BeeperRTC, is_beep_port_connected is true.
208  int dummy;
209 };
210 
211 #ifndef USE_HRPSYSUTIL
213 #endif // USE_HRPSYSUTIL
214 
215 extern "C"
216 {
217  void CollisionDetectorInit(RTC::Manager* manager);
218 };
219 
220 #endif // COLLISION_DETECTOR_H
ec_id
RTC::CorbaPort m_CollisionDetectorServicePort
boost::intrusive_ptr< VclipLinkPair > VclipLinkPairPtr
Definition: VclipLinkPair.h:21
InPort< TimedDoubleSeq > m_qCurrentIn
state
unsigned int m_debugLevel
void CollisionDetectorInit(RTC::Manager *manager)
std::vector< int > m_init_collision_mask
InPort< TimedDoubleSeq > m_qRefIn
hrp::Link * hrplinkFactory()
manager
CollisionLinkPair(VclipLinkPairPtr i_pair)
OutPort< TimedLongSeq > m_beepCommandOut
interpolator * m_interpolator
Definition: GLbody.h:11
Eigen::Vector3d Vector3
TimedDoubleSeq m_q
OpenHRP::vector3 Vector3
OutPort< TimedDoubleSeq > m_qOut
TimedDoubleSeq m_qRef
ExecutionContextHandle_t UniqueId
std::map< std::string, CollisionLinkPair * > m_pair
std::vector< Vclip::Polyhedron * > m_VclipLinks
TimedDoubleSeq m_qCurrent
OpenHRP::CollisionDetectorService::CollisionState m_state
OpenHRP::TimedLongSeqSeq m_servoState
TimedLongSeq m_beepCommand
sample RT component which has one data input port and one data output port
InPort< OpenHRP::TimedLongSeqSeq > m_servoStateIn
CollisionDetectorService_impl m_service0


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Thu May 6 2021 02:41:49