AutoBalancer.h
Go to the documentation of this file.
1 // -*- C++ -*-
10 #ifndef AUTOBALANCER_H
11 #define AUTOBALANCER_H
12 
13 #include <rtm/idl/BasicDataType.hh>
14 #include <rtm/idl/ExtendedDataTypes.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 <rtm/idl/ExtendedDataTypesSkel.h>
22 #include <hrpModel/Body.h>
23 #include "../ImpedanceController/JointPathEx.h"
24 #include "../ImpedanceController/RatsMatrix.h"
25 #include "GaitGenerator.h"
26 // Service implementation headers
27 // <rtc-template block="service_impl_h">
29 #include "interpolator.h"
30 #include "../TorqueFilter/IIRFilter.h"
32 
33 // </rtc-template>
34 
35 // Service Consumer stub headers
36 // <rtc-template block="consumer_stub_h">
37 
38 // </rtc-template>
39 
40 using namespace RTC;
41 
44 {
45  public:
47  virtual ~AutoBalancer();
48 
49  // The initialize action (on CREATED->ALIVE transition)
50  // formaer rtc_init_entry()
51  virtual RTC::ReturnCode_t onInitialize();
52 
53  // The finalize action (on ALIVE->END transition)
54  // formaer rtc_exiting_entry()
55  virtual RTC::ReturnCode_t onFinalize();
56 
57  // The startup action when ExecutionContext startup
58  // former rtc_starting_entry()
59  // virtual RTC::ReturnCode_t onStartup(RTC::UniqueId ec_id);
60 
61  // The shutdown action when ExecutionContext stop
62  // former rtc_stopping_entry()
63  // virtual RTC::ReturnCode_t onShutdown(RTC::UniqueId ec_id);
64 
65  // The activated action (Active state entry action)
66  // former rtc_active_entry()
67  virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id);
68 
69  // The deactivated action (Active state exit action)
70  // former rtc_active_exit()
71  virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id);
72 
73  // The execution action that is invoked periodically
74  // former rtc_active_do()
75  virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id);
76 
77  // The aborting action when main logic error occurred.
78  // former rtc_aborting_entry()
79  // virtual RTC::ReturnCode_t onAborting(RTC::UniqueId ec_id);
80 
81  // The error action in ERROR state
82  // former rtc_error_do()
83  // virtual RTC::ReturnCode_t onError(RTC::UniqueId ec_id);
84 
85  // The reset action that is invoked resetting
86  // This is same but different the former rtc_init_entry()
87  // virtual RTC::ReturnCode_t onReset(RTC::UniqueId ec_id);
88 
89  // The state update action that is invoked after onExecute() action
90  // no corresponding operation exists in OpenRTm-aist-0.2.0
91  // virtual RTC::ReturnCode_t onStateUpdate(RTC::UniqueId ec_id);
92 
93  // The action that is invoked when execution context's rate is changed
94  // no corresponding operation exists in OpenRTm-aist-0.2.0
95  // virtual RTC::ReturnCode_t onRateChanged(RTC::UniqueId ec_id);
96  bool goPos(const double& x, const double& y, const double& th);
97  bool goVelocity(const double& vx, const double& vy, const double& vth);
98  bool goStop();
99  bool emergencyStop ();
100  bool setFootSteps(const OpenHRP::AutoBalancerService::FootstepSequence& fs, CORBA::Long overwrite_fs_idx);
101  bool setFootSteps(const OpenHRP::AutoBalancerService::FootstepsSequence& fss, CORBA::Long overwrite_fs_idx);
102  bool setFootStepsWithParam(const OpenHRP::AutoBalancerService::FootstepSequence& fs, const OpenHRP::AutoBalancerService::StepParamSequence& sps, CORBA::Long overwrite_fs_idx);
103  bool setFootStepsWithParam(const OpenHRP::AutoBalancerService::FootstepsSequence& fss, const OpenHRP::AutoBalancerService::StepParamsSequence& spss, CORBA::Long overwrite_fs_idx);
104  void waitFootSteps();
105  void waitFootStepsEarly(const double tm);
106  bool startAutoBalancer(const ::OpenHRP::AutoBalancerService::StrSequence& limbs);
107  bool stopAutoBalancer();
108  bool setGaitGeneratorParam(const OpenHRP::AutoBalancerService::GaitGeneratorParam& i_param);
109  bool getGaitGeneratorParam(OpenHRP::AutoBalancerService::GaitGeneratorParam& i_param);
110  bool setAutoBalancerParam(const OpenHRP::AutoBalancerService::AutoBalancerParam& i_param);
111  bool getAutoBalancerParam(OpenHRP::AutoBalancerService::AutoBalancerParam& i_param);
112  bool getFootstepParam(OpenHRP::AutoBalancerService::FootstepParam& i_param);
113  bool adjustFootSteps(const OpenHRP::AutoBalancerService::Footstep& rfootstep, const OpenHRP::AutoBalancerService::Footstep& lfootstep);
114  bool getRemainingFootstepSequence(OpenHRP::AutoBalancerService::FootstepSequence_out o_footstep, CORBA::Long& o_current_fs_idx);
115  bool getGoPosFootstepsSequence(const double& x, const double& y, const double& th, OpenHRP::AutoBalancerService::FootstepsSequence_out o_footstep);
116  bool releaseEmergencyStop();
117  void distributeReferenceZMPToWrenches (const hrp::Vector3& _ref_zmp);
118 
119  protected:
120  // Configuration variable declaration
121  // <rtc-template block="config_declare">
122 
123  // </rtc-template>
124 
125  // DataInPort declaration
126  // <rtc-template block="inport_declare">
127  TimedDoubleSeq m_qRef;
129  TimedPoint3D m_basePos;
131  TimedOrientation3D m_baseRpy;
133  TimedPoint3D m_zmp;
135  TimedDoubleSeq m_optionalData;
137  std::vector<TimedDoubleSeq> m_ref_force;
138  std::vector<InPort<TimedDoubleSeq> *> m_ref_forceIn;
139  TimedLong m_emergencySignal;
141  TimedPoint3D m_diffCP;
143  TimedBooleanSeq m_actContactStates;
149  // for debug
150  TimedPoint3D m_cog;
151 
152  // </rtc-template>
153 
154  // DataOutPort declaration
155  // <rtc-template block="outport_declare">
160  TimedDoubleSeq m_baseTform;
162  TimedPose3D m_basePose;
164  TimedAcceleration3D m_accRef;
166  TimedBooleanSeq m_contactStates;
168  TimedDoubleSeq m_toeheelRatio;
172  TimedBoolean m_walkingStates;
174  TimedPoint3D m_sbpCogOffset;
176  std::vector<TimedDoubleSeq> m_force;
177  std::vector<OutPort<TimedDoubleSeq> *> m_ref_forceOut;
178  std::vector<TimedPoint3D> m_limbCOPOffset;
179  std::vector<OutPort<TimedPoint3D> *> m_limbCOPOffsetOut;
180  // for debug
182 
183  // </rtc-template>
184 
185  // CORBA Port declaration
186  // <rtc-template block="corbaport_declare">
188 
189  // </rtc-template>
190 
191  // Service declaration
192  // <rtc-template block="service_declare">
194 
195  // </rtc-template>
196 
197  // Consumer declaration
198  // <rtc-template block="consumer_declare">
199 
200  // </rtc-template>
201 
202  private:
203  struct ABCIKparam {
204  hrp::Vector3 target_p0, localPos, adjust_interpolation_target_p0, adjust_interpolation_org_p0;
205  hrp::Matrix33 target_r0, localR, adjust_interpolation_target_r0, adjust_interpolation_org_r0;
207  bool is_active, has_toe_joint;
208  };
209  void getTargetParameters();
210  void solveFullbodyIK ();
211  void startABCparam(const ::OpenHRP::AutoBalancerService::StrSequence& limbs);
212  void stopABCparam();
213  void waitABCTransition();
214  // Functions to calculate parameters for ABC output.
215  // Output parameters are EE, limbCOPOffset, contactStates, controlSwingSupportTime, toeheelPhaseRatio
216  void getOutputParametersForWalking ();
217  void getOutputParametersForABC ();
218  void getOutputParametersForIDLE ();
219  void interpolateLegNamesAndZMPOffsets();
220  void calcFixCoordsForAdjustFootstep (rats::coordinates& tmp_fix_coords);
221  void rotateRefForcesForFixCoords (rats::coordinates& tmp_fix_coords);
222  void updateTargetCoordsForHandFixMode (rats::coordinates& tmp_fix_coords);
223  void calculateOutputRefForces ();
224  hrp::Vector3 calcFootMidPosUsingZMPWeightMap ();
225  void updateWalkingVelocityFromHandError (rats::coordinates& tmp_fix_coords);
226  void calcReferenceJointAnglesForIK ();
227  hrp::Matrix33 OrientRotationMatrix (const hrp::Matrix33& rot, const hrp::Vector3& axis1, const hrp::Vector3& axis2);
228  void fixLegToCoords (const hrp::Vector3& fix_pos, const hrp::Matrix33& fix_rot);
229  void fixLegToCoords2 (rats::coordinates& tmp_fix_coords);
230  bool startWalking ();
231  void stopWalking ();
232  void copyRatscoords2Footstep(OpenHRP::AutoBalancerService::Footstep& out_fs, const rats::coordinates& in_fs);
233  // static balance point offsetting
234  void static_balance_point_proc_one(hrp::Vector3& tmp_input_sbp, const double ref_com_height);
235  void calc_static_balance_point_from_forces(hrp::Vector3& sb_point, const hrp::Vector3& tmpcog, const double ref_com_height);
236  hrp::Vector3 calc_vel_from_hand_error (const rats::coordinates& tmp_fix_coords);
237  bool isOptionalDataContact (const std::string& ee_name)
238  {
239  return (std::fabs(m_optionalData.data[contact_states_index_map[ee_name]]-1.0)<0.1)?true:false;
240  };
241  bool calc_inital_support_legs(const double& y, std::vector<rats::coordinates>& initial_support_legs_coords, std::vector<rats::leg_type>& initial_support_legs, rats::coordinates& start_ref_coords);
242  std::string getUseForceModeString ();
243 
244  // for gg
246  ggPtr gg;
247  bool gg_is_walking, gg_solved;
248  // for abc
250  fikPtr fik;
251  hrp::Vector3 ref_cog, ref_zmp, prev_ref_zmp, prev_imu_sensor_pos, prev_imu_sensor_vel, hand_fix_initial_offset;
252  enum {BIPED, TROT, PACE, CRAWL, GALLOP} gait_type;
253  enum {MODE_IDLE, MODE_ABC, MODE_SYNC_TO_IDLE, MODE_SYNC_TO_ABC} control_mode;
254  std::map<std::string, ABCIKparam> ikp;
255  std::map<std::string, size_t> contact_states_index_map;
256  std::map<std::string, hrp::VirtualForceSensorParam> m_vfs;
257  std::vector<std::string> sensor_names, leg_names, ee_vec;
261  std::vector<hrp::Vector3> default_zmp_offsets;
262  double m_dt;
265  double d_pos_z_root, limb_stretch_avoidance_time_const, limb_stretch_avoidance_vlimit[2];
267 
268  double transition_interpolator_ratio, transition_time, zmp_transition_time, adjust_footstep_transition_time, leg_names_interpolator_ratio;
273  hrp::Vector3 input_zmp, input_basePos;
275 
276  // static balance point offsetting
277  hrp::Vector3 sbp_offset, sbp_cog_offset;
278  enum {MODE_NO_FORCE, MODE_REF_FORCE, MODE_REF_FORCE_WITH_FOOT, MODE_REF_FORCE_RFU_EXT_MOMENT} use_force;
279  std::vector<hrp::Vector3> ref_forces, ref_moments;
280 
281  unsigned int m_debugLevel;
282  bool is_legged_robot, is_stop_mode, is_hand_fix_mode, is_hand_fix_initial;
283  int loop;
285  std::string graspless_manip_arm;
288 
290  std::vector<IIRFilter> invdyn_zmp_filters;
291 
292  // Used for ref force balancing.
295 };
296 
297 
298 extern "C"
299 {
301 };
302 
303 #endif // IMPEDANCE_H
TimedOrientation3D m_baseRpy
Definition: AutoBalancer.h:131
TimedBooleanSeq m_contactStates
Definition: AutoBalancer.h:166
TimedDoubleSeq m_toeheelRatio
Definition: AutoBalancer.h:168
ec_id
InPort< TimedBoolean > m_refFootOriginExtMomentIsHoldValueIn
Definition: AutoBalancer.h:148
bool graspless_manip_mode
Definition: AutoBalancer.h:284
TimedPoint3D m_diffCP
Definition: AutoBalancer.h:141
coil::Mutex m_mutex
Definition: AutoBalancer.h:264
hrp::Vector3 sbp_offset
Definition: AutoBalancer.h:277
rats::coordinates fix_leg_coords2
Definition: AutoBalancer.h:260
std::vector< InPort< TimedDoubleSeq > * > m_ref_forceIn
Definition: AutoBalancer.h:138
TimedPoint3D m_zmp
Definition: AutoBalancer.h:133
std::map< std::string, ABCIKparam > ikp
Definition: AutoBalancer.h:254
InPort< TimedPoint3D > m_basePosIn
Definition: AutoBalancer.h:130
void AutoBalancerInit(RTC::Manager *manager)
OutPort< TimedPoint3D > m_sbpCogOffsetOut
Definition: AutoBalancer.h:175
InPort< TimedDoubleSeq > m_optionalDataIn
Definition: AutoBalancer.h:136
OutPort< TimedDoubleSeq > m_controlSwingSupportTimeOut
Definition: AutoBalancer.h:171
std::vector< TimedPoint3D > m_limbCOPOffset
Definition: AutoBalancer.h:178
interpolator * adjust_footstep_interpolator
Definition: AutoBalancer.h:271
TimedDoubleSeq m_qRef
Definition: AutoBalancer.h:127
manager
std::vector< hrp::Vector3 > default_zmp_offsets
Definition: AutoBalancer.h:261
OutPort< TimedDoubleSeq > m_toeheelRatioOut
Definition: AutoBalancer.h:169
boost::shared_ptr< SimpleFullbodyInverseKinematicsSolver > fikPtr
Definition: AutoBalancer.h:249
boost::shared_ptr< rats::gait_generator > ggPtr
Definition: AutoBalancer.h:245
hrp::Link * additional_force_applied_link
Definition: AutoBalancer.h:293
double zmp_transition_time
Definition: AutoBalancer.h:268
bool use_limb_stretch_avoidance
Definition: AutoBalancer.h:266
OutPort< TimedBooleanSeq > m_contactStatesOut
Definition: AutoBalancer.h:167
Eigen::Vector3d Vector3
OutPort< TimedDoubleSeq > m_baseTformOut
Definition: AutoBalancer.h:161
Eigen::Matrix3d Matrix33
TimedLong m_emergencySignal
Definition: AutoBalancer.h:139
std::map< std::string, size_t > contact_states_index_map
Definition: AutoBalancer.h:255
TimedBooleanSeq m_actContactStates
Definition: AutoBalancer.h:143
OutPort< TimedPoint3D > m_cogOut
Definition: AutoBalancer.h:181
std::vector< std::string > sensor_names
Definition: AutoBalancer.h:257
unsigned int m_debugLevel
Definition: AutoBalancer.h:281
ExecutionContextHandle_t UniqueId
std::string graspless_manip_arm
Definition: AutoBalancer.h:285
interpolator * transition_interpolator
Definition: AutoBalancer.h:270
std::vector< hrp::Vector3 > ref_moments
Definition: AutoBalancer.h:279
std::map< std::string, hrp::VirtualForceSensorParam > m_vfs
Definition: AutoBalancer.h:256
interpolator * leg_names_interpolator
Definition: AutoBalancer.h:272
InPort< TimedLong > m_emergencySignalIn
Definition: AutoBalancer.h:140
hrp::Vector3 input_zmp
Definition: AutoBalancer.h:273
TimedPoint3D m_refFootOriginExtMoment
Definition: AutoBalancer.h:145
hrp::Vector3 ref_zmp
Definition: AutoBalancer.h:251
RTC::OutPort< RTC::TimedPoint3D > m_zmpOut
Definition: AutoBalancer.h:157
std::vector< OutPort< TimedDoubleSeq > * > m_ref_forceOut
Definition: AutoBalancer.h:177
InPort< TimedPoint3D > m_zmpIn
Definition: AutoBalancer.h:134
std::vector< TimedDoubleSeq > m_ref_force
Definition: AutoBalancer.h:137
TimedPoint3D m_sbpCogOffset
Definition: AutoBalancer.h:174
OutPort< TimedPoint3D > m_basePosOut
Definition: AutoBalancer.h:158
AutoBalancerService_impl m_service0
Definition: AutoBalancer.h:193
TimedAcceleration3D m_accRef
Definition: AutoBalancer.h:164
OutPort< TimedAcceleration3D > m_accRefOut
Definition: AutoBalancer.h:165
TimedDoubleSeq m_optionalData
Definition: AutoBalancer.h:135
TimedBoolean m_walkingStates
Definition: AutoBalancer.h:172
OutPort< TimedDoubleSeq > m_qOut
Definition: AutoBalancer.h:156
hrp::Vector3 graspless_manip_p_gain
Definition: AutoBalancer.h:286
TimedPoint3D m_cog
Definition: AutoBalancer.h:150
OutPort< TimedPose3D > m_basePoseOut
Definition: AutoBalancer.h:163
InPort< TimedBooleanSeq > m_actContactStatesIn
Definition: AutoBalancer.h:144
std::vector< OutPort< TimedPoint3D > * > m_limbCOPOffsetOut
Definition: AutoBalancer.h:179
hrp::InvDynStateBuffer idsb
Definition: AutoBalancer.h:289
InPort< TimedDoubleSeq > m_qRefIn
Definition: AutoBalancer.h:128
TimedDoubleSeq m_baseTform
Definition: AutoBalancer.h:160
InPort< TimedPoint3D > m_refFootOriginExtMomentIn
Definition: AutoBalancer.h:146
InPort< TimedOrientation3D > m_baseRpyIn
Definition: AutoBalancer.h:132
hrp::BodyPtr m_robot
Definition: AutoBalancer.h:263
std::vector< IIRFilter > invdyn_zmp_filters
Definition: AutoBalancer.h:290
TimedDoubleSeq m_controlSwingSupportTime
Definition: AutoBalancer.h:170
RTC::CorbaPort m_AutoBalancerServicePort
Definition: AutoBalancer.h:187
hrp::Vector3 additional_force_applied_point_offset
Definition: AutoBalancer.h:294
InPort< TimedPoint3D > m_diffCPIn
Definition: AutoBalancer.h:142
TimedPoint3D m_basePos
Definition: AutoBalancer.h:129
TimedPose3D m_basePose
Definition: AutoBalancer.h:162
OutPort< TimedOrientation3D > m_baseRpyOut
Definition: AutoBalancer.h:159
hrp::Matrix33 target_root_R
Definition: AutoBalancer.h:259
std::vector< TimedDoubleSeq > m_force
Definition: AutoBalancer.h:176
interpolator * zmp_offset_interpolator
Definition: AutoBalancer.h:269
OutPort< TimedBoolean > m_walkingStatesOut
Definition: AutoBalancer.h:173
hrp::Matrix33 input_baseRot
Definition: AutoBalancer.h:274
rats::coordinates graspless_manip_reference_trans_coords
Definition: AutoBalancer.h:287
bool isOptionalDataContact(const std::string &ee_name)
Definition: AutoBalancer.h:237
hrp::Vector3 target_root_p
Definition: AutoBalancer.h:258
TimedBoolean m_refFootOriginExtMomentIsHoldValue
Definition: AutoBalancer.h:147


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Sat Dec 17 2022 03:52:20