leg_state.h

[詳解]

 
 
// Copyright(c) 2023-2025 Design Engineering Laboratory, Saitama University
// Released under the MIT license
// https://opensource.org/licenses/mit-license.php
 
#ifndef DESIGNLAB_LEG_STATE_H_
#define DESIGNLAB_LEG_STATE_H_
 
#include <array>
#include <bitset>
#include <vector>
 
#include "discrete_com_pos.h"
#include "discrete_leg_pos.h"
#include "hexapod_const.h"
 
 
namespace designlab::leg_func
{
 
// 使用する型の定義
 
constexpr int kLegPosBitNum = 4;
 
constexpr int kComPosBitNum = 4;  
 
constexpr int kLegStateBitNum = HexapodConst::kLegNum * kLegPosBitNum + kComPosBitNum;
 
using LegStateBit = std::bitset<kLegStateBitNum>;
 
using LegGroundedBit = std::bitset<HexapodConst::kLegNum>;
 
 
 
// ビットをマスクするための定数
 
constexpr LegStateBit kLegPosMaskBit(0b0111);
 
constexpr LegStateBit kLegStateMaskBit(0b1111);
 
 
constexpr int kShiftToComNum = HexapodConst::kLegNum * 4;
 
constexpr LegStateBit kComStateMaskBit = (0b1111 << kShiftToComNum);
 
 
 
LegStateBit MakeLegStateBit(
  enums::DiscreteComPos discrete_com_pos,
  const std::array<bool, HexapodConst::kLegNum>& is_ground,
  const std::array<enums::DiscreteLegPos, HexapodConst::kLegNum>& leg_pos);
 
 
bool IsGrounded(const LegStateBit& leg_state_bit, int leg_index);
 
LegGroundedBit GetLegGroundedBit(const LegStateBit& leg_state);
 
int GetGroundedLegNum(const LegStateBit& leg_state);
 
int GetLiftedLegNum(const LegStateBit& leg_state);
 
void GetGroundedLegIndexByVector(
    const LegStateBit& leg_state, std::vector<int>* res_index);
 
void GetLiftedLegIndexByVector(const LegStateBit& leg_state, std::vector<int>* res_index);
 
 
enums::DiscreteLegPos GetDiscreteLegPos(const LegStateBit& leg_state, int leg_index);
 
enums::DiscreteComPos GetDiscreteComPos(const LegStateBit& leg_state);
 
 
void ChangeLegState(int leg_index, enums::DiscreteLegPos new_discretized_leg_pos,
                    bool is_ground, LegStateBit* leg_state);
 
void ChangeDiscreteLegPos(int leg_index, enums::DiscreteLegPos new_discretized_leg_pos,
                          LegStateBit* leg_state);
 
void ChangeGround(int leg_index, bool is_ground, LegStateBit* leg_state);
 
void ChangeAllLegGround(const LegGroundedBit& is_ground_list, LegStateBit* leg_state);
 
void ChangeDiscreteComPos(enums::DiscreteComPos new_com_pattern, LegStateBit* leg_state);
 
 
}  // namespace designlab::leg_func
 
 
#endif  // DESIGNLAB_LEG_STATE_H_