Skip to content

Commit 6f2f968

Browse files
christophfroehlichmergify[bot]
christophfroehlich
authored and
mergify[bot]
committed
Use ros2_control_cmake (#118)
(cherry picked from commit 048a964) # Conflicts: # kinematics_interface/src/kinematics_interface.cpp # kinematics_interface_kdl/src/kinematics_interface_kdl.cpp # kinematics_interface_kdl/test/test_kinematics_interface_kdl.cpp
1 parent 6d866d0 commit 6f2f968

9 files changed

+130
-34
lines changed

‎kinematics_interface.jazzy.repos

+4
Original file line numberDiff line numberDiff line change
@@ -12,3 +12,7 @@ repositories:
1212
type: git
1313
url: https://github.com/ros-controls/control_msgs.git
1414
version: jazzy
15+
ros2_control_cmake:
16+
type: git
17+
url: https://github.com/ros-controls/ros2_control_cmake.git
18+
version: master

‎kinematics_interface.rolling.repos

+4
Original file line numberDiff line numberDiff line change
@@ -12,3 +12,7 @@ repositories:
1212
type: git
1313
url: https://github.com/ros-controls/control_msgs.git
1414
version: master
15+
ros2_control_cmake:
16+
type: git
17+
url: https://github.com/ros-controls/ros2_control_cmake.git
18+
version: master

‎kinematics_interface/CMakeLists.txt

+3-7
Original file line numberDiff line numberDiff line change
@@ -1,13 +1,9 @@
11
cmake_minimum_required(VERSION 3.16)
22
project(kinematics_interface LANGUAGES CXX)
33

4-
if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)")
5-
add_compile_options(-Wall -Wextra -Wpedantic)
6-
endif()
7-
8-
# using this instead of visibility macros
9-
# S1 from https://github.com/ros-controls/ros2_controllers/issues/1053
10-
set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON)
4+
find_package(ros2_control_cmake REQUIRED)
5+
set_compiler_options()
6+
export_windows_symbols()
117

128
set(THIS_PACKAGE_INCLUDE_DEPENDS
139
rclcpp

‎kinematics_interface/package.xml

+2
Original file line numberDiff line numberDiff line change
@@ -12,6 +12,8 @@
1212

1313
<buildtool_depend>ament_cmake</buildtool_depend>
1414

15+
<build_depend>ros2_control_cmake</build_depend>
16+
1517
<depend>eigen</depend>
1618
<depend>rclcpp_lifecycle</depend>
1719

‎kinematics_interface/src/kinematics_interface.cpp

+54-12
Original file line numberDiff line numberDiff line change
@@ -26,16 +26,18 @@ bool KinematicsInterface::convert_cartesian_deltas_to_joint_deltas(
2626
std::vector<double> & joint_pos_vec, const std::vector<double> & delta_x_vec,
2727
const std::string & link_name, std::vector<double> & delta_theta_vec)
2828
{
29-
auto joint_pos = Eigen::Map<const Eigen::VectorXd>(joint_pos_vec.data(), joint_pos_vec.size());
30-
auto delta_x = Eigen::Map<const Eigen::VectorXd>(delta_x_vec.data(), delta_x_vec.size());
29+
auto joint_pos = Eigen::Map<const Eigen::VectorXd>(
30+
joint_pos_vec.data(), static_cast<Eigen::Index>(joint_pos_vec.size()));
31+
auto delta_x = Eigen::Map<const Eigen::VectorXd>(
32+
delta_x_vec.data(), static_cast<Eigen::Index>(delta_x_vec.size()));
3133
// TODO(anyone): heap allocation should be removed for realtime use
32-
Eigen::VectorXd delta_theta =
33-
Eigen::Map<Eigen::VectorXd>(delta_theta_vec.data(), delta_theta_vec.size());
34+
Eigen::VectorXd delta_theta = Eigen::Map<Eigen::VectorXd>(
35+
delta_theta_vec.data(), static_cast<Eigen::Index>(delta_theta_vec.size()));
3436

3537
bool ret = convert_cartesian_deltas_to_joint_deltas(joint_pos, delta_x, link_name, delta_theta);
3638
for (auto i = 0ul; i < delta_theta_vec.size(); i++)
3739
{
38-
delta_theta_vec[i] = delta_theta[i];
40+
delta_theta_vec[i] = delta_theta[static_cast<Eigen::Index>(i)];
3941
}
4042
return ret;
4143
}
@@ -44,9 +46,10 @@ bool KinematicsInterface::convert_joint_deltas_to_cartesian_deltas(
4446
const std::vector<double> & joint_pos_vec, const std::vector<double> & delta_theta_vec,
4547
const std::string & link_name, std::vector<double> & delta_x_vec)
4648
{
47-
auto joint_pos = Eigen::Map<const Eigen::VectorXd>(joint_pos_vec.data(), joint_pos_vec.size());
48-
Eigen::VectorXd delta_theta =
49-
Eigen::Map<const Eigen::VectorXd>(delta_theta_vec.data(), delta_theta_vec.size());
49+
auto joint_pos = Eigen::Map<const Eigen::VectorXd>(
50+
joint_pos_vec.data(), static_cast<Eigen::Index>(joint_pos_vec.size()));
51+
Eigen::VectorXd delta_theta = Eigen::Map<const Eigen::VectorXd>(
52+
delta_theta_vec.data(), static_cast<Eigen::Index>(delta_theta_vec.size()));
5053
if (delta_x_vec.size() != 6)
5154
{
5255
RCLCPP_ERROR(
@@ -57,7 +60,7 @@ bool KinematicsInterface::convert_joint_deltas_to_cartesian_deltas(
5760
bool ret = convert_joint_deltas_to_cartesian_deltas(joint_pos, delta_theta, link_name, delta_x);
5861
for (auto i = 0ul; i < delta_x_vec.size(); i++)
5962
{
60-
delta_x_vec[i] = delta_x[i];
63+
delta_x_vec[i] = delta_x[static_cast<Eigen::Index>(i)];
6164
}
6265
return ret;
6366
}
@@ -66,7 +69,8 @@ bool KinematicsInterface::calculate_link_transform(
6669
const std::vector<double> & joint_pos_vec, const std::string & link_name,
6770
Eigen::Isometry3d & transform)
6871
{
69-
auto joint_pos = Eigen::Map<const Eigen::VectorXd>(joint_pos_vec.data(), joint_pos_vec.size());
72+
auto joint_pos = Eigen::Map<const Eigen::VectorXd>(
73+
joint_pos_vec.data(), static_cast<Eigen::Index>(joint_pos_vec.size()));
7074

7175
return calculate_link_transform(joint_pos, link_name, transform);
7276
}
@@ -75,7 +79,8 @@ bool KinematicsInterface::calculate_jacobian(
7579
const std::vector<double> & joint_pos_vec, const std::string & link_name,
7680
Eigen::Matrix<double, 6, Eigen::Dynamic> & jacobian)
7781
{
78-
auto joint_pos = Eigen::Map<const Eigen::VectorXd>(joint_pos_vec.data(), joint_pos_vec.size());
82+
auto joint_pos = Eigen::Map<const Eigen::VectorXd>(
83+
joint_pos_vec.data(), static_cast<Eigen::Index>(joint_pos_vec.size()));
7984

8085
return calculate_jacobian(joint_pos, link_name, jacobian);
8186
}
@@ -84,9 +89,46 @@ bool KinematicsInterface::calculate_jacobian_inverse(
8489
const std::vector<double> & joint_pos_vec, const std::string & link_name,
8590
Eigen::Matrix<double, Eigen::Dynamic, 6> & jacobian_inverse)
8691
{
87-
auto joint_pos = Eigen::Map<const Eigen::VectorXd>(joint_pos_vec.data(), joint_pos_vec.size());
92+
auto joint_pos = Eigen::Map<const Eigen::VectorXd>(
93+
joint_pos_vec.data(), static_cast<Eigen::Index>(joint_pos_vec.size()));
8894

8995
return calculate_jacobian_inverse(joint_pos, link_name, jacobian_inverse);
9096
}
9197

98+
<<<<<<< HEAD
99+
=======
100+
bool KinematicsInterface::calculate_frame_difference(
101+
std::vector<double> & x_a_vec, std::vector<double> & x_b_vec, double dt,
102+
std::vector<double> & delta_x_vec)
103+
{
104+
if (x_a_vec.size() != 7)
105+
{
106+
RCLCPP_ERROR(
107+
LOGGER, "The length of the first cartesian vector (%zu) must be 7.", x_a_vec.size());
108+
return false;
109+
}
110+
Eigen::Matrix<double, 7, 1> x_a(x_a_vec.data());
111+
if (x_b_vec.size() != 7)
112+
{
113+
RCLCPP_ERROR(
114+
LOGGER, "The length of the second cartesian vector (%zu) must be 7.", x_b_vec.size());
115+
return false;
116+
}
117+
Eigen::Matrix<double, 7, 1> x_b(x_b_vec.data());
118+
if (delta_x_vec.size() != 6)
119+
{
120+
RCLCPP_ERROR(
121+
LOGGER, "The length of the cartesian delta vector (%zu) must be 6.", delta_x_vec.size());
122+
return false;
123+
}
124+
Eigen::Matrix<double, 6, 1> delta_x(delta_x_vec.data());
125+
bool ret = calculate_frame_difference(x_a, x_b, dt, delta_x);
126+
for (auto i = 0ul; i < delta_x_vec.size(); i++)
127+
{
128+
delta_x_vec[i] = delta_x[static_cast<Eigen::Index>(i)];
129+
}
130+
return ret;
131+
}
132+
133+
>>>>>>> 048a964 (Use ros2_control_cmake (#118))
92134
} // namespace kinematics_interface

‎kinematics_interface_kdl/CMakeLists.txt

+3-7
Original file line numberDiff line numberDiff line change
@@ -1,13 +1,9 @@
11
cmake_minimum_required(VERSION 3.16)
22
project(kinematics_interface_kdl LANGUAGES CXX)
33

4-
if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)")
5-
add_compile_options(-Wall -Wextra -Wpedantic)
6-
endif()
7-
8-
# using this instead of visibility macros
9-
# S1 from https://github.com/ros-controls/ros2_controllers/issues/1053
10-
set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON)
4+
find_package(ros2_control_cmake REQUIRED)
5+
set_compiler_options()
6+
export_windows_symbols()
117

128
set(THIS_PACKAGE_INCLUDE_DEPENDS
139
kdl_parser

‎kinematics_interface_kdl/package.xml

+2
Original file line numberDiff line numberDiff line change
@@ -13,6 +13,8 @@
1313
<buildtool_depend>ament_cmake</buildtool_depend>
1414
<buildtool_depend>eigen3_cmake_module</buildtool_depend>
1515

16+
<build_depend>ros2_control_cmake</build_depend>
17+
1618
<depend>eigen</depend>
1719
<depend>kdl_parser</depend>
1820
<depend>kinematics_interface</depend>

‎kinematics_interface_kdl/src/kinematics_interface_kdl.cpp

+35-3
Original file line numberDiff line numberDiff line change
@@ -94,11 +94,12 @@ bool KinematicsInterfaceKDL::initialize(
9494
// create map from link names to their index
9595
for (size_t i = 0; i < chain_.getNrOfSegments(); ++i)
9696
{
97-
link_name_map_[chain_.getSegment(i).getName()] = i + 1;
97+
link_name_map_[chain_.getSegment(static_cast<unsigned int>(i)).getName()] =
98+
static_cast<int>(i) + 1;
9899
}
99100
// allocate dynamics memory
100101
num_joints_ = chain_.getNrOfJoints();
101-
q_ = KDL::JntArray(num_joints_);
102+
q_ = KDL::JntArray(static_cast<unsigned int>(num_joints_));
102103
I = Eigen::MatrixXd(num_joints_, num_joints_);
103104
I.setIdentity();
104105
// create KDL solvers
@@ -230,8 +231,39 @@ bool KinematicsInterfaceKDL::calculate_link_transform(
230231
}
231232

232233
// create forward kinematics solver
234+
<<<<<<< HEAD
233235
fk_pos_solver_->JntToCart(q_, frame_, link_name_map_[link_name]);
234236
tf2::transformKDLToEigen(frame_, transform);
237+
=======
238+
fk_pos_solver_->JntToCart(q_, frames_(0), link_name_map_[link_name]);
239+
tf2::transformKDLToEigen(frames_(0), transform);
240+
return true;
241+
}
242+
243+
bool KinematicsInterfaceKDL::calculate_frame_difference(
244+
Eigen::Matrix<double, 7, 1> & x_a, Eigen::Matrix<double, 7, 1> & x_b, double dt,
245+
Eigen::Matrix<double, 6, 1> & delta_x)
246+
{
247+
// verify inputs
248+
if (!verify_initialized() || !verify_period(dt))
249+
{
250+
return false;
251+
}
252+
253+
// get frames
254+
frames_(0) = KDL::Frame(
255+
KDL::Rotation::Quaternion(x_a(3), x_a(4), x_a(5), x_a(6)), KDL::Vector(x_a(0), x_a(1), x_a(2)));
256+
frames_(1) = KDL::Frame(
257+
KDL::Rotation::Quaternion(x_b(3), x_b(4), x_b(5), x_b(6)), KDL::Vector(x_b(0), x_b(1), x_b(2)));
258+
259+
// compute the difference
260+
delta_x_ = KDL::diff(frames_(0), frames_(1), dt);
261+
for (size_t i = 0; i < 6; ++i)
262+
{
263+
delta_x(static_cast<Eigen::Index>(i)) = delta_x_[static_cast<int>(i)];
264+
}
265+
266+
>>>>>>> 048a964 (Use ros2_control_cmake (#118))
235267
return true;
236268
}
237269

@@ -244,7 +276,7 @@ bool KinematicsInterfaceKDL::verify_link_name(const std::string & link_name)
244276
if (link_name_map_.find(link_name) == link_name_map_.end())
245277
{
246278
std::string links;
247-
for (size_t i = 0; i < chain_.getNrOfSegments(); ++i)
279+
for (unsigned int i = 0; i < static_cast<unsigned int>(chain_.getNrOfSegments()); ++i)
248280
{
249281
links += "\n" + chain_.getSegment(i).getName();
250282
}

‎kinematics_interface_kdl/test/test_kinematics_interface_kdl.cpp

+23-5
Original file line numberDiff line numberDiff line change
@@ -104,7 +104,7 @@ TEST_F(TestKDLPlugin, KDL_plugin_function)
104104
ik_->convert_joint_deltas_to_cartesian_deltas(pos, delta_theta, end_effector_, delta_x_est));
105105

106106
// Ensure kinematics math is correct
107-
for (size_t i = 0; i < static_cast<size_t>(delta_x.size()); ++i)
107+
for (Eigen::Index i = 0; i < delta_x.size(); ++i)
108108
{
109109
ASSERT_NEAR(delta_x[i], delta_x_est[i], 0.02);
110110
}
@@ -121,13 +121,31 @@ TEST_F(TestKDLPlugin, KDL_plugin_function)
121121
ASSERT_TRUE(ik_->calculate_jacobian_inverse(pos, end_effector_, jacobian_inverse_est));
122122

123123
// ensure jacobian inverse math is correct
124-
for (size_t i = 0; i < static_cast<size_t>(jacobian_inverse.rows()); ++i)
124+
for (Eigen::Index i = 0; i < jacobian_inverse.rows(); ++i)
125125
{
126-
for (size_t j = 0; j < static_cast<size_t>(jacobian_inverse.cols()); ++j)
126+
for (Eigen::Index j = 0; j < jacobian_inverse.cols(); ++j)
127127
{
128128
ASSERT_NEAR(jacobian_inverse(i, j), jacobian_inverse_est(i, j), 0.02);
129129
}
130130
}
131+
<<<<<<< HEAD
132+
=======
133+
134+
// compute the difference between two cartesian frames
135+
Eigen::Matrix<double, 7, 1> x_a, x_b;
136+
x_a << 0, 1, 0, 0, 0, 0, 1;
137+
x_b << 2, 3, 0, 0, 1, 0, 0;
138+
double dt = 1.0;
139+
delta_x = Eigen::Matrix<double, 6, 1>::Zero();
140+
delta_x_est << 2, 2, 0, 0, 3.14, 0;
141+
ASSERT_TRUE(ik_->calculate_frame_difference(x_a, x_b, dt, delta_x));
142+
143+
// ensure that difference math is correct
144+
for (Eigen::Index i = 0; i < delta_x.size(); ++i)
145+
{
146+
ASSERT_NEAR(delta_x(i), delta_x_est(i), 0.02);
147+
}
148+
>>>>>>> 048a964 (Use ros2_control_cmake (#118))
131149
}
132150

133151
TEST_F(TestKDLPlugin, KDL_plugin_function_std_vector)
@@ -175,9 +193,9 @@ TEST_F(TestKDLPlugin, KDL_plugin_function_std_vector)
175193
ASSERT_TRUE(ik_->calculate_jacobian_inverse(pos, end_effector_, jacobian_inverse_est));
176194

177195
// ensure jacobian inverse math is correct
178-
for (size_t i = 0; i < static_cast<size_t>(jacobian_inverse.rows()); ++i)
196+
for (Eigen::Index i = 0; i < jacobian_inverse.rows(); ++i)
179197
{
180-
for (size_t j = 0; j < static_cast<size_t>(jacobian_inverse.cols()); ++j)
198+
for (Eigen::Index j = 0; j < jacobian_inverse.cols(); ++j)
181199
{
182200
ASSERT_NEAR(jacobian_inverse(i, j), jacobian_inverse_est(i, j), 0.02);
183201
}

0 commit comments

Comments
 (0)