Skip to content

Commit

Permalink
refactor: use jitsuyo to get value
Browse files Browse the repository at this point in the history
  • Loading branch information
@Mobilizes
Mobilizes committed Jun 24, 2024
1 parent 8bc1f4c commit 627480f
Show file tree
Hide file tree
Showing 2 changed files with 186 additions and 131 deletions.
202 changes: 115 additions & 87 deletions src/aruku/walking/node/walking_manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,6 +25,7 @@
#include "aruku/walking/node/walking_manager.hpp"

#include "aruku/walking/process/kinematic.hpp"
#include "jitsuyo/config.hpp"
#include "keisan/angle/angle.hpp"
#include "nlohmann/json.hpp"
#include "tachimawari/joint/model/joint_id.hpp"
Expand Down Expand Up @@ -55,94 +56,121 @@ void WalkingManager::set_config(
const nlohmann::json & walking_data,
const nlohmann::json & kinematic_data)
{
for (auto &[key, val] : walking_data.items()) {
if (key == "balance") {
try {
val.at("enable").get_to(balance_enable);
val.at("balance_knee_gain").get_to(balance_knee_gain);
val.at("balance_ankle_pitch_gain").get_to(balance_ankle_pitch_gain);
val.at("balance_hip_roll_gain").get_to(balance_hip_roll_gain);
val.at("balance_ankle_roll_gain").get_to(balance_ankle_roll_gain);
} catch (nlohmann::json::parse_error & ex) {
std::cerr << "parse error at byte " << ex.byte << std::endl;
throw ex;
}
} else if (key == "pid") {
try {
val.at("p_gain").get_to(p_gain);
val.at("i_gain").get_to(i_gain);
val.at("d_gain").get_to(d_gain);
} catch (nlohmann::json::parse_error & ex) {
std::cerr << "parse error at byte " << ex.byte << std::endl;
throw ex;
}
} else if (key == "odometry") {
try {
val.at("fx_coefficient").get_to(odometry_fx_coefficient);
val.at("ly_coefficient").get_to(odometry_ly_coefficient);
val.at("ry_coefficient").get_to(odometry_ry_coefficient);
val.at("bx_coefficient").get_to(odometry_bx_coefficient);
} catch (nlohmann::json::parse_error & ex) {
std::cerr << "parse error at byte " << ex.byte << std::endl;
throw ex;
}
} else if (key == "init_angles") {
try {
{
using tachimawari::joint::JointId;

val.at("right_hip_yaw").get_to(inital_joints[JointId::RIGHT_HIP_YAW]);
val.at("right_hip_pitch").get_to(inital_joints[JointId::RIGHT_HIP_PITCH]);
val.at("right_hip_roll").get_to(inital_joints[JointId::RIGHT_HIP_ROLL]);
val.at("right_knee").get_to(inital_joints[JointId::RIGHT_KNEE]);
val.at("right_ankle_pitch").get_to(inital_joints[JointId::RIGHT_ANKLE_PITCH]);
val.at("right_ankle_roll").get_to(inital_joints[JointId::RIGHT_ANKLE_ROLL]);
val.at("left_hip_yaw").get_to(inital_joints[JointId::LEFT_HIP_YAW]);
val.at("left_hip_pitch").get_to(inital_joints[JointId::LEFT_HIP_PITCH]);
val.at("left_hip_roll").get_to(inital_joints[JointId::LEFT_HIP_ROLL]);
val.at("left_knee").get_to(inital_joints[JointId::LEFT_KNEE]);
val.at("left_ankle_pitch").get_to(inital_joints[JointId::LEFT_ANKLE_PITCH]);
val.at("left_ankle_roll").get_to(inital_joints[JointId::LEFT_ANKLE_ROLL]);
val.at("right_shoulder_pitch").get_to(inital_joints[JointId::RIGHT_SHOULDER_PITCH]);
val.at("right_shoulder_roll").get_to(inital_joints[JointId::RIGHT_SHOULDER_ROLL]);
val.at("right_elbow").get_to(inital_joints[JointId::RIGHT_ELBOW]);
val.at("left_shoulder_pitch").get_to(inital_joints[JointId::LEFT_SHOULDER_PITCH]);
val.at("left_shoulder_roll").get_to(inital_joints[JointId::LEFT_SHOULDER_ROLL]);
val.at("left_elbow").get_to(inital_joints[JointId::LEFT_ELBOW]);
}
} catch (nlohmann::json::parse_error & ex) {
std::cerr << "parse error at byte " << ex.byte << std::endl;
throw ex;
}
} else if (key == "angles_direction") {
try {
{
using tachimawari::joint::JointId;

val.at("right_hip_yaw").get_to(joints_direction[JointId::RIGHT_HIP_YAW]);
val.at("right_hip_pitch").get_to(joints_direction[JointId::RIGHT_HIP_PITCH]);
val.at("right_hip_roll").get_to(joints_direction[JointId::RIGHT_HIP_ROLL]);
val.at("right_knee").get_to(joints_direction[JointId::RIGHT_KNEE]);
val.at("right_ankle_pitch").get_to(joints_direction[JointId::RIGHT_ANKLE_PITCH]);
val.at("right_ankle_roll").get_to(joints_direction[JointId::RIGHT_ANKLE_ROLL]);
val.at("left_hip_yaw").get_to(joints_direction[JointId::LEFT_HIP_YAW]);
val.at("left_hip_pitch").get_to(joints_direction[JointId::LEFT_HIP_PITCH]);
val.at("left_hip_roll").get_to(joints_direction[JointId::LEFT_HIP_ROLL]);
val.at("left_knee").get_to(joints_direction[JointId::LEFT_KNEE]);
val.at("left_ankle_pitch").get_to(joints_direction[JointId::LEFT_ANKLE_PITCH]);
val.at("left_ankle_roll").get_to(joints_direction[JointId::LEFT_ANKLE_ROLL]);
val.at("right_shoulder_pitch").get_to(joints_direction[JointId::RIGHT_SHOULDER_PITCH]);
val.at("right_shoulder_roll").get_to(joints_direction[JointId::RIGHT_SHOULDER_ROLL]);
val.at("right_elbow").get_to(joints_direction[JointId::RIGHT_ELBOW]);
val.at("left_shoulder_pitch").get_to(joints_direction[JointId::LEFT_SHOULDER_PITCH]);
val.at("left_shoulder_roll").get_to(joints_direction[JointId::LEFT_SHOULDER_ROLL]);
val.at("left_elbow").get_to(joints_direction[JointId::LEFT_ELBOW]);
}
} catch (nlohmann::json::parse_error & ex) {
std::cerr << "parse error at byte " << ex.byte << std::endl;
throw ex;
}
bool valid_config = true;

nlohmann::json balance_section;
if (jitsuyo::assign_val(walking_data, "balance", balance_section)) {
bool valid_section = true;
valid_section &= jitsuyo::assign_val(balance_section, "enable", balance_enable);
valid_section &= jitsuyo::assign_val(balance_section, "balance_knee_gain", balance_knee_gain);
valid_section &= jitsuyo::assign_val(balance_section, "balance_ankle_pitch_gain", balance_ankle_pitch_gain);
valid_section &= jitsuyo::assign_val(balance_section, "balance_hip_roll_gain", balance_hip_roll_gain);
valid_section &= jitsuyo::assign_val(balance_section, "balance_ankle_roll_gain", balance_ankle_roll_gain);
if (!valid_section) {
std::cout << "Error found at section `balance`" << std::endl;
valid_config = false;
}
} else {
valid_config = false;
}

nlohmann::json pid_section;
if (jitsuyo::assign_val(walking_data, "pid", pid_section)) {
bool valid_section = true;
valid_section &= jitsuyo::assign_val(pid_section, "p_gain", p_gain);
valid_section &= jitsuyo::assign_val(pid_section, "i_gain", i_gain);
valid_section &= jitsuyo::assign_val(pid_section, "d_gain", d_gain);
if (!valid_section) {
std::cout << "Error found at section `pid`" << std::endl;
valid_config = false;
}
} else {
valid_config = false;
}

nlohmann::json odometry_section;
if (jitsuyo::assign_val(walking_data, "odometry", odometry_section)) {
bool valid_section = true;
valid_section &= jitsuyo::assign_val(odometry_section, "fx_coefficient", odometry_fx_coefficient);
valid_section &= jitsuyo::assign_val(odometry_section, "ly_coefficient", odometry_ly_coefficient);
valid_section &= jitsuyo::assign_val(odometry_section, "ry_coefficient", odometry_ry_coefficient);
valid_section &= jitsuyo::assign_val(odometry_section, "bx_coefficient", odometry_bx_coefficient);
if (!valid_section) {
std::cout << "Error found at section `odometry`" << std::endl;
valid_config = false;
}
} else {
valid_config = false;
}

nlohmann::json init_angles_section;
if (jitsuyo::assign_val(walking_data, "init_angles", init_angles_section)) {
bool valid_section = true;

using tachimawari::joint::JointId;

valid_section &= jitsuyo::assign_val(init_angles_section, "right_hip_yaw", inital_joints[JointId::RIGHT_HIP_YAW]);
valid_section &= jitsuyo::assign_val(init_angles_section, "right_hip_pitch", inital_joints[JointId::RIGHT_HIP_PITCH]);
valid_section &= jitsuyo::assign_val(init_angles_section, "right_hip_roll", inital_joints[JointId::RIGHT_HIP_ROLL]);
valid_section &= jitsuyo::assign_val(init_angles_section, "right_knee", inital_joints[JointId::RIGHT_KNEE]);
valid_section &= jitsuyo::assign_val(init_angles_section, "right_ankle_pitch", inital_joints[JointId::RIGHT_ANKLE_PITCH]);
valid_section &= jitsuyo::assign_val(init_angles_section, "right_ankle_roll", inital_joints[JointId::RIGHT_ANKLE_ROLL]);
valid_section &= jitsuyo::assign_val(init_angles_section, "left_hip_yaw", inital_joints[JointId::LEFT_HIP_YAW]);
valid_section &= jitsuyo::assign_val(init_angles_section, "left_hip_pitch", inital_joints[JointId::LEFT_HIP_PITCH]);
valid_section &= jitsuyo::assign_val(init_angles_section, "left_hip_roll", inital_joints[JointId::LEFT_HIP_ROLL]);
valid_section &= jitsuyo::assign_val(init_angles_section, "left_knee", inital_joints[JointId::LEFT_KNEE]);
valid_section &= jitsuyo::assign_val(init_angles_section, "left_ankle_pitch", inital_joints[JointId::LEFT_ANKLE_PITCH]);
valid_section &= jitsuyo::assign_val(init_angles_section, "left_ankle_roll", inital_joints[JointId::LEFT_ANKLE_ROLL]);
valid_section &= jitsuyo::assign_val(init_angles_section, "right_shoulder_pitch", inital_joints[JointId::RIGHT_SHOULDER_PITCH]);
valid_section &= jitsuyo::assign_val(init_angles_section, "right_shoulder_roll", inital_joints[JointId::RIGHT_SHOULDER_ROLL]);
valid_section &= jitsuyo::assign_val(init_angles_section, "right_elbow", inital_joints[JointId::RIGHT_ELBOW]);
valid_section &= jitsuyo::assign_val(init_angles_section, "left_shoulder_pitch", inital_joints[JointId::LEFT_SHOULDER_PITCH]);
valid_section &= jitsuyo::assign_val(init_angles_section, "left_shoulder_roll", inital_joints[JointId::LEFT_SHOULDER_ROLL]);
valid_section &= jitsuyo::assign_val(init_angles_section, "left_elbow", inital_joints[JointId::LEFT_ELBOW]);

if (!valid_section) {
std::cout << "Error found at section `init_angles`" << std::endl;
valid_config = false;
}
} else {
valid_config = false;
}

nlohmann::json angles_direction_section;
if (jitsuyo::assign_val(walking_data, "angles_direction", angles_direction_section)) {
bool valid_section = true;

using tachimawari::joint::JointId;

valid_section &= jitsuyo::assign_val(angles_direction_section, "right_hip_yaw", joints_direction[JointId::RIGHT_HIP_YAW]);
valid_section &= jitsuyo::assign_val(angles_direction_section, "right_hip_pitch", joints_direction[JointId::RIGHT_HIP_PITCH]);
valid_section &= jitsuyo::assign_val(angles_direction_section, "right_hip_roll", joints_direction[JointId::RIGHT_HIP_ROLL]);
valid_section &= jitsuyo::assign_val(angles_direction_section, "right_knee", joints_direction[JointId::RIGHT_KNEE]);
valid_section &= jitsuyo::assign_val(angles_direction_section, "right_ankle_pitch", joints_direction[JointId::RIGHT_ANKLE_PITCH]);
valid_section &= jitsuyo::assign_val(angles_direction_section, "right_ankle_roll", joints_direction[JointId::RIGHT_ANKLE_ROLL]);
valid_section &= jitsuyo::assign_val(angles_direction_section, "left_hip_yaw", joints_direction[JointId::LEFT_HIP_YAW]);
valid_section &= jitsuyo::assign_val(angles_direction_section, "left_hip_pitch", joints_direction[JointId::LEFT_HIP_PITCH]);
valid_section &= jitsuyo::assign_val(angles_direction_section, "left_hip_roll", joints_direction[JointId::LEFT_HIP_ROLL]);
valid_section &= jitsuyo::assign_val(angles_direction_section, "left_knee", joints_direction[JointId::LEFT_KNEE]);
valid_section &= jitsuyo::assign_val(angles_direction_section, "left_ankle_pitch", joints_direction[JointId::LEFT_ANKLE_PITCH]);
valid_section &= jitsuyo::assign_val(angles_direction_section, "left_ankle_roll", joints_direction[JointId::LEFT_ANKLE_ROLL]);
valid_section &= jitsuyo::assign_val(angles_direction_section, "right_shoulder_pitch", joints_direction[JointId::RIGHT_SHOULDER_PITCH]);
valid_section &= jitsuyo::assign_val(angles_direction_section, "right_shoulder_roll", joints_direction[JointId::RIGHT_SHOULDER_ROLL]);
valid_section &= jitsuyo::assign_val(angles_direction_section, "right_elbow", joints_direction[JointId::RIGHT_ELBOW]);
valid_section &= jitsuyo::assign_val(angles_direction_section, "left_shoulder_pitch", joints_direction[JointId::LEFT_SHOULDER_PITCH]);
valid_section &= jitsuyo::assign_val(angles_direction_section, "left_shoulder_roll", joints_direction[JointId::LEFT_SHOULDER_ROLL]);
valid_section &= jitsuyo::assign_val(angles_direction_section, "left_elbow", joints_direction[JointId::LEFT_ELBOW]);

if (!valid_section) {
std::cout << "Error found at section `angles_direction`" << std::endl;
valid_config = false;
}
} else {
valid_config = false;
}

if (!valid_config) {
throw std::runtime_error("Failed to load config file `walking.json`");
}

kinematic.set_config(kinematic_data);
Expand Down
115 changes: 71 additions & 44 deletions src/aruku/walking/process/kinematic.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,6 +29,7 @@

#include "aruku/walking/process/kinematic.hpp"

#include "jitsuyo/config.hpp"
#include "keisan/keisan.hpp"
#include "nlohmann/json.hpp"
#include "tachimawari/joint/model/joint.hpp"
Expand Down Expand Up @@ -348,51 +349,77 @@ void Kinematic::update_move_amplitude()

void Kinematic::set_config(const nlohmann::json & kinematic_data)
{
for (auto &[key, val] : kinematic_data.items()) {
if (key == "ratio") {
try {
val.at("period_time").get_to(period_time);
val.at("dsp_ratio").get_to(dsp_ratio);
val.at("foot_height").get_to(z_move);
val.at("swing_right_left").get_to(y_swap_amplitude);
val.at("swing_up_down").get_to(z_swap_amplitude);
val.at("arm_swing_gain").get_to(arm_swing_gain);
val.at("backward_hip_comp_ratio").get_to(backward_hip_comp_ratio);
val.at("forward_hip_comp_ratio").get_to(forward_hip_comp_ratio);
val.at("foot_comp_ratio").get_to(foot_comp_ratio);
val.at("dsp_comp_ratio").get_to(dsp_comp_ratio);
val.at("period_comp_ratio").get_to(period_comp_ratio);
val.at("move_accel_ratio").get_to(move_accel_ratio);
val.at("foot_accel_ratio").get_to(foot_accel_ratio);
} catch (nlohmann::json::parse_error & ex) {
std::cerr << "parse error at byte " << ex.byte << std::endl;
throw ex;
}
} else if (key == "length") {
try {
val.at("thigh_length").get_to(thigh_length);
val.at("calf_length").get_to(calf_length);
val.at("ankle_length").get_to(ankle_length);
val.at("leg_length").get_to(leg_length);
} catch (nlohmann::json::parse_error & ex) {
std::cerr << "parse error at byte " << ex.byte << std::endl;
throw ex;
}
} else if (key == "offset") {
try {
val.at("x_offset").get_to(x_offset);
val.at("y_offset").get_to(y_offset);
val.at("z_offset").get_to(z_offset);

yaw_offset = keisan::make_degree(val.at("yaw_offset").get<double>());
roll_offset = keisan::make_degree(val.at("roll_offset").get<double>());
pitch_offset = keisan::make_degree(val.at("pitch_offset").get<double>());
hip_pitch_offset = keisan::make_degree(val.at("hip_pitch_offset").get<double>());
} catch (nlohmann::json::parse_error & ex) {
std::cerr << "parse error at byte " << ex.byte << std::endl;
throw ex;
}
bool valid_config = true;

nlohmann::json ratio_section;
if (jitsuyo::assign_val(kinematic_data, "ratio", ratio_section)) {
bool valid_section = true;
valid_section &= jitsuyo::assign_val(ratio_section, "period_time", period_time);
valid_section &= jitsuyo::assign_val(ratio_section, "dsp_ratio", dsp_ratio);
valid_section &= jitsuyo::assign_val(ratio_section, "z_move", z_move);
valid_section &= jitsuyo::assign_val(ratio_section, "y_swap_amplitude", y_swap_amplitude);
valid_section &= jitsuyo::assign_val(ratio_section, "z_swap_amplitude", z_swap_amplitude);
valid_section &= jitsuyo::assign_val(ratio_section, "arm_swing_gain", arm_swing_gain);
valid_section &= jitsuyo::assign_val(ratio_section, "backward_hip_comp_ratio", backward_hip_comp_ratio);
valid_section &= jitsuyo::assign_val(ratio_section, "forward_hip_comp_ratio", forward_hip_comp_ratio);
valid_section &= jitsuyo::assign_val(ratio_section, "foot_comp_ratio", foot_comp_ratio);
valid_section &= jitsuyo::assign_val(ratio_section, "dsp_comp_ratio", dsp_comp_ratio);
valid_section &= jitsuyo::assign_val(ratio_section, "period_comp_ratio", period_comp_ratio);
valid_section &= jitsuyo::assign_val(ratio_section, "move_accel_ratio", move_accel_ratio);
valid_section &= jitsuyo::assign_val(ratio_section, "foot_accel_ratio", foot_accel_ratio);

if (!valid_section) {
std::cout << "Error found at section `ratio`" << std::endl;
valid_config = false;
}
} else {
valid_config = false;
}

nlohmann::json length_section;
if (jitsuyo::assign_val(kinematic_data, "length", length_section)) {
bool valid_section = true;
valid_section &= jitsuyo::assign_val(length_section, "thigh_length", thigh_length);
valid_section &= jitsuyo::assign_val(length_section, "calf_length", calf_length);
valid_section &= jitsuyo::assign_val(length_section, "ankle_length", ankle_length);
valid_section &= jitsuyo::assign_val(length_section, "leg_length", leg_length);

if (!valid_section) {
std::cout << "Error found at section `length`" << std::endl;
valid_config = false;
}
} else {
valid_config = false;
}

nlohmann::json offset_section;
if (jitsuyo::assign_val(kinematic_data, "offset", offset_section)) {
bool valid_section = true;

double yaw_offset_double;
double roll_offset_double;
double pitch_offset_double;
double hip_pitch_offset_double;

valid_section &= jitsuyo::assign_val(offset_section, "x_offset", x_offset);
valid_section &= jitsuyo::assign_val(offset_section, "y_offset", y_offset);
valid_section &= jitsuyo::assign_val(offset_section, "z_offset", z_offset);
valid_section &= jitsuyo::assign_val(offset_section, "yaw_offset", yaw_offset_double);
valid_section &= jitsuyo::assign_val(offset_section, "roll_offset", roll_offset_double);
valid_section &= jitsuyo::assign_val(offset_section, "pitch_offset", pitch_offset_double);
valid_section &= jitsuyo::assign_val(offset_section, "hip_pitch_offset", hip_pitch_offset_double);

yaw_offset = keisan::make_degree(yaw_offset_double);
roll_offset = keisan::make_degree(roll_offset_double);
pitch_offset = keisan::make_degree(pitch_offset_double);
hip_pitch_offset = keisan::make_degree(hip_pitch_offset_double);

if (!valid_section) {
std::cout << "Error found at section `offset`" << std::endl;
valid_config = false;
}
} else {
valid_config = false;
}

update_times();
Expand Down

0 comments on commit 627480f

Please sign in to comment.