diff --git a/src/bitbots_misc/bitbots_extrinsic_calibration/config/amy.yaml b/src/bitbots_misc/bitbots_extrinsic_calibration/config/amy.yaml index 955e68f54..ba020af93 100644 --- a/src/bitbots_misc/bitbots_extrinsic_calibration/config/amy.yaml +++ b/src/bitbots_misc/bitbots_extrinsic_calibration/config/amy.yaml @@ -6,6 +6,6 @@ /bitbots_extrinsic_imu_calibration: ros__parameters: - offset_x: 0.295 - offset_y: 0.06 + offset_x: 0.3 + offset_y: 0.047 offset_z: 0.0 diff --git a/src/bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_engine.hpp b/src/bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_engine.hpp index 86f87e957..dbca5d796 100644 --- a/src/bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_engine.hpp +++ b/src/bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_engine.hpp @@ -81,6 +81,7 @@ class DynupEngine : public bitbots_splines::AbstractEngine walk_param_executor_; std::shared_ptr walking_param_node_; std::shared_ptr walking_param_client_; diff --git a/src/bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_ik.hpp b/src/bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_ik.hpp index 6dbdfb0ac..0a14f6f95 100644 --- a/src/bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_ik.hpp +++ b/src/bitbots_motion/bitbots_dynup/include/bitbots_dynup/dynup_ik.hpp @@ -3,11 +3,13 @@ #include #include +#include #include #include #include #include #include +#include #include #include "dynup_utils.hpp" @@ -23,13 +25,13 @@ class DynupIK : public bitbots_splines::AbstractIK { moveit::core::RobotStatePtr get_goal_state(); void set_joint_positions(sensor_msgs::msg::JointState::ConstSharedPtr joint_state); + const std::vector getLeftLegJointNames(); + const std::vector getRightLegJointNames(); + private: rclcpp::Node::SharedPtr node_; - moveit::core::JointModelGroup* all_joints_group_; moveit::core::JointModelGroup* l_arm_joints_group_; - moveit::core::JointModelGroup* l_leg_joints_group_; moveit::core::JointModelGroup* r_arm_joints_group_; - moveit::core::JointModelGroup* r_leg_joints_group_; moveit::core::RobotStatePtr goal_state_; sensor_msgs::msg::JointState::ConstSharedPtr joint_state_; DynupDirection direction_; diff --git a/src/bitbots_motion/bitbots_dynup/include/bitbots_dynup/ik.hpp b/src/bitbots_motion/bitbots_dynup/include/bitbots_dynup/ik.hpp new file mode 100644 index 000000000..de851a990 --- /dev/null +++ b/src/bitbots_motion/bitbots_dynup/include/bitbots_dynup/ik.hpp @@ -0,0 +1,380 @@ +/** + * Leg Inverse Kinematics Solver - C++ port of the Python implementation. + * + * Euler angle conventions used (all are from transforms3d, verified analytically): + * + * euler2mat / mat2euler 'sxyz': + * R = Rz(c) * Ry(b) * Rx(a) (static/extrinsic, applied x then y then z) + * Decompose: b = arcsin(-R[2,0]) + * a = arctan2( R[2,1], R[2,2]) + * c = arctan2( R[1,0], R[0,0]) + * + * euler2mat / mat2euler 'rzxy': + * R = Rz(a) * Rx(b) * Ry(c) + * Decompose: b = arcsin( R[2,1]) + * a = arctan2(-R[0,1], R[1,1]) + * c = arctan2(-R[2,0], R[2,2]) + * + * euler2mat / mat2euler 'rxyz': + * R = Rx(a) * Ry(b) * Rz(c) + * Decompose: b = arcsin( R[0,2]) + * a = arctan2(-R[1,2], R[2,2]) + * c = arctan2(-R[0,1], R[0,0]) + * + * All matrix indices are row-major (M[row][col]). + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +// ── Model constants ──────────────────────────────────────────────────────── +constexpr double UPPER_LEG_LENGTH = 0.1688; // Hip to knee [m] +constexpr double LOWER_LEG_LENGTH = 0.17; // Knee to ankle [m] +constexpr double HIP_PITCH_OFFSET = 0.023; // Hip-pitch joint offset [m] + +// Motor offsets (left leg) +constexpr double HIP_PITCH_ANGLE_OFFSET = 0.025795568467720936; +constexpr double KNEE_OFFSET = 0.26180265358979327; + +using Mat4 = Eigen::Matrix4d; +using Mat3 = Eigen::Matrix3d; +using Vec3 = Eigen::Vector3d; + +constexpr double BASE_LINK_SIDE_OFFSET = 0.055; +constexpr double BASE_LINK_Z_OFFSET = -0.0414; +constexpr double SOLE_X_OFFSET = 0.0216683; +constexpr double SOLE_Y_OFFSET = 0.0152219; +constexpr double SOLE_Z_OFFSET = -0.0529; + +// ── Custom exception ─────────────────────────────────────────────────────── +class SolverError : public std::runtime_error { + public: + explicit SolverError(const std::string& msg) : std::runtime_error(msg) {} +}; + +// ── Elementary rotation matrices ─────────────────────────────────────────── +static Mat3 Rx(double t) { + Mat3 R = Mat3::Identity(); + R(1, 1) = std::cos(t); + R(1, 2) = -std::sin(t); + R(2, 1) = std::sin(t); + R(2, 2) = std::cos(t); + return R; +} +static Mat3 Ry(double t) { + Mat3 R = Mat3::Identity(); + R(0, 0) = std::cos(t); + R(0, 2) = std::sin(t); + R(2, 0) = -std::sin(t); + R(2, 2) = std::cos(t); + return R; +} +static Mat3 Rz(double t) { + Mat3 R = Mat3::Identity(); + R(0, 0) = std::cos(t); + R(0, 1) = -std::sin(t); + R(1, 0) = std::sin(t); + R(1, 1) = std::cos(t); + return R; +} + +// ── Euler composition helpers ────────────────────────────────────────────── + +/** + * euler2mat 'sxyz'(a, b, c) = Rz(c) * Ry(b) * Rx(a) + */ +static Mat3 euler2mat_sxyz(double a, double b, double c) { return Rz(c) * Ry(b) * Rx(a); } + +/** + * mat2euler 'sxyz'(M) → (a, b, c) such that M = Rz(c)*Ry(b)*Rx(a) + * b = arcsin(-M[2,0]) + * a = arctan2( M[2,1], M[2,2]) + * c = arctan2( M[1,0], M[0,0]) + */ +static std::array mat2euler_sxyz(const Mat3& M) { + double b = std::asin(std::clamp(-M(2, 0), -1.0, 1.0)); + double a = std::atan2(M(2, 1), M(2, 2)); + double c = std::atan2(M(1, 0), M(0, 0)); + return {a, b, c}; +} + +/** + * euler2mat 'rzxy'(a, b, c) = Rz(a) * Rx(b) * Ry(c) + */ +static Mat3 euler2mat_rzxy(double a, double b, double c) { return Rz(a) * Rx(b) * Ry(c); } + +/** + * mat2euler 'rzxy'(M) → (a, b, c) such that M = Rz(a)*Rx(b)*Ry(c) + * b = arcsin( M[2,1]) + * a = arctan2(-M[0,1], M[1,1]) + * c = arctan2(-M[2,0], M[2,2]) + */ +static std::array mat2euler_rzxy(const Mat3& M) { + double b = std::asin(std::clamp(M(2, 1), -1.0, 1.0)); + double a = std::atan2(-M(0, 1), M(1, 1)); + double c = std::atan2(-M(2, 0), M(2, 2)); + return {a, b, c}; +} + +/** + * euler2mat 'rxyz'(a, b, c) = Rx(a) * Ry(b) * Rz(c) + */ +static Mat3 euler2mat_rxyz(double a, double b, double c) { return Rx(a) * Ry(b) * Rz(c); } + +/** + * mat2euler 'rxyz'(M) → (a, b, c) such that M = Rx(a)*Ry(b)*Rz(c) + * b = arcsin( M[0,2]) + * a = arctan2(-M[1,2], M[2,2]) + * c = arctan2(-M[0,1], M[0,0]) + */ +static std::array mat2euler_rxyz(const Mat3& M) { + double b = std::asin(std::clamp(M(0, 2), -1.0, 1.0)); + double a = std::atan2(-M(1, 2), M(2, 2)); + double c = std::atan2(-M(0, 1), M(0, 0)); + return {a, b, c}; +} + +// ── axangle2mat ──────────────────────────────────────────────────────────── +/** + * Rotation matrix from axis-angle (axis need not be unit; if zero vector, identity). + */ +static Mat3 axangle2mat(Vec3 axis, double angle) { + double n = axis.norm(); + if (n < 1e-12) return Mat3::Identity(); + axis /= n; + // Rodrigues' formula + Mat3 K; + // std::cout << K << 0, -axis(2), axis(1), + // axis(2), 0, -axis(0), + // -axis(1), axis(0), 0; + return Mat3::Identity() + std::sin(angle) * K + (1.0 - std::cos(angle)) * K * K; +} + +// ── foot_to_leg_from_vec_subproblem ──────────────────────────────────────── +/** + * Returns a 4×4 transform T whose rotation aligns a reference frame to + * the given leg_vector direction. + * + * Mirrors the Python: + * roll = arctan2(v[1], v[2]) + * pitch = arctan2(-v[0], sqrt(v[1]²+v[2]²)) + * yaw = 0 + * R = euler2mat(roll, pitch, yaw, 'sxyz') = Rz(0)*Ry(pitch)*Rx(roll) + * = Ry(pitch)*Rx(roll) + */ +static Mat4 foot_to_leg_from_vec_subproblem(const Vec3& leg_vector_in) { + Vec3 v = leg_vector_in.normalized(); + + if (std::abs(v(0)) > 0.99 || std::abs(v(1)) > 0.99) { + throw SolverError("Leg vector is too close to the x or y axis, cannot determine roll and pitch."); + } + + double roll = std::atan2(v(1), v(2)); + double pitch = std::atan2(-v(0), std::sqrt(v(1) * v(1) + v(2) * v(2))); + double yaw = 0.0; + + Mat4 T = Mat4::Identity(); + T.block<3, 3>(0, 0) = euler2mat_sxyz(roll, pitch, yaw); + return T; +} + +// ── Joint angle map type ─────────────────────────────────────────────────── +using JointAngles = std::map; + +// ── Forward kinematics ───────────────────────────────────────────────────── +/** + * Mirrors calculate_fk() exactly. + * Joint order (all around standard axes): + * HipYaw → Rz + * HipRoll → Rx + * HipPitch → Ry (+ translation HIP_PITCH_OFFSET along local x) + * Knee→ Ry (+ translation -UPPER_LEG_LENGTH along local z) + * AnklePitch→Ry (+ translation -LOWER_LEG_LENGTH along local z) + * AnkleRoll→ Rx + */ +static Mat4 calculate_fk(const JointAngles& q) { + auto make_T = [](const Mat3& R, const Vec3& t = Vec3::Zero()) { + Mat4 T = Mat4::Identity(); + T.block<3, 3>(0, 0) = R; + T.block<3, 1>(0, 3) = t; + return T; + }; + + Mat4 T = Mat4::Identity(); + + // Base link transform (translation from base_link to hip axis intersection) + T = T * make_T(Mat3::Identity(), {BASE_LINK_SIDE_OFFSET, BASE_LINK_Z_OFFSET, 0}); + + // HipYaw: rotation about Z + T = T * make_T(axangle2mat({0, 0, 1}, q.at("HipYaw"))); + + // HipRoll: rotation about X + T = T * make_T(axangle2mat({1, 0, 0}, q.at("HipRoll"))); + + // HipPitch: rotation about Y + offset along x + T = T * make_T(axangle2mat({0, 1, 0}, q.at("HipPitch")), {HIP_PITCH_OFFSET, 0, 0}); + + // Knee: rotation about Y + translation down z + T = T * make_T(axangle2mat({0, 1, 0}, q.at("Knee")), {0, 0, -UPPER_LEG_LENGTH}); + + // AnklePitch: rotation about Y + translation down z + T = T * make_T(axangle2mat({0, 1, 0}, q.at("AnklePitch")), {0, 0, -LOWER_LEG_LENGTH}); + + // AnkleRoll: rotation about X + T = T * make_T(axangle2mat({1, 0, 0}, q.at("AnkleRoll"))); + + // End effector offset + T = T * make_T(Mat3::Identity(), {SOLE_X_OFFSET, SOLE_Y_OFFSET, SOLE_Z_OFFSET}); + + return T; +} + +// ── Inverse kinematics ───────────────────────────────────────────────────── +static JointAngles calculate_ik(const Mat4& target_transform, bool left) { + JointAngles joint_angles = {{"HipYaw", 0.0}, {"HipRoll", 0.0}, {"HipPitch", 0.0}, + {"Knee", 0.0}, {"AnklePitch", 0.0}, {"AnkleRoll", 0.0}}; + + // std::cout << "Target transformation matrix:\n" << target_transform << "\n"; + + auto make_T = [](const Mat3& R, const Vec3& t = Vec3::Zero()) { + Mat4 T = Mat4::Identity(); + T.block<3, 3>(0, 0) = R; + T.block<3, 1>(0, 3) = t; + return T; + }; + + // Remove the ends of the kinematic chain up to the first and last joint + Mat4 T_ankle_to_sole, T_base_link_to_hip_intersection; + + if (left) { + T_ankle_to_sole = make_T(Mat3::Identity(), {SOLE_X_OFFSET, SOLE_Y_OFFSET, SOLE_Z_OFFSET}); + T_base_link_to_hip_intersection = make_T(Mat3::Identity(), {0, BASE_LINK_SIDE_OFFSET, BASE_LINK_Z_OFFSET}); + } else { + T_ankle_to_sole = make_T(Mat3::Identity(), {SOLE_X_OFFSET, -SOLE_Y_OFFSET, SOLE_Z_OFFSET}); + T_base_link_to_hip_intersection = make_T(Mat3::Identity(), {0, -BASE_LINK_SIDE_OFFSET, BASE_LINK_Z_OFFSET}); + } + + auto target_transform_leg_only = + (T_base_link_to_hip_intersection.inverse() * target_transform) * T_ankle_to_sole.inverse(); + + // std::cout << "Target transformation matrix (leg only):\n" << target_transform_leg_only << "\n"; + + // Axis-intersection transforms (identity = at origin) + Mat4 T_hip_axis_intersection = Mat4::Identity(); + Mat4 T_ankle_axis_intersection = target_transform_leg_only; + + // ankle-to-hip transform + Mat4 T_ankle_to_hip = T_ankle_axis_intersection.inverse() * T_hip_axis_intersection; + // std::cout << "Ankle to hip:\n" << T_ankle_to_hip << "\n"; + + // ankle-to-leg alignment (inverse of foot_to_leg subproblem) + Vec3 leg_vec = T_ankle_to_hip.block<3, 1>(0, 3); + Mat4 T_ankle_to_leg = foot_to_leg_from_vec_subproblem(leg_vec).inverse(); + + // hip-to-leg alignment + Mat4 T_hip_to_leg = T_ankle_to_hip.inverse() * T_ankle_to_leg; + // std::cout << "Hip to leg:\n" << T_hip_to_leg << "\n"; + + // HipYaw and HipRoll from rzxy decomposition: M = Rz(yaw)*Rx(roll)*Ry(pitch) + auto hip_euler = mat2euler_rzxy(T_hip_to_leg.block<3, 3>(0, 0)); + joint_angles["HipYaw"] = hip_euler[0]; + joint_angles["HipRoll"] = hip_euler[1]; + // hip_euler[2] would be HipPitch, but we compute it properly below + // std::cout << "Hip angles (spherical): " << joint_angles["HipYaw"] + // << ", " << joint_angles["HipRoll"] << "\n"; + + // Virtual leg length (distance from ankle axis intersection to hip axis intersection) + // double virtual_leg_length = leg_vec.norm(); + // std::cout << "Leg length: " << virtual_leg_length << "\n"; + + // Hip transform without pitch component + Mat4 T_hip_without_pitch = Mat4::Identity(); + T_hip_without_pitch.block<3, 3>(0, 0) = euler2mat_rzxy(joint_angles["HipYaw"], joint_angles["HipRoll"], 0.0); + + // Hip pitch offset transform + Mat4 T_hip_pitch_offset = Mat4::Identity(); + T_hip_pitch_offset.block<3, 1>(0, 3) = Vec3(HIP_PITCH_OFFSET, 0, 0); + + Mat4 T_hip_pitch_origin = T_hip_without_pitch * T_hip_pitch_offset; + + // Real leg: from ankle to hip-pitch origin + Mat4 T_real_leg = T_ankle_axis_intersection.inverse() * T_hip_axis_intersection * T_hip_pitch_origin; + // std::cout << "Real leg transformation matrix:\n" << T_real_leg << "\n"; + + double real_leg_length = T_real_leg.block<3, 1>(0, 3).norm(); + // std::cout << "Real leg length: " << real_leg_length << "\n"; + + // Ankle angles from real-leg direction + Vec3 real_leg_vec = T_real_leg.block<3, 1>(0, 3); + Mat4 T_ankle_to_real_leg = foot_to_leg_from_vec_subproblem(real_leg_vec).inverse(); + + // mat2euler 'rxyz': M = Rx(a)*Ry(b)*Rz(c) + auto ankle_euler = mat2euler_rxyz(T_ankle_to_real_leg.block<3, 3>(0, 0)); + joint_angles["AnkleRoll"] = -ankle_euler[0]; + joint_angles["AnklePitch"] = -ankle_euler[1]; + // std::cout << "Ankle to real leg: " << joint_angles["AnkleRoll"] + // << ", " << joint_angles["AnklePitch"] << "\n"; + + // Knee from cosine rule + double knee_arg = + (real_leg_length * real_leg_length - UPPER_LEG_LENGTH * UPPER_LEG_LENGTH - LOWER_LEG_LENGTH * LOWER_LEG_LENGTH) / + (2.0 * UPPER_LEG_LENGTH * LOWER_LEG_LENGTH); + joint_angles["Knee"] = std::acos(std::clamp(knee_arg, -1.0, 1.0)); + + // HipPitch base from rzxy[2] + Mat4 T_hip_to_leg_real = T_real_leg.inverse() * T_ankle_to_real_leg; + auto hip_euler_real = mat2euler_rzxy(T_hip_to_leg_real.block<3, 3>(0, 0)); + double hip_pitch_base = hip_euler_real[2]; + + // Hip pitch offset (cosine rule) + double hip_pitch_offset_arg = + (UPPER_LEG_LENGTH * UPPER_LEG_LENGTH + real_leg_length * real_leg_length - LOWER_LEG_LENGTH * LOWER_LEG_LENGTH) / + (2.0 * UPPER_LEG_LENGTH * real_leg_length); + double hip_pitch_offset_val = std::acos(std::clamp(hip_pitch_offset_arg, -1.0, 1.0)); + // std::cout << "Hip pitch offset: " << hip_pitch_offset_val << "\n"; + + // Ankle pitch offset (cosine rule) + double ankle_pitch_offset_arg = + (LOWER_LEG_LENGTH * LOWER_LEG_LENGTH + real_leg_length * real_leg_length - UPPER_LEG_LENGTH * UPPER_LEG_LENGTH) / + (2.0 * LOWER_LEG_LENGTH * real_leg_length); + double ankle_pitch_offset_val = std::acos(std::clamp(ankle_pitch_offset_arg, -1.0, 1.0)); + // std::cout << "Ankle pitch offset: " << ankle_pitch_offset_val << "\n"; + + // Solution i=0 (matches Python solutions[0]) + joint_angles["HipPitch"] = hip_pitch_base - hip_pitch_offset_val; + joint_angles["AnklePitch"] += -ankle_pitch_offset_val; // i=0: subtract + + return joint_angles; +} + +static JointAngles calculate_ik_right_leg(const Mat4& target_transform) { + auto ik_solution = calculate_ik(target_transform, false); + + ik_solution["HipYaw"] *= -1; + ik_solution["HipPitch"] -= HIP_PITCH_ANGLE_OFFSET; + ik_solution["Knee"] += KNEE_OFFSET; + ik_solution["Knee"] *= -1; + ik_solution["AnklePitch"] *= -1; + ik_solution["AnkleRoll"] *= -1; + + return ik_solution; +} + +static JointAngles calculate_ik_left_leg(const Mat4& target_transform) { + auto ik_solution = calculate_ik(target_transform, true); + + ik_solution["HipYaw"] *= -1; + ik_solution["HipPitch"] *= -1; + ik_solution["AnkleRoll"] *= -1; + ik_solution["HipPitch"] += HIP_PITCH_ANGLE_OFFSET; + ik_solution["Knee"] += KNEE_OFFSET; + + return ik_solution; +} diff --git a/src/bitbots_motion/bitbots_dynup/src/dynup_engine.cpp b/src/bitbots_motion/bitbots_dynup/src/dynup_engine.cpp index 407f089c0..e6e312262 100644 --- a/src/bitbots_motion/bitbots_dynup/src/dynup_engine.cpp +++ b/src/bitbots_motion/bitbots_dynup/src/dynup_engine.cpp @@ -12,8 +12,11 @@ DynupEngine::DynupEngine(rclcpp::Node::SharedPtr node, bitbots_dynup::Params::En pub_debug_marker_ = node_->create_publisher("dynup_engine_marker", 1); // We need a separate node for the parameter client because the parameter client adds the node to an executor // and our dynup node is already added to an executor + + walk_param_executor_ = std::make_shared(); walking_param_node_ = std::make_shared(std::string(node->get_name()) + "_walking_param_node"); - walking_param_client_ = std::make_shared(walking_param_node_, "/walking"); + walking_param_client_ = + std::make_shared(walk_param_executor_, walking_param_node_, "/walking"); } void DynupEngine::init(double arm_offset_y, double arm_offset_z) { @@ -660,11 +663,16 @@ void DynupEngine::setGoals(const DynupRequest& goals) { // get parameters from walking. If walking is not running, use default values // we re-request the values every time because they can be changed by dynamic reconfigure // and re-requesting them is fast enough - std::vector walking_params; - // Get params and wait for walking to be ready - walking_params = walking_param_client_->get_parameters( - {"engine.trunk_pitch", "engine.trunk_height", "engine.foot_distance", "engine.trunk_x_offset"}, - std::chrono::seconds(5)); + // We need to do this in another thread, because the sync parameter client runs spin_until_future_complete, + // which is not allowed to be called inside of another callback recusively. + // It should however use a different executor from the beginning, + // but somehow this does not work. + std::vector walking_params = + std::async(std::launch::async, [&]() { + return walking_param_client_->get_parameters( + {"engine.trunk_pitch", "engine.trunk_height", "engine.foot_distance", "engine.trunk_x_offset"}, + std::chrono::seconds(15)); + }).get(); // when the walking was killed, service_is_ready is still true but the parameters come back empty if (walking_params.size() != 4) { diff --git a/src/bitbots_motion/bitbots_dynup/src/dynup_ik.cpp b/src/bitbots_motion/bitbots_dynup/src/dynup_ik.cpp index 0511f7f8e..0f3e9f336 100644 --- a/src/bitbots_motion/bitbots_dynup/src/dynup_ik.cpp +++ b/src/bitbots_motion/bitbots_dynup/src/dynup_ik.cpp @@ -6,11 +6,8 @@ DynupIK::DynupIK(rclcpp::Node::SharedPtr node) : node_(node) {} void DynupIK::init(moveit::core::RobotModelPtr kinematic_model) { /* Extract joint groups from kinematics model */ - l_leg_joints_group_ = kinematic_model->getJointModelGroup("LeftLeg"); - r_leg_joints_group_ = kinematic_model->getJointModelGroup("RightLeg"); l_arm_joints_group_ = kinematic_model->getJointModelGroup("LeftArm"); r_arm_joints_group_ = kinematic_model->getJointModelGroup("RightArm"); - all_joints_group_ = kinematic_model->getJointModelGroup("All"); /* Reset kinematic goal to default */ goal_state_ = std::make_shared(kinematic_model); @@ -45,30 +42,64 @@ bitbots_splines::JointGoals DynupIK::calculate(const DynupResponse& ik_goals) { kinematics::KinematicsQueryOptions ik_options; ik_options.return_approximate_solution = true; - geometry_msgs::msg::Pose right_foot_goal_msg, left_foot_goal_msg, right_hand_goal_msg, left_hand_goal_msg; + geometry_msgs::msg::Transform left_foot_goal_tf, right_foot_goal_tf; + + auto transform_to_geo_tf = [](const tf2::Transform& transform) { + geometry_msgs::msg::Transform msg; + msg.translation.x = transform.getOrigin().x(); + msg.translation.y = transform.getOrigin().y(); + msg.translation.z = transform.getOrigin().z(); + msg.rotation.x = transform.getRotation().x(); + msg.rotation.y = transform.getRotation().y(); + msg.rotation.z = transform.getRotation().z(); + msg.rotation.w = transform.getRotation().w(); + return msg; + }; + + right_foot_goal_tf = transform_to_geo_tf(ik_goals.r_foot_goal_pose); + left_foot_goal_tf = transform_to_geo_tf(ik_goals.l_foot_goal_pose); + + geometry_msgs::msg::Pose right_hand_goal_msg, left_hand_goal_msg; - tf2::toMsg(ik_goals.r_foot_goal_pose, right_foot_goal_msg); - tf2::toMsg(ik_goals.l_foot_goal_pose, left_foot_goal_msg); tf2::toMsg(ik_goals.r_hand_goal_pose, right_hand_goal_msg); tf2::toMsg(ik_goals.l_hand_goal_pose, left_hand_goal_msg); - bool success; + bool success = true; goal_state_->updateLinkTransforms(); + // Kann man den einfach löschen? Scheint ja nciht so benutzt zu werden + bio_ik::BioIKKinematicsQueryOptions leg_ik_options; leg_ik_options.return_approximate_solution = true; // Add auxiliary goal to prevent bending the knees in the wrong direction when we go from init to walkready leg_ik_options.goals.push_back(std::make_unique()); - success = goal_state_->setFromIK(l_leg_joints_group_, left_foot_goal_msg, 0.005, - moveit::core::GroupStateValidityCallbackFn(), leg_ik_options); + // here udpate to analytic IK, feet are set here + bitbots_splines::JointGoals result; - goal_state_->updateLinkTransforms(); + // call IK + Eigen::Isometry3d right_iso = tf2::transformToEigen(right_foot_goal_tf); + Mat4 T_right = right_iso.matrix(); + auto result_right = calculate_ik_right_leg(T_right); - success &= goal_state_->setFromIK(r_leg_joints_group_, right_foot_goal_msg, 0.005, - moveit::core::GroupStateValidityCallbackFn(), leg_ik_options); + for (const auto& joint : getRightLegJointNames()) { + result.first.push_back(joint); + result.second.push_back( + result_right[joint.substr(1)]); // remove the "R" from the joint name to get the corresponding left joint + } + + Eigen::Isometry3d left_iso = tf2::transformToEigen(left_foot_goal_tf); + Mat4 T_left = left_iso.matrix(); + auto result_left = calculate_ik_left_leg(T_left); + for (const auto& joint : getLeftLegJointNames()) { + result.first.push_back(joint); + result.second.push_back( + result_left[joint.substr(1)]); // remove the "R" from the joint name to get the corresponding right joint + } + + // arms if (direction_ != DynupDirection::RISE_NO_ARMS and direction_ != DynupDirection::DESCEND_NO_ARMS) { goal_state_->updateLinkTransforms(); success &= goal_state_->setFromIK(l_arm_joints_group_, left_hand_goal_msg, 0.005, @@ -79,19 +110,31 @@ bitbots_splines::JointGoals DynupIK::calculate(const DynupResponse& ik_goals) { moveit::core::GroupStateValidityCallbackFn(), ik_options); } if (success) { - /* retrieve joint names and associated positions from */ - auto joint_names = all_joints_group_->getActiveJointModelNames(); - std::vector joint_goals; - goal_state_->copyJointGroupPositions(all_joints_group_, joint_goals); - - /* construct result object */ - bitbots_splines::JointGoals result = {joint_names, joint_goals}; - // Store the name of the arm joins so we can remove them if they are not needed const auto r_arm_motors = r_arm_joints_group_->getActiveJointModelNames(); const auto l_arm_motors = l_arm_joints_group_->getActiveJointModelNames(); - /* sets head motors to correct positions, as the IK will return random values for those unconstrained motors. */ + // append right arm + for (const auto& joint : r_arm_motors) { + double value = goal_state_->getVariablePosition(joint); + result.first.push_back(joint); + result.second.push_back(value); + } + + // append left arm + for (const auto& joint : l_arm_motors) { + double value = goal_state_->getVariablePosition(joint); + result.first.push_back(joint); + result.second.push_back(value); + } + + // head joints + result.first.push_back("HeadPan"); + result.second.push_back(goal_state_->getVariablePosition("HeadPan")); + + result.first.push_back("HeadTilt"); + result.second.push_back(goal_state_->getVariablePosition("HeadTilt")); + for (size_t i = result.first.size(); i-- > 0;) { if (result.first[i] == "HeadPan") { if (direction_ == DynupDirection::WALKREADY) { @@ -130,7 +173,13 @@ bitbots_splines::JointGoals DynupIK::calculate(const DynupResponse& ik_goals) { return {}; } } +const std::vector DynupIK::getLeftLegJointNames() { + return {"LHipYaw", "LHipRoll", "LHipPitch", "LKnee", "LAnklePitch", "LAnkleRoll"}; +} +const std::vector DynupIK::getRightLegJointNames() { + return {"RHipYaw", "RHipRoll", "RHipPitch", "RKnee", "RAnklePitch", "RAnkleRoll"}; +} moveit::core::RobotStatePtr DynupIK::get_goal_state() { return goal_state_; } void DynupIK::set_joint_positions(sensor_msgs::msg::JointState::ConstSharedPtr joint_state) { diff --git a/src/bitbots_motion/bitbots_quintic_walk/CMakeLists.txt b/src/bitbots_motion/bitbots_quintic_walk/CMakeLists.txt index 91119a9c4..4ca1b1ba7 100644 --- a/src/bitbots_motion/bitbots_quintic_walk/CMakeLists.txt +++ b/src/bitbots_motion/bitbots_quintic_walk/CMakeLists.txt @@ -56,7 +56,7 @@ rosidl_generate_interfaces( include_directories(include ${PYTHON_INCLUDE_DIRS}) -add_compile_options(-Wall -Werror -Wno-unused -Wextra -Wpedantic) +add_compile_options(-Wall -Werror -Wno-unused -Wextra -Wpedantic -Wfatal-errors) set(SOURCES src/walk_visualizer.cpp src/walk_engine.cpp src/walk_stabilizer.cpp src/walk_ik.cpp src/walk_node.cpp) diff --git a/src/bitbots_motion/bitbots_quintic_walk/config/robots/rory.yaml b/src/bitbots_motion/bitbots_quintic_walk/config/robots/rory.yaml index 7c48aea0f..572e7ad8b 100644 --- a/src/bitbots_motion/bitbots_quintic_walk/config/robots/rory.yaml +++ b/src/bitbots_motion/bitbots_quintic_walk/config/robots/rory.yaml @@ -3,5 +3,4 @@ walking: ros__parameters: engine: - trunk_x_offset: -0.005 - + trunk_x_offset: -0.01 diff --git a/src/bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/ik.hpp b/src/bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/ik.hpp new file mode 100644 index 000000000..de851a990 --- /dev/null +++ b/src/bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/ik.hpp @@ -0,0 +1,380 @@ +/** + * Leg Inverse Kinematics Solver - C++ port of the Python implementation. + * + * Euler angle conventions used (all are from transforms3d, verified analytically): + * + * euler2mat / mat2euler 'sxyz': + * R = Rz(c) * Ry(b) * Rx(a) (static/extrinsic, applied x then y then z) + * Decompose: b = arcsin(-R[2,0]) + * a = arctan2( R[2,1], R[2,2]) + * c = arctan2( R[1,0], R[0,0]) + * + * euler2mat / mat2euler 'rzxy': + * R = Rz(a) * Rx(b) * Ry(c) + * Decompose: b = arcsin( R[2,1]) + * a = arctan2(-R[0,1], R[1,1]) + * c = arctan2(-R[2,0], R[2,2]) + * + * euler2mat / mat2euler 'rxyz': + * R = Rx(a) * Ry(b) * Rz(c) + * Decompose: b = arcsin( R[0,2]) + * a = arctan2(-R[1,2], R[2,2]) + * c = arctan2(-R[0,1], R[0,0]) + * + * All matrix indices are row-major (M[row][col]). + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +// ── Model constants ──────────────────────────────────────────────────────── +constexpr double UPPER_LEG_LENGTH = 0.1688; // Hip to knee [m] +constexpr double LOWER_LEG_LENGTH = 0.17; // Knee to ankle [m] +constexpr double HIP_PITCH_OFFSET = 0.023; // Hip-pitch joint offset [m] + +// Motor offsets (left leg) +constexpr double HIP_PITCH_ANGLE_OFFSET = 0.025795568467720936; +constexpr double KNEE_OFFSET = 0.26180265358979327; + +using Mat4 = Eigen::Matrix4d; +using Mat3 = Eigen::Matrix3d; +using Vec3 = Eigen::Vector3d; + +constexpr double BASE_LINK_SIDE_OFFSET = 0.055; +constexpr double BASE_LINK_Z_OFFSET = -0.0414; +constexpr double SOLE_X_OFFSET = 0.0216683; +constexpr double SOLE_Y_OFFSET = 0.0152219; +constexpr double SOLE_Z_OFFSET = -0.0529; + +// ── Custom exception ─────────────────────────────────────────────────────── +class SolverError : public std::runtime_error { + public: + explicit SolverError(const std::string& msg) : std::runtime_error(msg) {} +}; + +// ── Elementary rotation matrices ─────────────────────────────────────────── +static Mat3 Rx(double t) { + Mat3 R = Mat3::Identity(); + R(1, 1) = std::cos(t); + R(1, 2) = -std::sin(t); + R(2, 1) = std::sin(t); + R(2, 2) = std::cos(t); + return R; +} +static Mat3 Ry(double t) { + Mat3 R = Mat3::Identity(); + R(0, 0) = std::cos(t); + R(0, 2) = std::sin(t); + R(2, 0) = -std::sin(t); + R(2, 2) = std::cos(t); + return R; +} +static Mat3 Rz(double t) { + Mat3 R = Mat3::Identity(); + R(0, 0) = std::cos(t); + R(0, 1) = -std::sin(t); + R(1, 0) = std::sin(t); + R(1, 1) = std::cos(t); + return R; +} + +// ── Euler composition helpers ────────────────────────────────────────────── + +/** + * euler2mat 'sxyz'(a, b, c) = Rz(c) * Ry(b) * Rx(a) + */ +static Mat3 euler2mat_sxyz(double a, double b, double c) { return Rz(c) * Ry(b) * Rx(a); } + +/** + * mat2euler 'sxyz'(M) → (a, b, c) such that M = Rz(c)*Ry(b)*Rx(a) + * b = arcsin(-M[2,0]) + * a = arctan2( M[2,1], M[2,2]) + * c = arctan2( M[1,0], M[0,0]) + */ +static std::array mat2euler_sxyz(const Mat3& M) { + double b = std::asin(std::clamp(-M(2, 0), -1.0, 1.0)); + double a = std::atan2(M(2, 1), M(2, 2)); + double c = std::atan2(M(1, 0), M(0, 0)); + return {a, b, c}; +} + +/** + * euler2mat 'rzxy'(a, b, c) = Rz(a) * Rx(b) * Ry(c) + */ +static Mat3 euler2mat_rzxy(double a, double b, double c) { return Rz(a) * Rx(b) * Ry(c); } + +/** + * mat2euler 'rzxy'(M) → (a, b, c) such that M = Rz(a)*Rx(b)*Ry(c) + * b = arcsin( M[2,1]) + * a = arctan2(-M[0,1], M[1,1]) + * c = arctan2(-M[2,0], M[2,2]) + */ +static std::array mat2euler_rzxy(const Mat3& M) { + double b = std::asin(std::clamp(M(2, 1), -1.0, 1.0)); + double a = std::atan2(-M(0, 1), M(1, 1)); + double c = std::atan2(-M(2, 0), M(2, 2)); + return {a, b, c}; +} + +/** + * euler2mat 'rxyz'(a, b, c) = Rx(a) * Ry(b) * Rz(c) + */ +static Mat3 euler2mat_rxyz(double a, double b, double c) { return Rx(a) * Ry(b) * Rz(c); } + +/** + * mat2euler 'rxyz'(M) → (a, b, c) such that M = Rx(a)*Ry(b)*Rz(c) + * b = arcsin( M[0,2]) + * a = arctan2(-M[1,2], M[2,2]) + * c = arctan2(-M[0,1], M[0,0]) + */ +static std::array mat2euler_rxyz(const Mat3& M) { + double b = std::asin(std::clamp(M(0, 2), -1.0, 1.0)); + double a = std::atan2(-M(1, 2), M(2, 2)); + double c = std::atan2(-M(0, 1), M(0, 0)); + return {a, b, c}; +} + +// ── axangle2mat ──────────────────────────────────────────────────────────── +/** + * Rotation matrix from axis-angle (axis need not be unit; if zero vector, identity). + */ +static Mat3 axangle2mat(Vec3 axis, double angle) { + double n = axis.norm(); + if (n < 1e-12) return Mat3::Identity(); + axis /= n; + // Rodrigues' formula + Mat3 K; + // std::cout << K << 0, -axis(2), axis(1), + // axis(2), 0, -axis(0), + // -axis(1), axis(0), 0; + return Mat3::Identity() + std::sin(angle) * K + (1.0 - std::cos(angle)) * K * K; +} + +// ── foot_to_leg_from_vec_subproblem ──────────────────────────────────────── +/** + * Returns a 4×4 transform T whose rotation aligns a reference frame to + * the given leg_vector direction. + * + * Mirrors the Python: + * roll = arctan2(v[1], v[2]) + * pitch = arctan2(-v[0], sqrt(v[1]²+v[2]²)) + * yaw = 0 + * R = euler2mat(roll, pitch, yaw, 'sxyz') = Rz(0)*Ry(pitch)*Rx(roll) + * = Ry(pitch)*Rx(roll) + */ +static Mat4 foot_to_leg_from_vec_subproblem(const Vec3& leg_vector_in) { + Vec3 v = leg_vector_in.normalized(); + + if (std::abs(v(0)) > 0.99 || std::abs(v(1)) > 0.99) { + throw SolverError("Leg vector is too close to the x or y axis, cannot determine roll and pitch."); + } + + double roll = std::atan2(v(1), v(2)); + double pitch = std::atan2(-v(0), std::sqrt(v(1) * v(1) + v(2) * v(2))); + double yaw = 0.0; + + Mat4 T = Mat4::Identity(); + T.block<3, 3>(0, 0) = euler2mat_sxyz(roll, pitch, yaw); + return T; +} + +// ── Joint angle map type ─────────────────────────────────────────────────── +using JointAngles = std::map; + +// ── Forward kinematics ───────────────────────────────────────────────────── +/** + * Mirrors calculate_fk() exactly. + * Joint order (all around standard axes): + * HipYaw → Rz + * HipRoll → Rx + * HipPitch → Ry (+ translation HIP_PITCH_OFFSET along local x) + * Knee→ Ry (+ translation -UPPER_LEG_LENGTH along local z) + * AnklePitch→Ry (+ translation -LOWER_LEG_LENGTH along local z) + * AnkleRoll→ Rx + */ +static Mat4 calculate_fk(const JointAngles& q) { + auto make_T = [](const Mat3& R, const Vec3& t = Vec3::Zero()) { + Mat4 T = Mat4::Identity(); + T.block<3, 3>(0, 0) = R; + T.block<3, 1>(0, 3) = t; + return T; + }; + + Mat4 T = Mat4::Identity(); + + // Base link transform (translation from base_link to hip axis intersection) + T = T * make_T(Mat3::Identity(), {BASE_LINK_SIDE_OFFSET, BASE_LINK_Z_OFFSET, 0}); + + // HipYaw: rotation about Z + T = T * make_T(axangle2mat({0, 0, 1}, q.at("HipYaw"))); + + // HipRoll: rotation about X + T = T * make_T(axangle2mat({1, 0, 0}, q.at("HipRoll"))); + + // HipPitch: rotation about Y + offset along x + T = T * make_T(axangle2mat({0, 1, 0}, q.at("HipPitch")), {HIP_PITCH_OFFSET, 0, 0}); + + // Knee: rotation about Y + translation down z + T = T * make_T(axangle2mat({0, 1, 0}, q.at("Knee")), {0, 0, -UPPER_LEG_LENGTH}); + + // AnklePitch: rotation about Y + translation down z + T = T * make_T(axangle2mat({0, 1, 0}, q.at("AnklePitch")), {0, 0, -LOWER_LEG_LENGTH}); + + // AnkleRoll: rotation about X + T = T * make_T(axangle2mat({1, 0, 0}, q.at("AnkleRoll"))); + + // End effector offset + T = T * make_T(Mat3::Identity(), {SOLE_X_OFFSET, SOLE_Y_OFFSET, SOLE_Z_OFFSET}); + + return T; +} + +// ── Inverse kinematics ───────────────────────────────────────────────────── +static JointAngles calculate_ik(const Mat4& target_transform, bool left) { + JointAngles joint_angles = {{"HipYaw", 0.0}, {"HipRoll", 0.0}, {"HipPitch", 0.0}, + {"Knee", 0.0}, {"AnklePitch", 0.0}, {"AnkleRoll", 0.0}}; + + // std::cout << "Target transformation matrix:\n" << target_transform << "\n"; + + auto make_T = [](const Mat3& R, const Vec3& t = Vec3::Zero()) { + Mat4 T = Mat4::Identity(); + T.block<3, 3>(0, 0) = R; + T.block<3, 1>(0, 3) = t; + return T; + }; + + // Remove the ends of the kinematic chain up to the first and last joint + Mat4 T_ankle_to_sole, T_base_link_to_hip_intersection; + + if (left) { + T_ankle_to_sole = make_T(Mat3::Identity(), {SOLE_X_OFFSET, SOLE_Y_OFFSET, SOLE_Z_OFFSET}); + T_base_link_to_hip_intersection = make_T(Mat3::Identity(), {0, BASE_LINK_SIDE_OFFSET, BASE_LINK_Z_OFFSET}); + } else { + T_ankle_to_sole = make_T(Mat3::Identity(), {SOLE_X_OFFSET, -SOLE_Y_OFFSET, SOLE_Z_OFFSET}); + T_base_link_to_hip_intersection = make_T(Mat3::Identity(), {0, -BASE_LINK_SIDE_OFFSET, BASE_LINK_Z_OFFSET}); + } + + auto target_transform_leg_only = + (T_base_link_to_hip_intersection.inverse() * target_transform) * T_ankle_to_sole.inverse(); + + // std::cout << "Target transformation matrix (leg only):\n" << target_transform_leg_only << "\n"; + + // Axis-intersection transforms (identity = at origin) + Mat4 T_hip_axis_intersection = Mat4::Identity(); + Mat4 T_ankle_axis_intersection = target_transform_leg_only; + + // ankle-to-hip transform + Mat4 T_ankle_to_hip = T_ankle_axis_intersection.inverse() * T_hip_axis_intersection; + // std::cout << "Ankle to hip:\n" << T_ankle_to_hip << "\n"; + + // ankle-to-leg alignment (inverse of foot_to_leg subproblem) + Vec3 leg_vec = T_ankle_to_hip.block<3, 1>(0, 3); + Mat4 T_ankle_to_leg = foot_to_leg_from_vec_subproblem(leg_vec).inverse(); + + // hip-to-leg alignment + Mat4 T_hip_to_leg = T_ankle_to_hip.inverse() * T_ankle_to_leg; + // std::cout << "Hip to leg:\n" << T_hip_to_leg << "\n"; + + // HipYaw and HipRoll from rzxy decomposition: M = Rz(yaw)*Rx(roll)*Ry(pitch) + auto hip_euler = mat2euler_rzxy(T_hip_to_leg.block<3, 3>(0, 0)); + joint_angles["HipYaw"] = hip_euler[0]; + joint_angles["HipRoll"] = hip_euler[1]; + // hip_euler[2] would be HipPitch, but we compute it properly below + // std::cout << "Hip angles (spherical): " << joint_angles["HipYaw"] + // << ", " << joint_angles["HipRoll"] << "\n"; + + // Virtual leg length (distance from ankle axis intersection to hip axis intersection) + // double virtual_leg_length = leg_vec.norm(); + // std::cout << "Leg length: " << virtual_leg_length << "\n"; + + // Hip transform without pitch component + Mat4 T_hip_without_pitch = Mat4::Identity(); + T_hip_without_pitch.block<3, 3>(0, 0) = euler2mat_rzxy(joint_angles["HipYaw"], joint_angles["HipRoll"], 0.0); + + // Hip pitch offset transform + Mat4 T_hip_pitch_offset = Mat4::Identity(); + T_hip_pitch_offset.block<3, 1>(0, 3) = Vec3(HIP_PITCH_OFFSET, 0, 0); + + Mat4 T_hip_pitch_origin = T_hip_without_pitch * T_hip_pitch_offset; + + // Real leg: from ankle to hip-pitch origin + Mat4 T_real_leg = T_ankle_axis_intersection.inverse() * T_hip_axis_intersection * T_hip_pitch_origin; + // std::cout << "Real leg transformation matrix:\n" << T_real_leg << "\n"; + + double real_leg_length = T_real_leg.block<3, 1>(0, 3).norm(); + // std::cout << "Real leg length: " << real_leg_length << "\n"; + + // Ankle angles from real-leg direction + Vec3 real_leg_vec = T_real_leg.block<3, 1>(0, 3); + Mat4 T_ankle_to_real_leg = foot_to_leg_from_vec_subproblem(real_leg_vec).inverse(); + + // mat2euler 'rxyz': M = Rx(a)*Ry(b)*Rz(c) + auto ankle_euler = mat2euler_rxyz(T_ankle_to_real_leg.block<3, 3>(0, 0)); + joint_angles["AnkleRoll"] = -ankle_euler[0]; + joint_angles["AnklePitch"] = -ankle_euler[1]; + // std::cout << "Ankle to real leg: " << joint_angles["AnkleRoll"] + // << ", " << joint_angles["AnklePitch"] << "\n"; + + // Knee from cosine rule + double knee_arg = + (real_leg_length * real_leg_length - UPPER_LEG_LENGTH * UPPER_LEG_LENGTH - LOWER_LEG_LENGTH * LOWER_LEG_LENGTH) / + (2.0 * UPPER_LEG_LENGTH * LOWER_LEG_LENGTH); + joint_angles["Knee"] = std::acos(std::clamp(knee_arg, -1.0, 1.0)); + + // HipPitch base from rzxy[2] + Mat4 T_hip_to_leg_real = T_real_leg.inverse() * T_ankle_to_real_leg; + auto hip_euler_real = mat2euler_rzxy(T_hip_to_leg_real.block<3, 3>(0, 0)); + double hip_pitch_base = hip_euler_real[2]; + + // Hip pitch offset (cosine rule) + double hip_pitch_offset_arg = + (UPPER_LEG_LENGTH * UPPER_LEG_LENGTH + real_leg_length * real_leg_length - LOWER_LEG_LENGTH * LOWER_LEG_LENGTH) / + (2.0 * UPPER_LEG_LENGTH * real_leg_length); + double hip_pitch_offset_val = std::acos(std::clamp(hip_pitch_offset_arg, -1.0, 1.0)); + // std::cout << "Hip pitch offset: " << hip_pitch_offset_val << "\n"; + + // Ankle pitch offset (cosine rule) + double ankle_pitch_offset_arg = + (LOWER_LEG_LENGTH * LOWER_LEG_LENGTH + real_leg_length * real_leg_length - UPPER_LEG_LENGTH * UPPER_LEG_LENGTH) / + (2.0 * LOWER_LEG_LENGTH * real_leg_length); + double ankle_pitch_offset_val = std::acos(std::clamp(ankle_pitch_offset_arg, -1.0, 1.0)); + // std::cout << "Ankle pitch offset: " << ankle_pitch_offset_val << "\n"; + + // Solution i=0 (matches Python solutions[0]) + joint_angles["HipPitch"] = hip_pitch_base - hip_pitch_offset_val; + joint_angles["AnklePitch"] += -ankle_pitch_offset_val; // i=0: subtract + + return joint_angles; +} + +static JointAngles calculate_ik_right_leg(const Mat4& target_transform) { + auto ik_solution = calculate_ik(target_transform, false); + + ik_solution["HipYaw"] *= -1; + ik_solution["HipPitch"] -= HIP_PITCH_ANGLE_OFFSET; + ik_solution["Knee"] += KNEE_OFFSET; + ik_solution["Knee"] *= -1; + ik_solution["AnklePitch"] *= -1; + ik_solution["AnkleRoll"] *= -1; + + return ik_solution; +} + +static JointAngles calculate_ik_left_leg(const Mat4& target_transform) { + auto ik_solution = calculate_ik(target_transform, true); + + ik_solution["HipYaw"] *= -1; + ik_solution["HipPitch"] *= -1; + ik_solution["AnkleRoll"] *= -1; + ik_solution["HipPitch"] += HIP_PITCH_ANGLE_OFFSET; + ik_solution["Knee"] += KNEE_OFFSET; + + return ik_solution; +} diff --git a/src/bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_ik.hpp b/src/bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_ik.hpp index 20da32c54..4db6ba5b9 100644 --- a/src/bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_ik.hpp +++ b/src/bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_ik.hpp @@ -1,10 +1,13 @@ #ifndef BITBOTS_QUINTIC_WALK_INCLUDE_BITBOTS_QUINTIC_WALK_WALK_IK_H_ #define BITBOTS_QUINTIC_WALK_INCLUDE_BITBOTS_QUINTIC_WALK_WALK_IK_H_ #include +#include #include #include -#include +#include +#include #include + namespace bitbots_quintic_walk { class WalkIK : public bitbots_splines::AbstractIK { @@ -16,17 +19,18 @@ class WalkIK : public bitbots_splines::AbstractIK { void reset() override; void setConfig(walking::Params::Node::Ik config); - const std::vector& getLeftLegJointNames(); - const std::vector& getRightLegJointNames(); + const std::vector getLeftLegJointNames(); + const std::vector getRightLegJointNames(); - moveit::core::RobotStatePtr get_goal_state(); + const geometry_msgs::msg::Pose get_right_goal(); + const geometry_msgs::msg::Pose get_left_goal(); private: rclcpp::Node::SharedPtr node_; - moveit::core::RobotStatePtr goal_state_; - const moveit::core::JointModelGroup* legs_joints_group_; - const moveit::core::JointModelGroup* left_leg_joints_group_; - const moveit::core::JointModelGroup* right_leg_joints_group_; + + geometry_msgs::msg::Pose right_foot_goal_; + geometry_msgs::msg::Pose left_foot_goal_; + walking::Params::Node::Ik config_; }; } // namespace bitbots_quintic_walk diff --git a/src/bitbots_motion/bitbots_quintic_walk/src/walk_ik.cpp b/src/bitbots_motion/bitbots_quintic_walk/src/walk_ik.cpp index d4e8fa1d3..07346c299 100644 --- a/src/bitbots_motion/bitbots_quintic_walk/src/walk_ik.cpp +++ b/src/bitbots_motion/bitbots_quintic_walk/src/walk_ik.cpp @@ -5,13 +5,6 @@ namespace bitbots_quintic_walk { WalkIK::WalkIK(rclcpp::Node::SharedPtr node, walking::Params::Node::Ik config) : node_(node), config_(config) {} void WalkIK::init(moveit::core::RobotModelPtr kinematic_model) { - legs_joints_group_ = kinematic_model->getJointModelGroup("Legs"); - left_leg_joints_group_ = kinematic_model->getJointModelGroup("LeftLeg"); - right_leg_joints_group_ = kinematic_model->getJointModelGroup("RightLeg"); - - goal_state_.reset(new moveit::core::RobotState(kinematic_model)); - goal_state_->setToDefaultValues(); - if (config_.reset) { reset(); } @@ -23,65 +16,84 @@ bitbots_splines::JointGoals WalkIK::calculate(const WalkResponse& ik_goals) { tf2::Transform trunk_to_flying_foot_goal = trunk_to_support_foot_goal * ik_goals.support_foot_to_flying_foot; // make pose msg for calling IK - geometry_msgs::msg::Pose left_foot_goal_msg; - geometry_msgs::msg::Pose right_foot_goal_msg; + geometry_msgs::msg::Transform left_foot_goal, right_foot_goal; + + auto transform_to_geo_tf = [](const tf2::Transform& transform) { + geometry_msgs::msg::Transform msg; + msg.translation.x = transform.getOrigin().x(); + msg.translation.y = transform.getOrigin().y(); + msg.translation.z = transform.getOrigin().z(); + msg.rotation.x = transform.getRotation().x(); + msg.rotation.y = transform.getRotation().y(); + msg.rotation.z = transform.getRotation().z(); + msg.rotation.w = transform.getRotation().w(); + return msg; + }; // decide which foot is which if (ik_goals.is_left_support_foot) { - tf2::toMsg(trunk_to_support_foot_goal, left_foot_goal_msg); - tf2::toMsg(trunk_to_flying_foot_goal, right_foot_goal_msg); + left_foot_goal = transform_to_geo_tf(trunk_to_support_foot_goal); + right_foot_goal = transform_to_geo_tf(trunk_to_flying_foot_goal); } else { - tf2::toMsg(trunk_to_support_foot_goal, right_foot_goal_msg); - tf2::toMsg(trunk_to_flying_foot_goal, left_foot_goal_msg); + right_foot_goal = transform_to_geo_tf(trunk_to_support_foot_goal); + left_foot_goal = transform_to_geo_tf(trunk_to_flying_foot_goal); } - // call IK two times, since we have two legs - bool success; - - // we have to do this otherwise there is an error - goal_state_->updateLinkTransforms(); + auto transform_to_geo_pose = [](const geometry_msgs::msg::Transform& transform) { + geometry_msgs::msg::Pose msg; + msg.position.x = transform.translation.x; + msg.position.y = transform.translation.y; + msg.position.z = transform.translation.z; + msg.orientation.x = transform.rotation.x; + msg.orientation.y = transform.rotation.y; + msg.orientation.z = transform.rotation.z; + msg.orientation.w = transform.rotation.w; + return msg; + }; + + left_foot_goal_ = transform_to_geo_pose(left_foot_goal); + right_foot_goal_ = transform_to_geo_pose(right_foot_goal); - success = goal_state_->setFromIK(left_leg_joints_group_, left_foot_goal_msg, config_.timeout, - moveit::core::GroupStateValidityCallbackFn()); - goal_state_->updateLinkTransforms(); + bitbots_splines::JointGoals result; - success &= goal_state_->setFromIK(right_leg_joints_group_, right_foot_goal_msg, config_.timeout, - moveit::core::GroupStateValidityCallbackFn()); + // call IK + Eigen::Isometry3d right_iso = tf2::transformToEigen(right_foot_goal); + Mat4 T_right = right_iso.matrix(); + auto result_right = calculate_ik_right_leg(T_right); - if (!success) { - RCLCPP_ERROR(node_->get_logger(), "IK failed with no solution found"); + for (const auto& joint : getRightLegJointNames()) { + result.first.push_back(joint); + result.second.push_back( + result_right[joint.substr(1)]); // remove the "R" from the joint name to get the corresponding left joint } - std::vector joint_names = legs_joints_group_->getActiveJointModelNames(); - std::vector joint_goals; - goal_state_->copyJointGroupPositions(legs_joints_group_, joint_goals); + Eigen::Isometry3d left_iso = tf2::transformToEigen(left_foot_goal); + Mat4 T_left = left_iso.matrix(); + auto result_left = calculate_ik_left_leg(T_left); + + for (const auto& joint : getLeftLegJointNames()) { + result.first.push_back(joint); + result.second.push_back( + result_left[joint.substr(1)]); // remove the "R" from the joint name to get the corresponding right joint + } - /* construct result object */ - bitbots_splines::JointGoals result; - result.first = joint_names; - result.second = joint_goals; return result; } -void WalkIK::reset() { - // we have to set some good initial position in the goal state, since we are using a gradient - // based method. Otherwise, the first step will be not correct - std::vector names_vec = {"LHipPitch", "LKnee", "LAnklePitch", "RHipPitch", "RKnee", "RAnklePitch"}; - std::vector pos_vec = {0.7, 1.0, -0.4, -0.7, -1.0, 0.4}; - for (size_t i = 0; i < names_vec.size(); i++) { - // besides its name, this method only changes a single joint position... - goal_state_->setJointPositions(names_vec[i], &pos_vec[i]); - } -} +void WalkIK::reset() {} void WalkIK::setConfig(walking::Params::Node::Ik config) { config_ = config; } -const std::vector& WalkIK::getLeftLegJointNames() { return left_leg_joints_group_->getJointModelNames(); } +const std::vector WalkIK::getLeftLegJointNames() { + return {"LHipYaw", "LHipRoll", "LHipPitch", "LKnee", "LAnklePitch", "LAnkleRoll"}; +} -const std::vector& WalkIK::getRightLegJointNames() { - return right_leg_joints_group_->getJointModelNames(); +const std::vector WalkIK::getRightLegJointNames() { + return {"RHipYaw", "RHipRoll", "RHipPitch", "RKnee", "RAnklePitch", "RAnkleRoll"}; } -moveit::core::RobotStatePtr WalkIK::get_goal_state() { return goal_state_; } +const geometry_msgs::msg::Pose WalkIK::get_right_goal() { return right_foot_goal_; } + +const geometry_msgs::msg::Pose WalkIK::get_left_goal() { return left_foot_goal_; } } // namespace bitbots_quintic_walk diff --git a/src/bitbots_motion/bitbots_quintic_walk/src/walk_node.cpp b/src/bitbots_motion/bitbots_quintic_walk/src/walk_node.cpp index 46f775fd2..4a7ba8f90 100644 --- a/src/bitbots_motion/bitbots_quintic_walk/src/walk_node.cpp +++ b/src/bitbots_motion/bitbots_quintic_walk/src/walk_node.cpp @@ -317,19 +317,9 @@ geometry_msgs::msg::PoseArray WalkNode::step_open_loop(double dt, return pose_array; } -geometry_msgs::msg::Pose WalkNode::get_left_foot_pose() { - moveit::core::RobotStatePtr goal_state = ik_.get_goal_state(); - geometry_msgs::msg::Pose pose; - tf2::convert(goal_state->getGlobalLinkTransform("l_sole"), pose); - return pose; -} +geometry_msgs::msg::Pose WalkNode::get_left_foot_pose() { return ik_.get_left_goal(); } -geometry_msgs::msg::Pose WalkNode::get_right_foot_pose() { - moveit::core::RobotStatePtr goal_state = ik_.get_goal_state(); - geometry_msgs::msg::Pose pose; - tf2::convert(goal_state->getGlobalLinkTransform("r_sole"), pose); - return pose; -} +geometry_msgs::msg::Pose WalkNode::get_right_foot_pose() { return ik_.get_right_goal(); } std::array WalkNode::get_step_from_vel(const geometry_msgs::msg::Twist::SharedPtr msg) { // We have to compute by dividing by step frequency which is a double step diff --git a/src/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters_amy.yaml b/src/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters_amy.yaml new file mode 100644 index 000000000..e69de29bb diff --git a/src/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters_default.yaml b/src/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters_default.yaml new file mode 100644 index 000000000..e69de29bb diff --git a/src/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters_donna.yaml b/src/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters_donna.yaml new file mode 100644 index 000000000..e69de29bb diff --git a/src/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters_jack.yaml b/src/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters_jack.yaml new file mode 100644 index 000000000..772a3140a --- /dev/null +++ b/src/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters_jack.yaml @@ -0,0 +1,5 @@ +bitbots_path_planning: + ros__parameters: + controller: + max_rotation_vel: 0.7 + max_vel_x: 0.18 diff --git a/src/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters_rory.yaml b/src/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters_rory.yaml new file mode 100644 index 000000000..e69de29bb diff --git a/src/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters.yaml b/src/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters_template.yaml similarity index 100% rename from src/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters.yaml rename to src/bitbots_navigation/bitbots_path_planning/config/path_planning_parameters_template.yaml diff --git a/src/bitbots_navigation/bitbots_path_planning/launch/path_planning.launch b/src/bitbots_navigation/bitbots_path_planning/launch/path_planning.launch index 0774f196a..043ddf64b 100755 --- a/src/bitbots_navigation/bitbots_path_planning/launch/path_planning.launch +++ b/src/bitbots_navigation/bitbots_path_planning/launch/path_planning.launch @@ -3,5 +3,6 @@ + diff --git a/src/bitbots_navigation/bitbots_path_planning/setup.py b/src/bitbots_navigation/bitbots_path_planning/setup.py index d18b72a7e..dd2b07cfa 100644 --- a/src/bitbots_navigation/bitbots_path_planning/setup.py +++ b/src/bitbots_navigation/bitbots_path_planning/setup.py @@ -5,7 +5,7 @@ generate_parameter_module( "path_planning_parameters", # python module name for parameter library - "config/path_planning_parameters.yaml", # path to input yaml file + "config/path_planning_parameters_template.yaml", # path to input yaml file ) package_name = "bitbots_path_planning" diff --git a/src/bitbots_robot/wolfgang_description/urdf/robot.urdf b/src/bitbots_robot/wolfgang_description/urdf/robot.urdf index b964ae9a6..49fdd84a5 100644 --- a/src/bitbots_robot/wolfgang_description/urdf/robot.urdf +++ b/src/bitbots_robot/wolfgang_description/urdf/robot.urdf @@ -2200,7 +2200,7 @@ - +