diff --git a/doxygen/FootManager_8h_source.html b/doxygen/FootManager_8h_source.html index 1876535..73c637e 100644 --- a/doxygen/FootManager_8h_source.html +++ b/doxygen/FootManager_8h_source.html @@ -217,149 +217,151 @@
213 
218  bool appendFootstep(const Footstep & newFootstep);
219 
-
224  Eigen::Vector3d calcRefZmp(double t, int derivOrder = 0) const;
-
225 
-
230  Eigen::Vector3d clampDeltaTrans(const Eigen::Vector3d & deltaTrans, const Foot & foot);
-
231 
-
236  double calcRefGroundPosZ(double t, int derivOrder = 0) const;
-
237 
-
245  std::unordered_map<Foot, sva::PTransformd> calcContactFootPoses(double t) const;
-
246 
-
251  std::set<Foot> getCurrentContactFeet() const;
-
252 
-
259  std::unordered_map<Foot, std::shared_ptr<ForceColl::Contact>> calcCurrentContactList() const;
-
260 
-
265  double leftFootSupportRatio() const;
-
266 
-
271  Eigen::Vector3d calcZmpWithOffset(const Foot & foot, const sva::PTransformd & footPose) const;
-
272 
-
278  Eigen::Vector3d calcZmpWithOffset(const std::unordered_map<Foot, sva::PTransformd> & footPoses) const;
-
279 
-
281  inline const std::deque<Footstep> & footstepQueue() const noexcept
-
282  {
-
283  return footstepQueue_;
-
284  }
-
285 
-
287  std::shared_ptr<Footstep> prevFootstep() const
-
288  {
-
289  return prevFootstep_;
-
290  }
-
291 
-
293  inline const sva::PTransformd & targetFootPose(const Foot & foot) const
-
294  {
-
295  return targetFootPoses_.at(foot);
-
296  }
-
297 
-
299  inline const sva::MotionVecd & targetFootVel(const Foot & foot) const
-
300  {
-
301  return targetFootVels_.at(foot);
-
302  }
-
303 
-
305  inline const sva::MotionVecd & targetFootAccel(const Foot & foot) const
-
306  {
-
307  return targetFootAccels_.at(foot);
-
308  }
-
309 
-
311  inline SupportPhase supportPhase() const noexcept
-
312  {
-
313  return supportPhase_;
-
314  }
-
315 
-
322  bool walkToRelativePose(const Eigen::Vector3d & targetTrans,
-
323  int lastFootstepNum = 0,
-
324  const std::vector<Eigen::Vector3d> & waypointTransList = {});
-
325 
-
329  bool startVelMode();
-
330 
-
334  bool endVelMode();
-
335 
-
339  inline void setRelativeVel(const Eigen::Vector3d & targetVel)
-
340  {
-
341  velModeData_.targetVel_ = targetVel;
-
342  }
-
343 
-
345  inline bool velModeEnabled() const
-
346  {
-
347  return velModeData_.enabled_;
-
348  }
-
349 
-
350 protected:
-
352  inline const BaselineWalkingController & ctl() const
-
353  {
-
354  return *ctlPtr_;
-
355  }
-
356 
-
358  inline BaselineWalkingController & ctl()
-
359  {
-
360  return *ctlPtr_;
-
361  }
-
362 
-
364  virtual void updateFootTraj();
+
221  void clearFootstepQueue();
+
222 
+
227  Eigen::Vector3d calcRefZmp(double t, int derivOrder = 0) const;
+
228 
+
233  Eigen::Vector3d clampDeltaTrans(const Eigen::Vector3d & deltaTrans, const Foot & foot);
+
234 
+
239  double calcRefGroundPosZ(double t, int derivOrder = 0) const;
+
240 
+
248  std::unordered_map<Foot, sva::PTransformd> calcContactFootPoses(double t) const;
+
249 
+
254  std::set<Foot> getCurrentContactFeet() const;
+
255 
+
262  std::unordered_map<Foot, std::shared_ptr<ForceColl::Contact>> calcCurrentContactList() const;
+
263 
+
268  double leftFootSupportRatio() const;
+
269 
+
274  Eigen::Vector3d calcZmpWithOffset(const Foot & foot, const sva::PTransformd & footPose) const;
+
275 
+
281  Eigen::Vector3d calcZmpWithOffset(const std::unordered_map<Foot, sva::PTransformd> & footPoses) const;
+
282 
+
284  inline const std::deque<Footstep> & footstepQueue() const noexcept
+
285  {
+
286  return footstepQueue_;
+
287  }
+
288 
+
290  std::shared_ptr<Footstep> prevFootstep() const
+
291  {
+
292  return prevFootstep_;
+
293  }
+
294 
+
296  inline const sva::PTransformd & targetFootPose(const Foot & foot) const
+
297  {
+
298  return targetFootPoses_.at(foot);
+
299  }
+
300 
+
302  inline const sva::MotionVecd & targetFootVel(const Foot & foot) const
+
303  {
+
304  return targetFootVels_.at(foot);
+
305  }
+
306 
+
308  inline const sva::MotionVecd & targetFootAccel(const Foot & foot) const
+
309  {
+
310  return targetFootAccels_.at(foot);
+
311  }
+
312 
+
314  inline SupportPhase supportPhase() const noexcept
+
315  {
+
316  return supportPhase_;
+
317  }
+
318 
+
325  bool walkToRelativePose(const Eigen::Vector3d & targetTrans,
+
326  int lastFootstepNum = 0,
+
327  const std::vector<Eigen::Vector3d> & waypointTransList = {});
+
328 
+
332  bool startVelMode();
+
333 
+
337  bool endVelMode();
+
338 
+
342  inline void setRelativeVel(const Eigen::Vector3d & targetVel)
+
343  {
+
344  velModeData_.targetVel_ = targetVel;
+
345  }
+
346 
+
348  inline bool velModeEnabled() const
+
349  {
+
350  return velModeData_.enabled_;
+
351  }
+
352 
+
353 protected:
+
355  inline const BaselineWalkingController & ctl() const
+
356  {
+
357  return *ctlPtr_;
+
358  }
+
359 
+
361  inline BaselineWalkingController & ctl()
+
362  {
+
363  return *ctlPtr_;
+
364  }
365 
-
367  virtual void updateZmpTraj();
+
367  virtual void updateFootTraj();
368 
-
370  void updateVelMode();
+
370  virtual void updateZmpTraj();
371 
-
375  double touchDownRemainingDuration() const;
-
376 
-
380  bool detectTouchDown() const;
-
381 
-
382 protected:
-
384  Configuration config_;
-
385 
-
387  VelModeData velModeData_;
+
373  void updateVelMode();
+
374 
+
378  double touchDownRemainingDuration() const;
+
379 
+
383  bool detectTouchDown() const;
+
384 
+
385 protected:
+
387  Configuration config_;
388 
-
390  BaselineWalkingController * ctlPtr_ = nullptr;
+
390  VelModeData velModeData_;
391 
-
393  std::deque<Footstep> footstepQueue_;
+
393  BaselineWalkingController * ctlPtr_ = nullptr;
394 
-
396  std::shared_ptr<Footstep> prevFootstep_;
+
396  std::deque<Footstep> footstepQueue_;
397 
-
399  std::unordered_map<Foot, sva::PTransformd> targetFootPoses_;
+
399  std::shared_ptr<Footstep> prevFootstep_;
400 
-
402  std::unordered_map<Foot, sva::MotionVecd> targetFootVels_;
+
402  std::unordered_map<Foot, sva::PTransformd> targetFootPoses_;
403 
-
405  std::unordered_map<Foot, sva::MotionVecd> targetFootAccels_;
+
405  std::unordered_map<Foot, sva::MotionVecd> targetFootVels_;
406 
-
408  std::unordered_map<Foot, TaskGain> footTaskGains_;
+
408  std::unordered_map<Foot, sva::MotionVecd> targetFootAccels_;
409 
-
411  std::unordered_map<Foot, sva::PTransformd> trajStartFootPoses_;
+
411  std::unordered_map<Foot, TaskGain> footTaskGains_;
412 
-
414  std::unordered_map<Foot, std::shared_ptr<TrajColl::CubicInterpolator<sva::PTransformd, sva::MotionVecd>>>
-
415  trajStartFootPoseFuncs_;
-
416 
-
418  SupportPhase supportPhase_ = SupportPhase::DoubleSupport;
+
414  std::unordered_map<Foot, sva::PTransformd> trajStartFootPoses_;
+
415 
+
417  std::unordered_map<Foot, std::shared_ptr<TrajColl::CubicInterpolator<sva::PTransformd, sva::MotionVecd>>>
+
418  trajStartFootPoseFuncs_;
419 
-
421  std::shared_ptr<TrajColl::CubicInterpolator<Eigen::Vector3d>> zmpFunc_;
+
421  SupportPhase supportPhase_ = SupportPhase::DoubleSupport;
422 
-
424  std::shared_ptr<TrajColl::CubicInterpolator<double>> groundPosZFunc_;
+
424  std::shared_ptr<TrajColl::CubicInterpolator<Eigen::Vector3d>> zmpFunc_;
425 
-
427  std::map<double, std::unordered_map<Foot, sva::PTransformd>> contactFootPosesList_;
+
427  std::shared_ptr<TrajColl::CubicInterpolator<double>> groundPosZFunc_;
428 
-
430  Footstep * swingFootstep_ = nullptr;
+
430  std::map<double, std::unordered_map<Foot, sva::PTransformd>> contactFootPosesList_;
431 
-
433  std::shared_ptr<SwingTraj> swingTraj_ = nullptr;
+
433  Footstep * swingFootstep_ = nullptr;
434 
-
436  std::shared_ptr<TrajColl::CubicInterpolator<Eigen::Matrix3d, Eigen::Vector3d>> baseYawFunc_;
+
436  std::shared_ptr<SwingTraj> swingTraj_ = nullptr;
437 
-
439  std::shared_ptr<TrajColl::CubicSpline<Eigen::VectorXd>> armSwingFunc_;
+
439  std::shared_ptr<TrajColl::CubicInterpolator<Eigen::Matrix3d, Eigen::Vector3d>> baseYawFunc_;
440 
-
442  bool touchDown_ = false;
+
442  std::shared_ptr<TrajColl::CubicSpline<Eigen::VectorXd>> armSwingFunc_;
443 
-
445  std::unordered_map<Foot, std::string> impGainTypes_;
+
445  bool touchDown_ = false;
446 
-
448  bool requireImpGainUpdate_ = true;
-
449 };
-
450 } // namespace BWC
+
448  std::unordered_map<Foot, std::string> impGainTypes_;
+
449 
+
451  bool requireImpGainUpdate_ = true;
+
452 };
+
453 } // namespace BWC
-
BWC::FootManager::prevFootstep
std::shared_ptr< Footstep > prevFootstep() const
Access previous footstep.
Definition: FootManager.h:287
+
BWC::FootManager::prevFootstep
std::shared_ptr< Footstep > prevFootstep() const
Access previous footstep.
Definition: FootManager.h:290
BWC::FootManager::Configuration::impGains
std::unordered_map< std::string, mc_tasks::force::ImpedanceGains > impGains
Impedance gains for foot tasks.
Definition: FootManager.h:92
-
BWC::FootManager::contactFootPosesList_
std::map< double, std::unordered_map< Foot, sva::PTransformd > > contactFootPosesList_
Map of start time and contact foot poses.
Definition: FootManager.h:427
+
BWC::FootManager::contactFootPosesList_
std::map< double, std::unordered_map< Foot, sva::PTransformd > > contactFootPosesList_
Map of start time and contact foot poses.
Definition: FootManager.h:430
BWC::FootManager::update
virtual void update()
Update.
-
BWC::FootManager::swingFootstep_
Footstep * swingFootstep_
Footstep during swing.
Definition: FootManager.h:430
+
BWC::FootManager::swingFootstep_
Footstep * swingFootstep_
Footstep during swing.
Definition: FootManager.h:433
BWC::FootManager::appendFootstep
bool appendFootstep(const Footstep &newFootstep)
Append a target footstep to the queue.
BWC::FootManager::endVelMode
bool endVelMode()
End velocity mode.
-
BWC::FootManager::targetFootPoses_
std::unordered_map< Foot, sva::PTransformd > targetFootPoses_
Target foot pose represented in world frame.
Definition: FootManager.h:399
+
BWC::FootManager::targetFootPoses_
std::unordered_map< Foot, sva::PTransformd > targetFootPoses_
Target foot pose represented in world frame.
Definition: FootManager.h:402
BWC::FootManager::leftFootSupportRatio
double leftFootSupportRatio() const
Get the support ratio of left foot.
BWC::BaselineWalkingController
Humanoid walking controller with various baseline methods.
Definition: BaselineWalkingController.h:24
BWC::FootManager::removeFromLogger
void removeFromLogger(mc_rtc::Logger &logger)
Remove entries from the logger.
@@ -369,49 +371,50 @@
BWC::Foot::Left
@ Left
Left foot.
BWC::FootManager::config
const Configuration & config() const noexcept
Const accessor to the configuration.
Definition: FootManager.h:177
BWC::FootManager::VelModeData
Velocity mode data.
Definition: FootManager.h:113
-
BWC::FootManager::ctl
BaselineWalkingController & ctl()
Accessor to the controller.
Definition: FootManager.h:358
+
BWC::FootManager::ctl
BaselineWalkingController & ctl()
Accessor to the controller.
Definition: FootManager.h:361
BWC::FootManager::stop
void stop()
Stop.
+
BWC::FootManager::clearFootstepQueue
void clearFootstepQueue()
Clear footstep queue (with retaining footstep during swing).
BWC::FootManager
Foot manager.
Definition: FootManager.h:31
BWC::FootManager::Configuration::fricCoeff
double fricCoeff
Friction coefficient of foot contact.
Definition: FootManager.h:82
BWC::FootManager::Configuration::jointAnglesForArmSwing
std::unordered_map< std::string, std::map< std::string, std::vector< double > > > jointAnglesForArmSwing
Arm swing joint angles.
Definition: FootManager.h:98
-
BWC::FootManager::supportPhase_
SupportPhase supportPhase_
Support phase.
Definition: FootManager.h:418
+
BWC::FootManager::supportPhase_
SupportPhase supportPhase_
Support phase.
Definition: FootManager.h:421
BWC::FootManager::Configuration::deltaTransLimit
Eigen::Vector3d deltaTransLimit
Limit of foot midpose transformation for one footstep (x [m], y [m], theta [rad])
Definition: FootManager.h:47
-
BWC::FootManager::setRelativeVel
void setRelativeVel(const Eigen::Vector3d &targetVel)
Set the relative target velocity.
Definition: FootManager.h:339
+
BWC::FootManager::setRelativeVel
void setRelativeVel(const Eigen::Vector3d &targetVel)
Set the relative target velocity.
Definition: FootManager.h:342
BWC::FootManager::Configuration::keepPoseForTouchDownFoot
bool keepPoseForTouchDownFoot
Whether to keep foot pose of touch down foot during support phase.
Definition: FootManager.h:73
BWC::FootManager::reset
void reset()
Reset.
BWC::SupportPhase
SupportPhase
Support phase.
Definition: FootTypes.h:38
BWC::FootManager::clampDeltaTrans
Eigen::Vector3d clampDeltaTrans(const Eigen::Vector3d &deltaTrans, const Foot &foot)
Clamp a foot midpose transformation with limit.
BWC::FootManager::updateZmpTraj
virtual void updateZmpTraj()
Update ZMP trajectory.
-
BWC::FootManager::armSwingFunc_
std::shared_ptr< TrajColl::CubicSpline< Eigen::VectorXd > > armSwingFunc_
Arm swing joint angles trajectory.
Definition: FootManager.h:439
+
BWC::FootManager::armSwingFunc_
std::shared_ptr< TrajColl::CubicSpline< Eigen::VectorXd > > armSwingFunc_
Arm swing joint angles trajectory.
Definition: FootManager.h:442
BWC::Footstep
Footstep.
Definition: FootTypes.h:51
-
BWC::FootManager::velModeEnabled
bool velModeEnabled() const
Whether the velocity mode is enabled.
Definition: FootManager.h:345
+
BWC::FootManager::velModeEnabled
bool velModeEnabled() const
Whether the velocity mode is enabled.
Definition: FootManager.h:348
BWC::FootManager::calcRefGroundPosZ
double calcRefGroundPosZ(double t, int derivOrder=0) const
Calculate reference ground Z position.
BWC::FootManager::Configuration::name
std::string name
Name.
Definition: FootManager.h:38
BWC::FootManager::Configuration::enableArmSwing
bool enableArmSwing
Whether to enable arm swing.
Definition: FootManager.h:79
BWC::FootManager::VelModeData::config_
Configuration config_
Configuration.
Definition: FootManager.h:142
-
BWC::FootManager::ctl
const BaselineWalkingController & ctl() const
Const accessor to the controller.
Definition: FootManager.h:352
+
BWC::FootManager::ctl
const BaselineWalkingController & ctl() const
Const accessor to the controller.
Definition: FootManager.h:355
BWC::FootManager::velModeData
const VelModeData & velModeData() const noexcept
Const accessor to the velocity mode data.
Definition: FootManager.h:183
BWC::FootManager::Configuration::footTaskGain
TaskGain footTaskGain
Foot task gains.
Definition: FootManager.h:55
BWC::FootManager::Configuration::zmpOffset
Eigen::Vector3d zmpOffset
ZMP offset of each foot (positive for x-forward, y-outside, z-upward) [m].
Definition: FootManager.h:61
BWC::FootManager::VelModeData::reset
void reset(bool enabled)
Reset.
-
BWC::FootManager::supportPhase
SupportPhase supportPhase() const noexcept
Get the support phase.
Definition: FootManager.h:311
+
BWC::FootManager::supportPhase
SupportPhase supportPhase() const noexcept
Get the support phase.
Definition: FootManager.h:314
BWC::FootManager::VelModeData::targetVel_
Eigen::Vector3d targetVel_
Relative target velocity of foot midpose in the velocity mode (x [m/s], y [m/s], theta [rad/s])
Definition: FootManager.h:148
BWC::FootManager::VelModeData::Configuration
Configuration.
Definition: FootManager.h:117
BWC::FootManager::Configuration::touchDownForceZ
double touchDownForceZ
Definition: FootManager.h:88
-
BWC::FootManager::footTaskGains_
std::unordered_map< Foot, TaskGain > footTaskGains_
Foot task gains.
Definition: FootManager.h:408
+
BWC::FootManager::footTaskGains_
std::unordered_map< Foot, TaskGain > footTaskGains_
Foot task gains.
Definition: FootManager.h:411
BWC
Definition: BaselineWalkingController.h:18
BWC::FootManager::Configuration
Configuration.
Definition: FootManager.h:35
BWC::FootManager::Configuration::load
void load(const mc_rtc::Configuration &mcRtcConfig)
Load mc_rtc configuration.
-
BWC::FootManager::baseYawFunc_
std::shared_ptr< TrajColl::CubicInterpolator< Eigen::Matrix3d, Eigen::Vector3d > > baseYawFunc_
Base link Yaw trajectory.
Definition: FootManager.h:436
-
BWC::FootManager::targetFootAccels_
std::unordered_map< Foot, sva::MotionVecd > targetFootAccels_
Target foot acceleration represented in world frame.
Definition: FootManager.h:405
+
BWC::FootManager::baseYawFunc_
std::shared_ptr< TrajColl::CubicInterpolator< Eigen::Matrix3d, Eigen::Vector3d > > baseYawFunc_
Base link Yaw trajectory.
Definition: FootManager.h:439
+
BWC::FootManager::targetFootAccels_
std::unordered_map< Foot, sva::MotionVecd > targetFootAccels_
Target foot acceleration represented in world frame.
Definition: FootManager.h:408
BWC::FootManager::Configuration::touchDownPosError
double touchDownPosError
Definition: FootManager.h:87
-
BWC::FootManager::trajStartFootPoses_
std::unordered_map< Foot, sva::PTransformd > trajStartFootPoses_
Foot poses of start of trajectory.
Definition: FootManager.h:411
+
BWC::FootManager::trajStartFootPoses_
std::unordered_map< Foot, sva::PTransformd > trajStartFootPoses_
Foot poses of start of trajectory.
Definition: FootManager.h:414
BWC::FootManager::startVelMode
bool startVelMode()
Start velocity mode.
BWC::FootManager::VelModeData::VelModeData
VelModeData()
Constructor.
Definition: FootManager.h:133
BWC::FootManager::touchDownRemainingDuration
double touchDownRemainingDuration() const
Get the remaining duration for next touch down.
BWC::Foot
Foot
Foot.
Definition: FootTypes.h:10
-
BWC::FootManager::zmpFunc_
std::shared_ptr< TrajColl::CubicInterpolator< Eigen::Vector3d > > zmpFunc_
ZMP function.
Definition: FootManager.h:421
-
BWC::FootManager::velModeData_
VelModeData velModeData_
Velocity mode data.
Definition: FootManager.h:387
+
BWC::FootManager::zmpFunc_
std::shared_ptr< TrajColl::CubicInterpolator< Eigen::Vector3d > > zmpFunc_
ZMP function.
Definition: FootManager.h:424
+
BWC::FootManager::velModeData_
VelModeData velModeData_
Velocity mode data.
Definition: FootManager.h:390
BWC::FootManager::detectTouchDown
bool detectTouchDown() const
Detect touch down.
BWC::FootManager::addToLogger
void addToLogger(mc_rtc::Logger &logger)
Add entries to the logger.
BWC::FootManager::calcRefZmp
Eigen::Vector3d calcRefZmp(double t, int derivOrder=0) const
Calculate reference ZMP.
@@ -423,41 +426,41 @@
BWC::FootManager::walkToRelativePose
bool walkToRelativePose(const Eigen::Vector3d &targetTrans, int lastFootstepNum=0, const std::vector< Eigen::Vector3d > &waypointTransList={})
Send footstep sequence to walk to the relative target pose.
BWC::FootManager::Configuration::footstepDuration
double footstepDuration
Duration of one footstep. [sec].
Definition: FootManager.h:41
BWC::FootManager::Configuration::doubleSupportRatio
double doubleSupportRatio
Duration ratio of double support phase.
Definition: FootManager.h:44
-
BWC::FootManager::impGainTypes_
std::unordered_map< Foot, std::string > impGainTypes_
Types of impedance gains for foot tasks.
Definition: FootManager.h:445
-
BWC::FootManager::swingTraj_
std::shared_ptr< SwingTraj > swingTraj_
Foot swing trajectory.
Definition: FootManager.h:433
+
BWC::FootManager::impGainTypes_
std::unordered_map< Foot, std::string > impGainTypes_
Types of impedance gains for foot tasks.
Definition: FootManager.h:448
+
BWC::FootManager::swingTraj_
std::shared_ptr< SwingTraj > swingTraj_
Foot swing trajectory.
Definition: FootManager.h:436
BWC::FootManager::addToGUI
void addToGUI(mc_rtc::gui::StateBuilder &gui)
Add entries to the GUI.
BWC::FootManager::Configuration::overwriteLandingPose
bool overwriteLandingPose
Whether to overwrite landing pose so that the relative pose from support foot to swing foot is retain...
Definition: FootManager.h:67
ForceColl
Definition: CentroidalManager.h:15
BWC::FootManager::surfaceName
const std::string & surfaceName(const Foot &foot) const
Get foot surface name.
-
BWC::FootManager::targetFootAccel
const sva::MotionVecd & targetFootAccel(const Foot &foot) const
Get the target foot acceleration represented in world frame.
Definition: FootManager.h:305
+
BWC::FootManager::targetFootAccel
const sva::MotionVecd & targetFootAccel(const Foot &foot) const
Get the target foot acceleration represented in world frame.
Definition: FootManager.h:308
BWC::FootManager::Configuration::zmpHorizon
double zmpHorizon
Horizon of ZMP trajectory [sec].
Definition: FootManager.h:58
-
BWC::FootManager::footstepQueue_
std::deque< Footstep > footstepQueue_
Footstep queue.
Definition: FootManager.h:393
+
BWC::FootManager::footstepQueue_
std::deque< Footstep > footstepQueue_
Footstep queue.
Definition: FootManager.h:396
BWC::FootManager::makeFootstep
Footstep makeFootstep(const Foot &foot, const sva::PTransformd &footMidpose, double startTime, const mc_rtc::Configuration &swingTrajConfig={}) const
Make a footstep.
-
BWC::FootManager::targetFootVels_
std::unordered_map< Foot, sva::MotionVecd > targetFootVels_
Target foot velocity represented in world frame.
Definition: FootManager.h:402
+
BWC::FootManager::targetFootVels_
std::unordered_map< Foot, sva::MotionVecd > targetFootVels_
Target foot velocity represented in world frame.
Definition: FootManager.h:405
BWC::FootManager::Configuration::defaultSwingTrajType
std::string defaultSwingTrajType
Default swing trajectory type.
Definition: FootManager.h:64
-
BWC::FootManager::prevFootstep_
std::shared_ptr< Footstep > prevFootstep_
Previous footstep.
Definition: FootManager.h:396
+
BWC::FootManager::prevFootstep_
std::shared_ptr< Footstep > prevFootstep_
Previous footstep.
Definition: FootManager.h:399
BWC::FootManager::removeFromGUI
void removeFromGUI(mc_rtc::gui::StateBuilder &gui)
Remove entries from the GUI.
-
BWC::FootManager::groundPosZFunc_
std::shared_ptr< TrajColl::CubicInterpolator< double > > groundPosZFunc_
Ground Z position function.
Definition: FootManager.h:424
+
BWC::FootManager::groundPosZFunc_
std::shared_ptr< TrajColl::CubicInterpolator< double > > groundPosZFunc_
Ground Z position function.
Definition: FootManager.h:427
BWC::FootManager::VelModeData::Configuration::footstepQueueSize
int footstepQueueSize
Queue size of footsteps to be sent in the velocity mode (must be at least 3)
Definition: FootManager.h:120
BWC::FootManager::calcZmpWithOffset
Eigen::Vector3d calcZmpWithOffset(const Foot &foot, const sva::PTransformd &footPose) const
Calculate ZMP with offset.
BWC::FootManager::updateVelMode
void updateVelMode()
Update footstep sequence for the velocity mode.
BWC::SupportPhase::DoubleSupport
@ DoubleSupport
Both feet support phase.
RobotUtils.h
-
BWC::FootManager::ctlPtr_
BaselineWalkingController * ctlPtr_
Pointer to controller.
Definition: FootManager.h:390
+
BWC::FootManager::ctlPtr_
BaselineWalkingController * ctlPtr_
Pointer to controller.
Definition: FootManager.h:393
BWC::FootManager::Configuration::touchDownRemainingDuration
double touchDownRemainingDuration
Definition: FootManager.h:86
-
BWC::FootManager::targetFootPose
const sva::PTransformd & targetFootPose(const Foot &foot) const
Get the target foot pose represented in world frame.
Definition: FootManager.h:293
-
BWC::FootManager::footstepQueue
const std::deque< Footstep > & footstepQueue() const noexcept
Access footstep queue.
Definition: FootManager.h:281
-
BWC::FootManager::requireImpGainUpdate_
bool requireImpGainUpdate_
Whether to require updating impedance gains for foot tasks.
Definition: FootManager.h:448
+
BWC::FootManager::targetFootPose
const sva::PTransformd & targetFootPose(const Foot &foot) const
Get the target foot pose represented in world frame.
Definition: FootManager.h:296
+
BWC::FootManager::footstepQueue
const std::deque< Footstep > & footstepQueue() const noexcept
Access footstep queue.
Definition: FootManager.h:284
+
BWC::FootManager::requireImpGainUpdate_
bool requireImpGainUpdate_
Whether to require updating impedance gains for foot tasks.
Definition: FootManager.h:451
BWC::FootManager::getCurrentContactFeet
std::set< Foot > getCurrentContactFeet() const
Get current contact feet.
BWC::FootManager::Configuration::stopSwingTrajForTouchDownFoot
bool stopSwingTrajForTouchDownFoot
Whether to stop swing trajectory for touch down foot.
Definition: FootManager.h:70
BWC::FootManager::VelModeData::Configuration::load
void load(const mc_rtc::Configuration &mcRtcConfig)
Load mc_rtc configuration.
-
BWC::FootManager::touchDown_
bool touchDown_
Whether touch down is detected during swing.
Definition: FootManager.h:442
+
BWC::FootManager::touchDown_
bool touchDown_
Whether touch down is detected during swing.
Definition: FootManager.h:445
FootTypes.h
-
BWC::FootManager::config_
Configuration config_
Configuration.
Definition: FootManager.h:384
+
BWC::FootManager::config_
Configuration config_
Configuration.
Definition: FootManager.h:387
BWC::FootManager::Configuration::midToFootTranss
std::unordered_map< Foot, sva::PTransformd > midToFootTranss
Transformation from foot midpose to each foot pose.
Definition: FootManager.h:50
-
BWC::FootManager::trajStartFootPoseFuncs_
std::unordered_map< Foot, std::shared_ptr< TrajColl::CubicInterpolator< sva::PTransformd, sva::MotionVecd > > > trajStartFootPoseFuncs_
Functions for foot poses of start of trajectory.
Definition: FootManager.h:415
+
BWC::FootManager::trajStartFootPoseFuncs_
std::unordered_map< Foot, std::shared_ptr< TrajColl::CubicInterpolator< sva::PTransformd, sva::MotionVecd > > > trajStartFootPoseFuncs_
Functions for foot poses of start of trajectory.
Definition: FootManager.h:418
BWC::FootManager::VelModeData::enabled_
bool enabled_
Whether the velocity mode is enabled.
Definition: FootManager.h:145
-
BWC::FootManager::targetFootVel
const sva::MotionVecd & targetFootVel(const Foot &foot) const
Get the target foot velocity represented in world frame.
Definition: FootManager.h:299
+
BWC::FootManager::targetFootVel
const sva::MotionVecd & targetFootVel(const Foot &foot) const
Get the target foot velocity represented in world frame.
Definition: FootManager.h:302