Use gradient bio_ik solver and fix memory issues (#705)

Author: Flova

bitbots_motion/bitbots_splines/src/Spline/pose_spline.cpp

@@ -30,49 +30,19 @@ geometry_msgs::msg::Quaternion PoseSpline::getGeometryMsgOrientation(double time
   return msg;
 }
 
-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;
-}
+tf2::Vector3 PoseSpline::getPositionPos(double time) { 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;
-}
-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;
-}
+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) {
-  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) {

bitbots_motion/bitbots_splines/src/Spline/position_spline.cpp

@@ -11,28 +11,10 @@ geometry_msgs::msg::Point PositionSpline::getGeometryMsgPosition(double time) {
   return msg;
 }
 
-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;
-}
+tf2::Vector3 PositionSpline::getPos(double time) { 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;
-}
-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;
-}
+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_; }
 

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