From ef2361b419cec5ccaaab1cf8d5d7cfc1867b3eca Mon Sep 17 00:00:00 2001 From: Florian Vahl Date: Wed, 18 Jun 2025 13:48:59 +0200 Subject: [PATCH 1/3] Use gradient bio_ik solver --- .../wolfgang_moveit_config/config/kinematics.yaml | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/bitbots_robot/wolfgang_moveit_config/config/kinematics.yaml b/bitbots_robot/wolfgang_moveit_config/config/kinematics.yaml index c595bf4e4..90b1d50cc 100644 --- a/bitbots_robot/wolfgang_moveit_config/config/kinematics.yaml +++ b/bitbots_robot/wolfgang_moveit_config/config/kinematics.yaml @@ -2,28 +2,36 @@ LeftLeg: kinematics_solver: bio_ik/BioIKKinematicsPlugin # kdl_kinematics_plugin/KDLKinematicsPlugin kinematics_solver_search_resolution: 0.00001 kinematics_solver_timeout: 0.01 + mode: gd_c RightLeg: kinematics_solver: bio_ik/BioIKKinematicsPlugin # kdl_kinematics_plugin/KDLKinematicsPlugin kinematics_solver_search_resolution: 0.00001 kinematics_solver_timeout: 0.01 + mode: gd_c Legs: kinematics_solver_search_resolution: 0.0001 kinematics_solver_timeout: 0.005 + mode: gd_c RightArm: kinematics_solver: bio_ik/BioIKKinematicsPlugin kinematics_solver_search_resolution: 0.00001 kinematics_solver_timeout: 0.01 + mode: gd_c LeftArm: kinematics_solver: bio_ik/BioIKKinematicsPlugin kinematics_solver_search_resolution: 0.00001 kinematics_solver_timeout: 0.01 + mode: gd_c Arms: kinematics_solver_search_resolution: 0.00001 kinematics_solver_timeout: 0.01 + mode: gd_c Head: kinematics_solver: bio_ik/BioIKKinematicsPlugin kinematics_solver_search_resolution: 0.001 kinematics_solver_timeout: 0.001 + mode: gd_c All: kinematics_solver_search_resolution: 0.005 kinematics_solver_timeout: 0.005 + mode: gd_c From fbe404866f816eed442a53238174ad55886490fb Mon Sep 17 00:00:00 2001 From: Florian Vahl Date: Wed, 18 Jun 2025 13:58:29 +0200 Subject: [PATCH 2/3] Fix memory issue discovered during visit at the wolves. --- .../src/Spline/pose_spline.cpp | 36 ++++--------------- .../src/Spline/position_spline.cpp | 18 ++-------- 2 files changed, 9 insertions(+), 45 deletions(-) diff --git a/bitbots_motion/bitbots_splines/src/Spline/pose_spline.cpp b/bitbots_motion/bitbots_splines/src/Spline/pose_spline.cpp index 3e8392cb7..a8f033282 100644 --- a/bitbots_motion/bitbots_splines/src/Spline/pose_spline.cpp +++ b/bitbots_motion/bitbots_splines/src/Spline/pose_spline.cpp @@ -31,48 +31,24 @@ geometry_msgs::msg::Quaternion PoseSpline::getGeometryMsgOrientation(double time } tf2::Vector3 PoseSpline::getPositionPos(double time) { - tf2::Vector3 pos; - pos[0] = x_.pos(time); - pos[1] = y_.pos(time); - pos[2] = z_.pos(time); - return pos; + return tf2::Vector3(x_.pos(time), y_.pos(time), z_.pos(time)); } tf2::Vector3 PoseSpline::getPositionVel(double time) { - tf2::Vector3 vel; - vel[0] = x_.vel(time); - vel[1] = y_.vel(time); - vel[2] = z_.vel(time); - return vel; + return tf2::Vector3(x_.vel(time), y_.vel(time), z_.vel(time)); } tf2::Vector3 PoseSpline::getPositionAcc(double time) { - tf2::Vector3 acc; - acc[0] = x_.acc(time); - acc[1] = y_.acc(time); - acc[2] = z_.acc(time); - return acc; + return tf2::Vector3(x_.acc(time), y_.acc(time), z_.acc(time)); } tf2::Vector3 PoseSpline::getEulerAngles(double time) { - tf2::Vector3 pos; - pos[0] = roll_.pos(time); - pos[1] = pitch_.pos(time); - pos[2] = yaw_.pos(time); - return pos; + return tf2::Vector3(roll_.pos(time), pitch_.pos(time), yaw_.pos(time)); } tf2::Vector3 PoseSpline::getEulerVel(double time) { - tf2::Vector3 vel; - vel[0] = roll_.vel(time); - vel[1] = pitch_.vel(time); - vel[2] = yaw_.vel(time); - return vel; + return tf2::Vector3(roll_.vel(time), pitch_.vel(time), yaw_.vel(time)); } tf2::Vector3 PoseSpline::getEulerAcc(double time) { - tf2::Vector3 acc; - acc[0] = roll_.acc(time); - acc[1] = pitch_.acc(time); - acc[2] = yaw_.acc(time); - return acc; + return tf2::Vector3(roll_.acc(time), pitch_.acc(time), yaw_.acc(time)); } tf2::Quaternion PoseSpline::getOrientation(double time) { diff --git a/bitbots_motion/bitbots_splines/src/Spline/position_spline.cpp b/bitbots_motion/bitbots_splines/src/Spline/position_spline.cpp index c979e9c6f..09ef35cd6 100644 --- a/bitbots_motion/bitbots_splines/src/Spline/position_spline.cpp +++ b/bitbots_motion/bitbots_splines/src/Spline/position_spline.cpp @@ -12,26 +12,14 @@ geometry_msgs::msg::Point PositionSpline::getGeometryMsgPosition(double time) { } tf2::Vector3 PositionSpline::getPos(double time) { - tf2::Vector3 pos; - pos[0] = x_.pos(time); - pos[1] = y_.pos(time); - pos[2] = z_.pos(time); - return pos; + return tf2::Vector3(x_.pos(time), y_.pos(time), z_.pos(time)); } tf2::Vector3 PositionSpline::getVel(double time) { - tf2::Vector3 vel; - vel[0] = x_.vel(time); - vel[1] = y_.vel(time); - vel[2] = z_.vel(time); - return vel; + return tf2::Vector3(x_.vel(time), y_.vel(time), z_.vel(time)); } tf2::Vector3 PositionSpline::getAcc(double time) { - tf2::Vector3 acc; - acc[0] = x_.acc(time); - acc[1] = y_.acc(time); - acc[2] = z_.acc(time); - return acc; + return tf2::Vector3(x_.acc(time), y_.acc(time), z_.acc(time)); } SmoothSpline *PositionSpline::x() { return &x_; } From e7d8d89b386b0150d999c91f06e173cdea2c00a1 Mon Sep 17 00:00:00 2001 From: Florian Vahl Date: Wed, 18 Jun 2025 14:00:18 +0200 Subject: [PATCH 3/3] Apply formatting --- .../bitbots_splines/src/Spline/pose_spline.cpp | 12 +++--------- .../bitbots_splines/src/Spline/position_spline.cpp | 12 +++--------- 2 files changed, 6 insertions(+), 18 deletions(-) diff --git a/bitbots_motion/bitbots_splines/src/Spline/pose_spline.cpp b/bitbots_motion/bitbots_splines/src/Spline/pose_spline.cpp index a8f033282..95ea19c80 100644 --- a/bitbots_motion/bitbots_splines/src/Spline/pose_spline.cpp +++ b/bitbots_motion/bitbots_splines/src/Spline/pose_spline.cpp @@ -30,16 +30,10 @@ geometry_msgs::msg::Quaternion PoseSpline::getGeometryMsgOrientation(double time return msg; } -tf2::Vector3 PoseSpline::getPositionPos(double time) { - return tf2::Vector3(x_.pos(time), y_.pos(time), z_.pos(time)); -} +tf2::Vector3 PoseSpline::getPositionPos(double time) { return tf2::Vector3(x_.pos(time), y_.pos(time), z_.pos(time)); } -tf2::Vector3 PoseSpline::getPositionVel(double time) { - return tf2::Vector3(x_.vel(time), y_.vel(time), z_.vel(time)); -} -tf2::Vector3 PoseSpline::getPositionAcc(double time) { - return tf2::Vector3(x_.acc(time), y_.acc(time), z_.acc(time)); -} +tf2::Vector3 PoseSpline::getPositionVel(double time) { return tf2::Vector3(x_.vel(time), y_.vel(time), z_.vel(time)); } +tf2::Vector3 PoseSpline::getPositionAcc(double time) { return tf2::Vector3(x_.acc(time), y_.acc(time), z_.acc(time)); } tf2::Vector3 PoseSpline::getEulerAngles(double time) { return tf2::Vector3(roll_.pos(time), pitch_.pos(time), yaw_.pos(time)); diff --git a/bitbots_motion/bitbots_splines/src/Spline/position_spline.cpp b/bitbots_motion/bitbots_splines/src/Spline/position_spline.cpp index 09ef35cd6..c6f862a6c 100644 --- a/bitbots_motion/bitbots_splines/src/Spline/position_spline.cpp +++ b/bitbots_motion/bitbots_splines/src/Spline/position_spline.cpp @@ -11,16 +11,10 @@ geometry_msgs::msg::Point PositionSpline::getGeometryMsgPosition(double time) { return msg; } -tf2::Vector3 PositionSpline::getPos(double time) { - return tf2::Vector3(x_.pos(time), y_.pos(time), z_.pos(time)); -} +tf2::Vector3 PositionSpline::getPos(double time) { return tf2::Vector3(x_.pos(time), y_.pos(time), z_.pos(time)); } -tf2::Vector3 PositionSpline::getVel(double time) { - return tf2::Vector3(x_.vel(time), y_.vel(time), z_.vel(time)); -} -tf2::Vector3 PositionSpline::getAcc(double time) { - return tf2::Vector3(x_.acc(time), y_.acc(time), z_.acc(time)); -} +tf2::Vector3 PositionSpline::getVel(double time) { return tf2::Vector3(x_.vel(time), y_.vel(time), z_.vel(time)); } +tf2::Vector3 PositionSpline::getAcc(double time) { return tf2::Vector3(x_.acc(time), y_.acc(time), z_.acc(time)); } SmoothSpline *PositionSpline::x() { return &x_; }