Skip to content

Commit

Permalink
Config for LIPMWalking
Browse files Browse the repository at this point in the history
  • Loading branch information
@arntanguy
arntanguy committed Mar 19, 2024
1 parent 212abbe commit bed296d
Show file tree
Hide file tree
Showing 3 changed files with 251 additions and 14 deletions.
3 changes: 3 additions & 0 deletions CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,3 +19,6 @@ if(NOT ${DISABLE_TESTS})
enable_testing()
add_subdirectory(tests)
endif()

install(FILES etc/LIPMWalking/hoap3.yaml
DESTINATION ${MC_CONTROLLER_RUNTIME_INSTALL_PREFIX}/LIPMWalking/)
227 changes: 227 additions & 0 deletions etc/LIPMWalking/hoap3.yaml
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,227 @@
initial_plan: custom_forward
# for the external planner HybridPlanner
HybridPlanner:
Tp: 6
delta: 0.05
Ts_limit: [1,2]
kinematics_cstr: [0.35,0.08]
feet_distance: 0.1
offset_angle_deg: 2
mean_speed: 0.1
robot_height: 150
max_rotation: 0.17
robot_models:
hoap3:
sole:
half_length: 0.0455
half_width: 0.0355
friction: 0.7
torso:
pitch: 0.1

plans:
hoap3:
# ashibumi: # stepping in place
# double_support_duration: 0.2
# single_support_duration: 0.8
# swing_height: 0.04
# contacts:
# - pose:
# translation: [0.01, -0.055, 0.0]
# surface: RightFootCenter
# - pose:
# translation: [0.01, 0.055, 0.0]
# surface: LeftFootCenter
# - pose:
# translation: [0.01, -0.055, 0.0]
# surface: RightFootCenter
# - pose:
# translation: [0.01, 0.055, 0.0]
# surface: LeftFootCenter
# - pose:
# translation: [0.01, -0.055, 0.0]
# surface: RightFootCenter
# - pose:
# translation: [0.01, 0.055, 0.0]
# surface: LeftFootCenter
# - pose:
# translation: [0.01, -0.055, 0.0]
# surface: RightFootCenter
# - pose:
# translation: [0.01, 0.055, 0.0]
# surface: LeftFootCenter
# - pose:
# translation: [0.01, -0.055, 0.0]
# surface: RightFootCenter
# - pose:
# translation: [0.01, 0.055, 0.0]
# surface: LeftFootCenter
# - pose:
# translation: [0.01, -0.055, 0.0]
# surface: RightFootCenter
# - pose:
# translation: [0.01, 0.055, 0.0]
# surface: LeftFootCenter
# custom_backward:
# double_support_duration: 0.2
# single_support_duration: 0.8
# step_length: 0.05
# swing_height: 0.05
# contacts:
# - pose:
# translation: [0.0, -0.055, 0.0]
# surface: RightFootCenter
# - pose:
# translation: [0.0, 0.055, 0.0]
# surface: LeftFootCenter
custom_forward:
double_support_duration: 0.1
single_support_duration: 0.7
step_length: 0.05
swing_height: 0.04
contacts:
- pose:
translation: [0.0, -0.01, 0.0]
surface: RightFootCenter
- pose:
translation: [0.0, 0.01, 0.0]
surface: LeftFootCenter
# custom_lateral:
# double_support_duration: 0.2
# single_support_duration: 0.8
# step_length: 0.05
# swing_height: 0.04
# contacts:
# - pose:
# translation: [0.0, -0.055, 0.0]
# surface: RightFootCenter
# - pose:
# translation: [0.0, 0.055, 0.0]
# surface: LeftFootCenter
# mpc:
# weights:
# jerk: 1.0
# vel: [10.0, 300.0]
# zmp: 1000.0
# walk_backward_75cm:
# double_support_duration: 0.2
# single_support_duration: 0.8
# swing_height: 0.05
# contacts:
# - pose:
# translation: [0.0, -0.055, 0.0]
# ref_vel: [0.0, 0.0, 0.0]
# surface: RightFootCenter
# - pose:
# translation: [0.0, 0.055, 0.0]
# ref_vel: [0.0, 0.0, 0.0]
# surface: LeftFootCenter
# - pose:
# translation: [-0.15, -0.055, 0.0]
# ref_vel: [-0.075, 0.0, 0.0]
# surface: RightFootCenter
# - pose:
# translation: [-0.3, 0.055, 0.0]
# ref_vel: [-0.15, 0.0, 0.0]
# surface: LeftFootCenter
# - pose:
# translation: [-0.45, -0.055, 0.0]
# ref_vel: [-0.15, 0.0, 0.0]
# surface: RightFootCenter
# - pose:
# translation: [-0.6, 0.055, 0.0]
# ref_vel: [-0.075, 0.0, 0.0]
# surface: LeftFootCenter
# - pose:
# translation: [-0.75, -0.055, 0.0]
# ref_vel: [0.0, 0.0, 0.0]
# surface: RightFootCenter
# - pose:
# translation: [-0.75, 0.055, 0.0]
# ref_vel: [0.0, 0.0, 0.0]
# surface: LeftFootCenter
# walk_forward_100cm:
# double_support_duration: 0.1
# single_support_duration: 0.7
# swing_height: 0.04
# contacts:
# - pose:
# translation: [0.0, -0.055, 0.0]
# ref_vel: [0.0, 0.0, 0.0]
# surface: RightFootCenter
# - pose:
# translation: [0.0, 0.055, 0.0]
# ref_vel: [0.0, 0.0, 0.0]
# surface: LeftFootCenter
# - pose:
# translation: [0.2, -0.055, 0.0]
# ref_vel: [0.1, 0.0, 0.0]
# surface: RightFootCenter
# - pose:
# translation: [0.4, 0.055, 0.0]
# ref_vel: [0.2, 0.0, 0.0]
# surface: LeftFootCenter
# - pose:
# translation: [0.6, -0.055, 0.0]
# ref_vel: [0.2, 0.0, 0.0]
# surface: RightFootCenter
# - pose:
# translation: [0.8, 0.055, 0.0]
# ref_vel: [0.1, 0.0, 0.0]
# surface: LeftFootCenter
# - pose:
# translation: [1.0, -0.055, 0.0]
# ref_vel: [0.0, 0.0, 0.0]
# surface: RightFootCenter
# - pose:
# translation: [1.0, 0.055, 0.0]
# ref_vel: [0.0, 0.0, 0.0]
# surface: LeftFootCenter
# warmup:
# double_support_duration: 0.1
# single_support_duration: 0.7
# swing_height: 0.04
# contacts:
# - pose:
# translation: [0.035, -0.055, 0.0]
# surface: RightFootCenter
# - pose:
# translation: [0.035, 0.055, 0.0]
# surface: LeftFootCenter
# - pose:
# translation: [0.035, -0.055, 0.0]
# surface: RightFootCenter
# - pose:
# translation: [0.035, 0.055, 0.0]
# surface: LeftFootCenter
# external:
# double_support_duration: 0.5
# single_support_duration: 0.7
# allowed_planning_time:
# standing: 2.0
# single_support: 0.2
# swing_height: 0.04
# com_height: 0.3
# contacts:
# - pose:
# translation: [0.0, -0.055, 0.0]
# surface: RightFootCenter
# - pose:
# translation: [0.0, 0.055, 0.0]
# surface: LeftFootCenter
# leftFootLandingOffset: [0.0, 0.055, 0.0] # x, y, theta
# rightFootLandingOffset: [0.0, -0.055, 0.0] # x, y, theta
# Set realRobot's joint configuration from encoder readings
ObserverPipelines:
name: "LIPMWalkingObserverPipeline"
gui: true
observers:
- type: Encoder
- type: Attitude
required: false
- type: KinematicInertial
config:
anchorFrame:
maxAnchorFrameDiscontinuity: 0.02
- type: BodySensor
update: false
35 changes: 21 additions & 14 deletions src/hoap3.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,8 +7,6 @@
namespace mc_robots
{

constexpr auto PI = mc_rtc::constants::PI;

HOAP3RobotModule::HOAP3RobotModule(const std::string & moduleName, Base base, bool canonical)
: RobotModule(HOAP3_DESCRIPTION_PATH, "hoap3", fmt::format("{}/urdf/hoap3.urdf", HOAP3_DESCRIPTION_PATH)),
moduleName_(moduleName), base_(base), canonical_(canonical)
Expand All @@ -29,8 +27,8 @@ HOAP3RobotModule::HOAP3RobotModule(const std::string & moduleName, Base base, bo
}

// TODO:
_bodySensors.emplace_back("Accelerometer", "body", sva::PTransformd(Eigen::Vector3d(-0.0325, 0, 0.1095)));
_bodySensors.emplace_back("FloatingBase", "body", sva::PTransformd::Identity());
_bodySensors.emplace_back("Accelerometer", "WAIST", sva::PTransformd(Eigen::Vector3d(-0.0325, 0, 0.1095)));
_bodySensors.emplace_back("FloatingBase", "WAIST", sva::PTransformd::Identity());

_stance["CHEST_H_PAN_joint"] = {0.0};
_stance["H_PAN_H_TILT_joint"] = {0.0};
Expand Down Expand Up @@ -60,10 +58,8 @@ HOAP3RobotModule::HOAP3RobotModule(const std::string & moduleName, Base base, bo
_stance["L_AFE_L_AAA_joint"] = {-0.10471975512};

// FIXME
_forceSensors.push_back(
mc_rbdyn::ForceSensor("RightFootForceSensor", "r_ankle", sva::PTransformd(Eigen::Vector3d(0, 0, -0.093))));
_forceSensors.push_back(
mc_rbdyn::ForceSensor("LeftFootForceSensor", "l_ankle", sva::PTransformd(Eigen::Vector3d(0, 0, -0.093))));
_forceSensors.push_back(mc_rbdyn::ForceSensor("RightFootForceSensor", "R_FOOT", sva::PTransformd::Identity()));
_forceSensors.push_back(mc_rbdyn::ForceSensor("LeftFootForceSensor", "L_FOOT", sva::PTransformd::Identity()));

// FIXME
_minimalSelfCollisions = {
Expand All @@ -90,7 +86,7 @@ HOAP3RobotModule::HOAP3RobotModule(const std::string & moduleName, Base base, bo
// _grippers = {{"l_gripper", {"L_HAND_J0", "L_HAND_J1"}, false}, {"r_gripper", {"R_HAND_J0", "R_HAND_J1"}, true}};

// FIXME
_default_attitude = {{1., 0., 0., 0., 0., 0., 0.747187}};
_default_attitude = {{1., 0., 0., 0., 0., 0., 0.30}};

// Compound Joints
_compoundJoints = {};
Expand All @@ -99,11 +95,22 @@ HOAP3RobotModule::HOAP3RobotModule(const std::string & moduleName, Base base, bo
// Configure the stabilizer
_lipmStabilizerConfig.leftFootSurface = "LeftFootCenter";
_lipmStabilizerConfig.rightFootSurface = "RightFootCenter";
_lipmStabilizerConfig.torsoBodyName = "torso";
_lipmStabilizerConfig.comHeight = 0.78;
_lipmStabilizerConfig.comActiveJoints = {"Root", "R_HIP_Y", "R_HIP_R", "R_HIP_P", "R_KNEE_P",
"R_ANKLE_P", "R_ANKLE_R", "L_HIP_Y", "L_HIP_R", "L_HIP_P",
"L_KNEE_P", "L_ANKLE_P", "L_ANKLE_R"};
_lipmStabilizerConfig.torsoBodyName = "CHEST";
_lipmStabilizerConfig.comHeight = 0.21;
_lipmStabilizerConfig.comActiveJoints = {"Root",
"WAIST_R_LR_joint",
"R_LR_R_LAA_joint",
"R_LAA_R_LFE_joint",
"R_LFE_R_KN_joint",
"R_KN_R_AFE_joint",
"R_AFE_R_AAA_joint",
"WAIST_L_LR_joint",
"L_LR_L_LAA_joint",
"L_LAA_L_LFE_joint",
"L_LFE_L_KN_joint",
"L_KN_L_AFE_joint",
"L_AFE_L_AAA_joint"};

_lipmStabilizerConfig.torsoPitch = 0;
_lipmStabilizerConfig.copAdmittance = Eigen::Vector2d{0.01, 0.01};
_lipmStabilizerConfig.dcmPropGain = 5.0;
Expand Down

0 comments on commit bed296d

Please sign in to comment.