Skip to content

Commit 4143d6b

Browse files
evan-palmerevan-palmer
committed
testing ik_solver lib in whole_body_controllers
1 parent eee3c79 commit 4143d6b

File tree

10 files changed

+934
-10
lines changed

10 files changed

+934
-10
lines changed

‎ik_solvers/CMakeLists.txt

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -72,7 +72,6 @@ install(
7272
FILE_SET HEADERS
7373
)
7474

75-
# ament_export_include_directories(include)
7675
ament_export_libraries(ik_solvers)
7776
ament_export_targets(export_ik_solvers)
7877
ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})

‎ik_solvers/COLCON_IGNORE

Whitespace-only changes.

‎whole_body_controllers/CMakeLists.txt

Lines changed: 50 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -9,21 +9,23 @@ include(GNUInstallDirs)
99

1010
# dependencies
1111
set(THIS_PACKAGE_INCLUDE_DEPENDS
12+
controller_common
1213
controller_interface
14+
control_msgs
15+
Eigen3
16+
eigen3_cmake_module
17+
geometry_msgs
1318
hardware_interface
19+
message_transforms
20+
nav_msgs
21+
pinocchio
1422
rclcpp
1523
rclcpp_lifecycle
1624
realtime_tools
17-
nav_msgs
1825
std_msgs
19-
control_msgs
20-
pinocchio
21-
geometry_msgs
2226
tf2_ros
2327
tf2_eigen
24-
message_transforms
25-
controller_common
26-
ik_solvers
28+
trajectory_msgs
2729
)
2830

2931
foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
@@ -38,6 +40,45 @@ generate_parameter_library(ik_controller_parameters
3840
src/ik_controller_parameters.yaml
3941
)
4042

43+
generate_parameter_library(task_priority_solver_parameters
44+
src/ik_solvers/task_priority_solver_parameters.yaml
45+
)
46+
47+
# ik base solver library
48+
# this needs to be independent of the plugin library
49+
add_library(solver_base SHARED)
50+
target_sources(
51+
solver_base
52+
PRIVATE src/ik_solvers/solver.cpp
53+
PUBLIC
54+
FILE_SET HEADERS
55+
BASE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include
56+
FILES ${CMAKE_CURRENT_SOURCE_DIR}/include/whole_body_controllers/ik_solvers/solver.hpp
57+
)
58+
ament_target_dependencies(solver_base PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS})
59+
target_compile_features(solver_base PUBLIC cxx_std_23)
60+
61+
# ik solver plugin library
62+
add_library(ik_solvers SHARED)
63+
target_sources(
64+
ik_solvers
65+
PRIVATE src/ik_solvers/task_priority_solver.cpp
66+
PUBLIC
67+
FILE_SET HEADERS
68+
BASE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include
69+
FILES
70+
${CMAKE_CURRENT_SOURCE_DIR}/include/whole_body_controllers/ik_solvers/task_priority_solver.hpp
71+
)
72+
ament_target_dependencies(ik_solvers PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS})
73+
target_compile_features(ik_solvers PUBLIC cxx_std_23)
74+
target_link_libraries(
75+
ik_solvers
76+
PUBLIC solver_base task_priority_solver_parameters
77+
)
78+
79+
# the first argument is the name of the package, NOT the target name
80+
pluginlib_export_plugin_description_file(ik_solvers solvers.xml)
81+
4182
add_library(whole_body_controllers SHARED)
4283
target_sources(
4384
whole_body_controllers
@@ -50,11 +91,11 @@ target_sources(
5091
)
5192
ament_target_dependencies(whole_body_controllers PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS})
5293
target_compile_features(whole_body_controllers PUBLIC cxx_std_23)
53-
target_link_libraries(whole_body_controllers PUBLIC ik_controller_parameters)
94+
target_link_libraries(whole_body_controllers PUBLIC ik_controller_parameters ik_solvers)
5495
pluginlib_export_plugin_description_file(controller_interface whole_body_controllers.xml)
5596

5697
install(
57-
TARGETS whole_body_controllers ik_controller_parameters
98+
TARGETS whole_body_controllers ik_controller_parameters solver_base ik_solvers task_priority_solver_parameters
5899
EXPORT export_whole_body_controllers
59100
RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}
60101
LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}

‎whole_body_controllers/duplicate.txt

Lines changed: 68 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,68 @@
1+
cmake_minimum_required(VERSION 3.23)
2+
project(whole_body_controllers)
3+
4+
if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)")
5+
add_compile_options(-Wall -Wextra -Wpedantic)
6+
endif()
7+
8+
include(GNUInstallDirs)
9+
10+
# dependencies
11+
set(THIS_PACKAGE_INCLUDE_DEPENDS
12+
controller_interface
13+
hardware_interface
14+
rclcpp
15+
rclcpp_lifecycle
16+
realtime_tools
17+
nav_msgs
18+
std_msgs
19+
control_msgs
20+
pinocchio
21+
geometry_msgs
22+
tf2_ros
23+
tf2_eigen
24+
message_transforms
25+
controller_common
26+
ik_solvers
27+
)
28+
29+
foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
30+
find_package(${Dependency} REQUIRED)
31+
endforeach()
32+
33+
find_package(ament_cmake REQUIRED)
34+
find_package(generate_parameter_library REQUIRED)
35+
find_package(pluginlib REQUIRED)
36+
37+
generate_parameter_library(ik_controller_parameters
38+
src/ik_controller_parameters.yaml
39+
)
40+
41+
add_library(whole_body_controllers SHARED)
42+
target_sources(
43+
whole_body_controllers
44+
PRIVATE src/ik_controller.cpp
45+
PUBLIC
46+
FILE_SET HEADERS
47+
BASE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include
48+
FILES
49+
${CMAKE_CURRENT_SOURCE_DIR}/include/whole_body_controllers/ik_controller.hpp
50+
)
51+
ament_target_dependencies(whole_body_controllers PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS})
52+
target_compile_features(whole_body_controllers PUBLIC cxx_std_23)
53+
target_link_libraries(whole_body_controllers PUBLIC ik_controller_parameters)
54+
pluginlib_export_plugin_description_file(controller_interface whole_body_controllers.xml)
55+
56+
install(
57+
TARGETS whole_body_controllers ik_controller_parameters
58+
EXPORT export_whole_body_controllers
59+
RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}
60+
LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}
61+
ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR}
62+
FILE_SET HEADERS
63+
)
64+
65+
ament_export_targets(export_whole_body_controllers HAS_LIBRARY_TARGET)
66+
ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
67+
68+
ament_package()

‎whole_body_controllers/include/whole_body_controllers/ik_solvers/solver.hpp

Lines changed: 81 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,81 @@
1+
// Copyright 2025, Evan Palmer
2+
//
3+
// Permission is hereby granted, free of charge, to any person obtaining a copy
4+
// of this software and associated documentation files (the "Software"), to deal
5+
// in the Software without restriction, including without limitation the rights
6+
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
7+
// copies of the Software, and to permit persons to whom the Software is
8+
// furnished to do so, subject to the following conditions:
9+
//
10+
// The above copyright notice and this permission notice shall be included in all
11+
// copies or substantial portions of the Software.
12+
//
13+
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
14+
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
15+
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
16+
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
17+
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
18+
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
19+
// SOFTWARE.
20+
21+
#pragma once
22+
23+
#include <Eigen/Geometry>
24+
#include <cstdint>
25+
#include <expected>
26+
27+
#include "pinocchio/algorithm/joint-configuration.hpp"
28+
#include "rclcpp_lifecycle/lifecycle_node.hpp"
29+
#include "trajectory_msgs/msg/joint_trajectory_point.hpp"
30+
31+
namespace ik_solvers
32+
{
33+
34+
/// Error codes for the inverse kinematics solvers.
35+
enum class SolverError : std::uint8_t
36+
{
37+
NO_SOLUTION,
38+
SOLVER_ERROR
39+
};
40+
41+
/// Base class for inverse kinematics solvers.
42+
class IKSolver
43+
{
44+
public:
45+
/// Constructor.
46+
IKSolver() = default;
47+
48+
/// Destructor.
49+
virtual ~IKSolver() = default;
50+
51+
/// Initialize the solver.
52+
virtual auto initialize(
53+
const std::shared_ptr<rclcpp_lifecycle::LifecycleNode> & node,
54+
const std::shared_ptr<pinocchio::Model> & model,
55+
const std::shared_ptr<pinocchio::Data> & data,
56+
const std::string & prefix) -> void;
57+
58+
/// Solve the IK problem for a target pose given the integration period and current joint configuration.
59+
[[nodiscard]] auto solve(const rclcpp::Duration & period, const Eigen::Affine3d & goal, const Eigen::VectorXd & q)
60+
-> std::expected<trajectory_msgs::msg::JointTrajectoryPoint, SolverError>;
61+
62+
protected:
63+
/// Solve the IK problem for the given target pose and joint configuration.
64+
///
65+
/// This is wrapped by the public API. The public API handles updating pinocchio for Jacobian calculation and converts
66+
/// of the result into a `JointTrajectoryPoint`. This method only needs to compute the IK solution.
67+
[[nodiscard]] virtual auto solve_ik(const Eigen::Affine3d & goal, const Eigen::VectorXd & q)
68+
-> std::expected<Eigen::VectorXd, SolverError> = 0;
69+
70+
std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node_;
71+
72+
std::string ee_frame_;
73+
74+
std::shared_ptr<pinocchio::Model> model_;
75+
std::shared_ptr<pinocchio::Data> data_;
76+
77+
private:
78+
auto update_pinocchio(const Eigen::VectorXd & q) const -> void;
79+
};
80+
81+
} // namespace ik_solvers

0 commit comments

Comments
 (0)