From 7fca1a10b5e2de21eaaa6bc24d1aaabca8ee333c Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Tue, 1 Jan 2019 10:24:51 +0100 Subject: [PATCH] tf -> tf2 --- CMakeLists.txt | 11 ++- include/bio_ik/bio_ik.h | 2 - include/bio_ik/frame.h | 44 ++++++----- include/bio_ik/goal_types.h | 153 ++++++++++++++++++------------------ package.xml | 14 ++-- src/forward_kinematics.h | 4 +- src/goal_types.cpp | 4 +- src/ik_neural.cpp | 4 +- src/kinematics_plugin.cpp | 4 +- src/problem.h | 6 +- src/utils.h | 8 +- test/utest.cpp | 4 +- 12 files changed, 133 insertions(+), 125 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 556b981..608afb1 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -11,8 +11,10 @@ find_package(catkin REQUIRED COMPONENTS moveit_ros_planning pluginlib roscpp - tf - tf_conversions + tf2 + tf2_eigen + tf2_kdl + tf2_geometry_msgs ) find_package(OpenMP) @@ -68,8 +70,9 @@ catkin_package( moveit_ros_planning pluginlib roscpp - tf - tf_conversions + tf2 + tf2_kdl + tf2_geometry_msgs ) add_compile_options(-frecord-gcc-switches) diff --git a/include/bio_ik/bio_ik.h b/include/bio_ik/bio_ik.h index 6303fd2..115bd6a 100644 --- a/include/bio_ik/bio_ik.h +++ b/include/bio_ik/bio_ik.h @@ -39,8 +39,6 @@ #include #include -#include - #include "goal.h" #include "goal_types.h" diff --git a/include/bio_ik/frame.h b/include/bio_ik/frame.h index eadafc4..8e10125 100644 --- a/include/bio_ik/frame.h +++ b/include/bio_ik/frame.h @@ -37,13 +37,16 @@ #include #include -#include +#include +#include +#include +#include namespace bio_ik { -typedef tf::Quaternion Quaternion; -typedef tf::Vector3 Vector3; +typedef tf2::Quaternion Quaternion; +typedef tf2::Vector3 Vector3; struct Frame { @@ -51,28 +54,28 @@ struct Frame double __padding[4 - (sizeof(Vector3) / sizeof(double))]; Quaternion rot; inline Frame() {} - inline Frame(const tf::Vector3& pos, const tf::Quaternion& rot) + inline Frame(const tf2::Vector3& pos, const tf2::Quaternion& rot) : pos(pos) , rot(rot) { } explicit inline Frame(const KDL::Frame& kdl) { - pos = tf::Vector3(kdl.p.x(), kdl.p.y(), kdl.p.z()); + pos = tf2::Vector3(kdl.p.x(), kdl.p.y(), kdl.p.z()); double qx, qy, qz, qw; kdl.M.GetQuaternion(qx, qy, qz, qw); - rot = tf::Quaternion(qx, qy, qz, qw); + rot = tf2::Quaternion(qx, qy, qz, qw); } explicit inline Frame(const geometry_msgs::Pose& msg) { - tf::quaternionMsgToTF(msg.orientation, rot); - pos = tf::Vector3(msg.position.x, msg.position.y, msg.position.z); + tf2::fromMsg(msg.orientation, rot); + pos = tf2::Vector3(msg.position.x, msg.position.y, msg.position.z); } explicit inline Frame(const Eigen::Affine3d& f) { - pos = tf::Vector3(f.translation().x(), f.translation().y(), f.translation().z()); + pos = tf2::Vector3(f.translation().x(), f.translation().y(), f.translation().z()); Eigen::Quaterniond q(f.rotation()); - rot = tf::Quaternion(q.x(), q.y(), q.z(), q.w()); + rot = tf2::Quaternion(q.x(), q.y(), q.z(), q.w()); } inline const Vector3& getPosition() const { return pos; } @@ -92,25 +95,24 @@ struct Frame inline void frameToKDL(const Frame& frame, KDL::Frame& kdl_frame) { - KDL::Rotation kdl_rot; - KDL::Vector kdl_pos; - tf::quaternionTFToKDL(frame.rot, kdl_rot); - tf::vectorTFToKDL(frame.pos, kdl_pos); - kdl_frame = KDL::Frame(kdl_rot, kdl_pos); + kdl_frame.p.x(frame.pos.x()); + kdl_frame.p.y(frame.pos.y()); + kdl_frame.p.z(frame.pos.z()); + kdl_frame.M = KDL::Rotation::Quaternion(frame.rot.x(), frame.rot.y(), frame.rot.z(), frame.rot.w()); } template const Frame Frame::IdentityFrameTemplate::identity_frame(Vector3(0, 0, 0), Quaternion(0, 0, 0, 1)); static std::ostream& operator<<(std::ostream& os, const Frame& f) { return os << "(" << f.pos.x() << "," << f.pos.y() << "," << f.pos.z() << ";" << f.rot.x() << "," << f.rot.y() << "," << f.rot.z() << "," << f.rot.w() << ")"; } -__attribute__((always_inline)) inline void quat_mul_vec(const tf::Quaternion& q, const tf::Vector3& v, tf::Vector3& r) +__attribute__((always_inline)) inline void quat_mul_vec(const tf2::Quaternion& q, const tf2::Vector3& v, tf2::Vector3& r) { double v_x = v.x(); double v_y = v.y(); double v_z = v.z(); - // if(__builtin_expect(v_x == 0 && v_y == 0 && v_z == 0, 0)) { r = tf::Vector3(0, 0, 0); return; } - // if(v_x == 0 && v_y == 0 && v_z == 0) { r = tf::Vector3(0, 0, 0); return; } + // if(__builtin_expect(v_x == 0 && v_y == 0 && v_z == 0, 0)) { r = tf2::Vector3(0, 0, 0); return; } + // if(v_x == 0 && v_y == 0 && v_z == 0) { r = tf2::Vector3(0, 0, 0); return; } double q_x = q.x(); double q_y = q.y(); @@ -146,7 +148,7 @@ __attribute__((always_inline)) inline void quat_mul_vec(const tf::Quaternion& q, r.setZ(r_z); } -__attribute__((always_inline)) inline void quat_mul_quat(const tf::Quaternion& p, const tf::Quaternion& q, tf::Quaternion& r) +__attribute__((always_inline)) inline void quat_mul_quat(const tf2::Quaternion& p, const tf2::Quaternion& q, tf2::Quaternion& r) { double p_x = p.x(); double p_y = p.y(); @@ -171,7 +173,7 @@ __attribute__((always_inline)) inline void quat_mul_quat(const tf::Quaternion& p __attribute__((always_inline)) inline void concat(const Frame& a, const Frame& b, Frame& r) { - tf::Vector3 d; + tf2::Vector3 d; quat_mul_vec(a.rot, b.pos, d); r.pos = a.pos + d; quat_mul_quat(a.rot, b.rot, r.rot); @@ -184,7 +186,7 @@ __attribute__((always_inline)) inline void concat(const Frame& a, const Frame& b concat(tmp, c, r); } -__attribute__((always_inline)) inline void quat_inv(const tf::Quaternion& q, tf::Quaternion& r) +__attribute__((always_inline)) inline void quat_inv(const tf2::Quaternion& q, tf2::Quaternion& r) { r.setX(-q.x()); r.setY(-q.y()); diff --git a/include/bio_ik/goal_types.h b/include/bio_ik/goal_types.h index a6957a0..2708b9c 100644 --- a/include/bio_ik/goal_types.h +++ b/include/bio_ik/goal_types.h @@ -38,7 +38,8 @@ #include "robot_info.h" -#include +#include +#include #include #include @@ -78,39 +79,39 @@ class LinkGoalBase : public Goal class PositionGoal : public LinkGoalBase { - tf::Vector3 position_; + tf2::Vector3 position_; public: PositionGoal() : position_(0, 0, 0) { } - PositionGoal(const std::string& link_name, const tf::Vector3& position, double weight = 1.0) + PositionGoal(const std::string& link_name, const tf2::Vector3& position, double weight = 1.0) : LinkGoalBase(link_name, weight) , position_(position) { } - inline const tf::Vector3& getPosition() const { return position_; } - inline void setPosition(const tf::Vector3& position) { position_ = position; } + inline const tf2::Vector3& getPosition() const { return position_; } + inline void setPosition(const tf2::Vector3& position) { position_ = position; } virtual double evaluate(const GoalContext& context) const { return context.getLinkFrame().getPosition().distance2(getPosition()); } }; class OrientationGoal : public LinkGoalBase { - tf::Quaternion orientation_; + tf2::Quaternion orientation_; public: OrientationGoal() : orientation_(0, 0, 0, 1) { } - OrientationGoal(const std::string& link_name, const tf::Quaternion& orientation, double weight = 1.0) + OrientationGoal(const std::string& link_name, const tf2::Quaternion& orientation, double weight = 1.0) : LinkGoalBase(link_name, weight) , orientation_(orientation.normalized()) { } - inline const tf::Quaternion& getOrientation() const { return orientation_; } - inline void setOrientation(const tf::Quaternion& orientation) { orientation_ = orientation.normalized(); } + inline const tf2::Quaternion& getOrientation() const { return orientation_; } + inline void setOrientation(const tf2::Quaternion& orientation) { orientation_ = orientation.normalized(); } virtual double evaluate(const GoalContext& context) const { // return getOrientation().distance2(context.getLinkFrame().getOrientation()); @@ -133,16 +134,16 @@ class PoseGoal : public LinkGoalBase , frame_(Frame::identity()) { } - PoseGoal(const std::string& link_name, const tf::Vector3& position, const tf::Quaternion& orientation, double weight = 1.0) + PoseGoal(const std::string& link_name, const tf2::Vector3& position, const tf2::Quaternion& orientation, double weight = 1.0) : LinkGoalBase(link_name, weight) , frame_(position, orientation.normalized()) , rotation_scale_(0.5) { } - inline const tf::Vector3& getPosition() const { return frame_.getPosition(); } - inline void setPosition(const tf::Vector3& position) { frame_.setPosition(position); } - inline const tf::Quaternion& getOrientation() const { return frame_.getOrientation(); } - inline void setOrientation(const tf::Quaternion& orientation) { frame_.setOrientation(orientation.normalized()); } + inline const tf2::Vector3& getPosition() const { return frame_.getPosition(); } + inline void setPosition(const tf2::Vector3& position) { frame_.setPosition(position); } + inline const tf2::Quaternion& getOrientation() const { return frame_.getOrientation(); } + inline void setOrientation(const tf2::Quaternion& orientation) { frame_.setOrientation(orientation.normalized()); } inline double getRotationScale() const { return rotation_scale_; } inline void setRotationScale(double rotation_scale) { rotation_scale_ = rotation_scale; } virtual double evaluate(const GoalContext& context) const @@ -181,8 +182,8 @@ class PoseGoal : public LinkGoalBase class LookAtGoal : public LinkGoalBase { - tf::Vector3 axis_; - tf::Vector3 target_; + tf2::Vector3 axis_; + tf2::Vector3 target_; public: LookAtGoal() @@ -190,20 +191,20 @@ class LookAtGoal : public LinkGoalBase , target_(0, 0, 0) { } - LookAtGoal(const std::string& link_name, const tf::Vector3& axis, const tf::Vector3& target, double weight = 1.0) + LookAtGoal(const std::string& link_name, const tf2::Vector3& axis, const tf2::Vector3& target, double weight = 1.0) : LinkGoalBase(link_name, weight) , axis_(axis) , target_(target) { } - const tf::Vector3& getAxis() const { return axis_; } - const tf::Vector3& getTarget() const { return target_; } - void setAxis(const tf::Vector3& axis) { axis_ = axis.normalized(); } - void setTarget(const tf::Vector3& target) { target_ = target; } + const tf2::Vector3& getAxis() const { return axis_; } + const tf2::Vector3& getTarget() const { return target_; } + void setAxis(const tf2::Vector3& axis) { axis_ = axis.normalized(); } + void setTarget(const tf2::Vector3& target) { target_ = target; } virtual double evaluate(const GoalContext& context) const { auto& fb = context.getLinkFrame(); - tf::Vector3 axis; + tf2::Vector3 axis; quat_mul_vec(fb.getOrientation(), axis_, axis); return (target_ - fb.getPosition()).normalized().distance2(axis.normalized()); // return (target_ - axis * axis.dot(target_ - fb.getPosition())).distance2(fb.getPosition()); @@ -212,7 +213,7 @@ class LookAtGoal : public LinkGoalBase class MaxDistanceGoal : public LinkGoalBase { - tf::Vector3 target; + tf2::Vector3 target; double distance; public: @@ -221,14 +222,14 @@ class MaxDistanceGoal : public LinkGoalBase , distance(1) { } - MaxDistanceGoal(const std::string& link_name, const tf::Vector3& target, double distance, double weight = 1.0) + MaxDistanceGoal(const std::string& link_name, const tf2::Vector3& target, double distance, double weight = 1.0) : LinkGoalBase(link_name, weight) , target(target) , distance(distance) { } - const tf::Vector3& getTarget() const { return target; } - void setTarget(const tf::Vector3& t) { target = t; } + const tf2::Vector3& getTarget() const { return target; } + void setTarget(const tf2::Vector3& t) { target = t; } double getDistance() const { return distance; } void setDistance(double d) { distance = d; } virtual double evaluate(const GoalContext& context) const @@ -241,7 +242,7 @@ class MaxDistanceGoal : public LinkGoalBase class MinDistanceGoal : public LinkGoalBase { - tf::Vector3 target; + tf2::Vector3 target; double distance; public: @@ -250,14 +251,14 @@ class MinDistanceGoal : public LinkGoalBase , distance(1) { } - MinDistanceGoal(const std::string& link_name, const tf::Vector3& target, double distance, double weight = 1.0) + MinDistanceGoal(const std::string& link_name, const tf2::Vector3& target, double distance, double weight = 1.0) : LinkGoalBase(link_name, weight) , target(target) , distance(distance) { } - const tf::Vector3& getTarget() const { return target; } - void setTarget(const tf::Vector3& t) { target = t; } + const tf2::Vector3& getTarget() const { return target; } + void setTarget(const tf2::Vector3& t) { target = t; } double getDistance() const { return distance; } void setDistance(double d) { distance = d; } virtual double evaluate(const GoalContext& context) const @@ -270,8 +271,8 @@ class MinDistanceGoal : public LinkGoalBase class LineGoal : public LinkGoalBase { - tf::Vector3 position; - tf::Vector3 direction; + tf2::Vector3 position; + tf2::Vector3 direction; public: LineGoal() @@ -279,16 +280,16 @@ class LineGoal : public LinkGoalBase , direction(0, 0, 0) { } - LineGoal(const std::string& link_name, const tf::Vector3& position, const tf::Vector3& direction, double weight = 1.0) + LineGoal(const std::string& link_name, const tf2::Vector3& position, const tf2::Vector3& direction, double weight = 1.0) : LinkGoalBase(link_name, weight) , position(position) , direction(direction.normalized()) { } - const tf::Vector3& getPosition() const { return position; } - void setPosition(const tf::Vector3& p) { position = p; } - const tf::Vector3& getDirection() const { return direction; } - void setDirection(const tf::Vector3& d) { direction = d.normalized(); } + const tf2::Vector3& getPosition() const { return position; } + void setPosition(const tf2::Vector3& p) { position = p; } + const tf2::Vector3& getDirection() const { return direction; } + void setDirection(const tf2::Vector3& d) { direction = d.normalized(); } virtual double evaluate(const GoalContext& context) const { auto& fb = context.getLinkFrame(); @@ -298,8 +299,8 @@ class LineGoal : public LinkGoalBase class TouchGoal : public LinkGoalBase { - tf::Vector3 position; - tf::Vector3 normal; + tf2::Vector3 position; + tf2::Vector3 normal; struct CollisionShape { std::vector vertices; @@ -333,7 +334,7 @@ class TouchGoal : public LinkGoalBase , normal(0, 0, 0) { } - TouchGoal(const std::string& link_name, const tf::Vector3& position, const tf::Vector3& normal, double weight = 1.0) + TouchGoal(const std::string& link_name, const tf2::Vector3& position, const tf2::Vector3& normal, double weight = 1.0) : LinkGoalBase(link_name, weight) , position(position) , normal(normal.normalized()) @@ -487,10 +488,10 @@ class JointFunctionGoal : public Goal class BalanceGoal : public Goal { - tf::Vector3 target_, axis_; + tf2::Vector3 target_, axis_; struct BalanceInfo { - tf::Vector3 center; + tf2::Vector3 center; double weight; }; mutable std::vector balance_infos; @@ -501,39 +502,39 @@ class BalanceGoal : public Goal , axis_(0, 0, 1) { } - BalanceGoal(const tf::Vector3& target, double weight = 1.0) + BalanceGoal(const tf2::Vector3& target, double weight = 1.0) : target_(target) , axis_(0, 0, 1) { weight_ = weight; } - const tf::Vector3& getTarget() const { return target_; } - const tf::Vector3& getAxis() const { return axis_; } - void setTarget(const tf::Vector3& target) { target_ = target; } - void setAxis(const tf::Vector3& axis) { axis_ = axis; } + const tf2::Vector3& getTarget() const { return target_; } + const tf2::Vector3& getAxis() const { return axis_; } + void setTarget(const tf2::Vector3& target) { target_ = target; } + void setAxis(const tf2::Vector3& axis) { axis_ = axis; } virtual void describe(GoalContext& context) const; virtual double evaluate(const GoalContext& context) const; }; class LinkFunctionGoal : public LinkGoalBase { - std::function function; + std::function function; public: LinkFunctionGoal() {} - LinkFunctionGoal(const std::string& link_name, const std::function& function, double weight = 1.0) + LinkFunctionGoal(const std::string& link_name, const std::function& function, double weight = 1.0) : LinkGoalBase(link_name, weight) , function(function) { } - void setLinkFunction(const std::function& f) { function = f; } + void setLinkFunction(const std::function& f) { function = f; } virtual double evaluate(const GoalContext& context) const { return function(context.getLinkFrame().getPosition(), context.getLinkFrame().getOrientation()); } }; class SideGoal : public LinkGoalBase { - tf::Vector3 axis; - tf::Vector3 direction; + tf2::Vector3 axis; + tf2::Vector3 direction; public: SideGoal() @@ -541,16 +542,16 @@ class SideGoal : public LinkGoalBase , direction(0, 0, 1) { } - SideGoal(const std::string& link_name, const tf::Vector3& axis, const tf::Vector3& direction, double weight = 1.0) + SideGoal(const std::string& link_name, const tf2::Vector3& axis, const tf2::Vector3& direction, double weight = 1.0) : LinkGoalBase(link_name, weight) , axis(axis) , direction(direction) { } - const tf::Vector3& getAxis() const { return axis; } - const tf::Vector3& getDirection() const { return direction; } - void setAxis(const tf::Vector3& a) { axis = a.normalized(); } - void setDirection(const tf::Vector3& d) { direction = d.normalized(); } + const tf2::Vector3& getAxis() const { return axis; } + const tf2::Vector3& getDirection() const { return direction; } + void setAxis(const tf2::Vector3& a) { axis = a.normalized(); } + void setDirection(const tf2::Vector3& d) { direction = d.normalized(); } virtual double evaluate(const GoalContext& context) const { auto& fb = context.getLinkFrame(); @@ -563,8 +564,8 @@ class SideGoal : public LinkGoalBase class DirectionGoal : public LinkGoalBase { - tf::Vector3 axis; - tf::Vector3 direction; + tf2::Vector3 axis; + tf2::Vector3 direction; public: DirectionGoal() @@ -572,16 +573,16 @@ class DirectionGoal : public LinkGoalBase , direction(0, 0, 1) { } - DirectionGoal(const std::string& link_name, const tf::Vector3& axis, const tf::Vector3& direction, double weight = 1.0) + DirectionGoal(const std::string& link_name, const tf2::Vector3& axis, const tf2::Vector3& direction, double weight = 1.0) : LinkGoalBase(link_name, weight) , axis(axis) , direction(direction) { } - const tf::Vector3& getAxis() const { return axis; } - const tf::Vector3& getDirection() const { return direction; } - void setAxis(const tf::Vector3& a) { axis = a.normalized(); } - void setDirection(const tf::Vector3& d) { direction = d.normalized(); } + const tf2::Vector3& getAxis() const { return axis; } + const tf2::Vector3& getDirection() const { return direction; } + void setAxis(const tf2::Vector3& a) { axis = a.normalized(); } + void setDirection(const tf2::Vector3& d) { direction = d.normalized(); } virtual double evaluate(const GoalContext& context) const { auto& fb = context.getLinkFrame(); @@ -593,22 +594,22 @@ class DirectionGoal : public LinkGoalBase class ConeGoal : public LinkGoalBase { - tf::Vector3 position; + tf2::Vector3 position; double position_weight; - tf::Vector3 axis; - tf::Vector3 direction; + tf2::Vector3 axis; + tf2::Vector3 direction; double angle; public: - const tf::Vector3& getPosition() const { return position; } + const tf2::Vector3& getPosition() const { return position; } double getPositionWeight() const { return position_weight; } - const tf::Vector3& getAxis() const { return axis; } - const tf::Vector3& getDirection() const { return direction; } + const tf2::Vector3& getAxis() const { return axis; } + const tf2::Vector3& getDirection() const { return direction; } double getAngle() const { return angle; } - void setPosition(const tf::Vector3& p) { position = p; } + void setPosition(const tf2::Vector3& p) { position = p; } void setPositionWeight(double w) { position_weight = w; } - void setAxis(const tf::Vector3& a) { axis = a.normalized(); } - void setDirection(const tf::Vector3& d) { direction = d.normalized(); } + void setAxis(const tf2::Vector3& a) { axis = a.normalized(); } + void setDirection(const tf2::Vector3& d) { direction = d.normalized(); } void setAngle(double a) { angle = a; } ConeGoal() : position(0, 0, 0) @@ -618,7 +619,7 @@ class ConeGoal : public LinkGoalBase , angle(0) { } - ConeGoal(const std::string& link_name, const tf::Vector3& axis, const tf::Vector3& direction, double angle, double weight = 1.0) + ConeGoal(const std::string& link_name, const tf2::Vector3& axis, const tf2::Vector3& direction, double angle, double weight = 1.0) : LinkGoalBase(link_name, weight) , position(0, 0, 0) , position_weight(0) @@ -627,7 +628,7 @@ class ConeGoal : public LinkGoalBase , angle(angle) { } - ConeGoal(const std::string& link_name, const tf::Vector3& position, const tf::Vector3& axis, const tf::Vector3& direction, double angle, double weight = 1.0) + ConeGoal(const std::string& link_name, const tf2::Vector3& position, const tf2::Vector3& axis, const tf2::Vector3& direction, double angle, double weight = 1.0) : LinkGoalBase(link_name, weight) , position(position) , position_weight(1) @@ -636,7 +637,7 @@ class ConeGoal : public LinkGoalBase , angle(angle) { } - ConeGoal(const std::string& link_name, const tf::Vector3& position, double position_weight, const tf::Vector3& axis, const tf::Vector3& direction, double angle, double weight = 1.0) + ConeGoal(const std::string& link_name, const tf2::Vector3& position, double position_weight, const tf2::Vector3& axis, const tf2::Vector3& direction, double angle, double weight = 1.0) : LinkGoalBase(link_name, weight) , position(position) , position_weight(position_weight) diff --git a/package.xml b/package.xml index 86138dc..13903c3 100644 --- a/package.xml +++ b/package.xml @@ -29,11 +29,15 @@ roscpp roscpp - tf - tf - - tf_conversions - tf_conversions + tf2 + tf2 + + tf2_eigen + tf2_eigen + tf2_kdl + tf2_kdl + tf2_geometry_msgs + tf2_geometry_msgs rosunit diff --git a/src/forward_kinematics.h b/src/forward_kinematics.h index 904a1f7..94c5662 100644 --- a/src/forward_kinematics.h +++ b/src/forward_kinematics.h @@ -1439,9 +1439,9 @@ class RobotFK_Mutator_2 : public RobotFK_Mutator auto& link = link_approximators[tip_links[itip]->getLinkIndex()]; tip_mutation.pos = link.position; - tip_mutation.rot = tf::Quaternion(link.rotation.x() * 0.5, link.rotation.y() * 0.5, link.rotation.z() * 0.5, 1.0) * tip_frame.rot; + tip_mutation.rot = tf2::Quaternion(link.rotation.x() * 0.5, link.rotation.y() * 0.5, link.rotation.z() * 0.5, 1.0) * tip_frame.rot; - /*tip_mutation.rot = tf::Quaternion( + /*tip_mutation.rot = tf2::Quaternion( link.rotation.x() * 0.5, link.rotation.y() * 0.5, link.rotation.z() * 0.5, diff --git a/src/goal_types.cpp b/src/goal_types.cpp index a642a58..b58f408 100644 --- a/src/goal_types.cpp +++ b/src/goal_types.cpp @@ -235,7 +235,7 @@ void BalanceGoal::describe(GoalContext& context) const if(!link_urdf) continue; if(!link_urdf->inertial) continue; const auto& center_urdf = link_urdf->inertial->origin.position; - tf::Vector3 center(center_urdf.x, center_urdf.y, center_urdf.z); + tf2::Vector3 center(center_urdf.x, center_urdf.y, center_urdf.z); double mass = link_urdf->inertial->mass; if(!(mass > 0)) continue; balance_infos.emplace_back(); @@ -252,7 +252,7 @@ void BalanceGoal::describe(GoalContext& context) const double BalanceGoal::evaluate(const GoalContext& context) const { - tf::Vector3 center = tf::Vector3(0, 0, 0); + tf2::Vector3 center(0, 0, 0); for(size_t i = 0; i < balance_infos.size(); i++) { auto& info = balance_infos[i]; diff --git a/src/ik_neural.cpp b/src/ik_neural.cpp index 8c4c2a3..1a6c652 100644 --- a/src/ik_neural.cpp +++ b/src/ik_neural.cpp @@ -555,7 +555,7 @@ struct IKNeural2 : IKBase auto rot = frames[itip].rot; rot = rot * rot; - // rot = tf::Quaternion(0, 0, 0, 1); + // rot = tf2::Quaternion(0, 0, 0, 1); inputs.push_back(rot.x()); inputs.push_back(rot.y()); inputs.push_back(rot.z()); @@ -662,7 +662,7 @@ struct IKNeural2 : IKBase auto rot = frames[itip].rot; rot = rot * rot; - // rot = tf::Quaternion(0, 0, 0, 1); + // rot = tf2::Quaternion(0, 0, 0, 1); inputs.push_back(rot.x()); inputs.push_back(rot.y()); inputs.push_back(rot.z()); diff --git a/src/kinematics_plugin.cpp b/src/kinematics_plugin.cpp index 0f5049b..bf54f3d 100644 --- a/src/kinematics_plugin.cpp +++ b/src/kinematics_plugin.cpp @@ -51,7 +51,7 @@ #include #include -#include +#include //#include #include #include @@ -467,7 +467,7 @@ struct BioIKKinematicsPlugin : kinematics::KinematicsBase { tipFrames.clear(); for (size_t i = 0; i < ik_poses.size(); i++) { Eigen::Affine3d p, r; - tf::poseMsgToEigen(ik_poses[i], p); + tf2::fromMsg(ik_poses[i], p); if (context_state) { r = context_state->getGlobalLinkTransform(getBaseFrame()); } else { diff --git a/src/problem.h b/src/problem.h index da8d260..513f89d 100644 --- a/src/problem.h +++ b/src/problem.h @@ -103,9 +103,9 @@ class Problem double rotation_scale; double rotation_scale_sq; Frame frame; - tf::Vector3 target; - tf::Vector3 direction; - tf::Vector3 axis; + tf2::Vector3 target; + tf2::Vector3 direction; + tf2::Vector3 axis; double distance; ssize_t active_variable_index; double variable_position; diff --git a/src/utils.h b/src/utils.h index a8edc22..6c219df 100644 --- a/src/utils.h +++ b/src/utils.h @@ -484,22 +484,22 @@ class XmlRpcReader XmlRpcReader at(int i) { return v[i]; } void conv(bool& r) { r = (bool)v; } void conv(double& r) { r = (v.getType() == var::TypeInt) ? ((double)(int)v) : ((double)v); } - void conv(tf::Vector3& r) + void conv(tf2::Vector3& r) { double x, y, z; at(0).conv(x); at(1).conv(y); at(2).conv(z); - r = tf::Vector3(x, y, z); + r = tf2::Vector3(x, y, z); } - void conv(tf::Quaternion& r) + void conv(tf2::Quaternion& r) { double x, y, z, w; at(0).conv(x); at(1).conv(y); at(2).conv(z); at(3).conv(w); - r = tf::Quaternion(x, y, z, w).normalized(); + r = tf2::Quaternion(x, y, z, w).normalized(); } void conv(std::string& r) { r = (std::string)v; } diff --git a/test/utest.cpp b/test/utest.cpp index 080e24f..3492398 100644 --- a/test/utest.cpp +++ b/test/utest.cpp @@ -20,9 +20,9 @@ template Frame random_frame(RNG& rng) { std::uniform_real_distribution dist(-1, 1); - tf::Quaternion q(dist(rng), dist(rng), dist(rng), dist(rng)); + tf2::Quaternion q(dist(rng), dist(rng), dist(rng), dist(rng)); q.normalize(); - tf::Vector3 v(dist(rng), dist(rng), dist(rng)); + tf2::Vector3 v(dist(rng), dist(rng), dist(rng)); return Frame(v, q); }