ros2的moveit进行正反解进行计算及不同求解器对于工作空间的某个位姿使用ros2的moveit进行正反解进行计算已及不同求解器的选择 在ros2程序中使用一个完整包体现在ROS2中要完整体现 MoveIt2的正逆运动学计算以及不同求解器的选择 最规范的做法是创建一个独立的ROS2功能包。 下面一个完整的工程结构包含C源码、编译配置文件CMakeLists.txt 和package.xml 以及控制求解器选择的配置文件kinematics.yaml。完整的 ROS 2 功能包结构假设将功能包命名为 cpp_kinematics_demo其标准目录结构如下cpp_kinematics_demo/├── CMakeLists.txt ├── package.xml ├── config/│ └── kinematics.yaml # 核心不同求解器的选择配置文件 ├── launch/│ └── kinematics_demo.launch.py # 启动文件加载配置并运行节点 └── src/└── kinematics_node.cpp # C源码实现正反解计算逻辑1C 源码 (src/kinematics_node.cpp)这段代码展示了如何通过 MoveGroupInterface 获取当前位姿正解 FK以及如何设定目标位姿触发逆解IK并规划。#includerclcpp/rclcpp.hpp#includemoveit/move_group_interface/move_group_interface.h#includegeometry_msgs/msg/pose.hppintmain(intargc,char**argv){rclcpp::init(argc,argv);rclcpp::Node::SharedPtr noderclcpp::Node::make_shared(cpp_kinematics_node);// 异步旋转器处理后台回调rclcpp::executors::SingleThreadedExecutor executor;executor.add_node(node);std::thread([executor](){executor.spin();}).detach();// 假设规划组名为 panda_arm末端连杆名为 panda_handconststd::string PLANNING_GROUPpanda_arm;conststd::string END_EFFECTOR_LINKpanda_hand;// 初始化 MoveIt 2 接口automove_groupmoveit::planning_interface::MoveGroupInterface(node,PLANNING_GROUP);RCLCPP_INFO(node-get_logger(),MoveIt 2 接口初始化成功);// 正运动学 (FK) // 获取当前末端执行器的实际位姿geometry_msgs::msg::Pose current_posemove_group.getCurrentPose(END_EFFECTOR_LINK).pose;RCLCPP_INFO(node-get_logger(),[FK] 当前末端位姿 - x: %.3f, y: %.3f, z: %.3f,current_pose.position.x,current_pose.position.y,current_pose.position.z);// 逆运动学 (IK) // 设定一个目标位姿在当前 X 基础上偏移 0.1 米geometry_msgs::msg::Pose target_posecurrent_pose;target_pose.position.x0.1;// 设置目标位姿底层会自动调用 kinematics.yaml 中配置的 IK 求解器move_group.setPoseTarget(target_pose,END_EFFECTOR_LINK);// 触发规划如果 IK 求解失败plan 会直接报错moveit::planning_interface::MoveGroupInterface::Plan my_plan;boolsuccess(move_group.plan(my_plan)moveit::core::MoveItErrorCode::SUCCESS);if(success){RCLCPP_INFO(node-get_logger(),[IK] 逆解求解与轨迹规划成功);}else{RCLCPP_ERROR(node-get_logger(),[IK] 逆解求解失败目标位姿可能超出工作空间。);}rclcpp::shutdown();return0;}2求解器配置文件 (config/kinematics.yaml)这是体现“不同求解器选择”的核心文件。程序在运行时会自动读取该文件加载指定的插件。 可以根据需求修改 kinematics_solver 的值panda_arm:# 必须与你的 MoveIt 规划组名称一致 # 求解器选择取消注释你想使用的求解器 #1.默认数值解(KDL)开箱即用但速度慢易受关节限位影响#kinematics_solver:kdl_kinematics_plugin/KDLKinematicsPlugin#2.优化数值解(TRAC-IK)推荐速度快成功率高。需提前 sudo apt install ros-humble-trac-ik-kinematics-plugin kinematics_solver:trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin #3.解析解(IKFast)最快但需提前为特定机械臂生成专属插件#kinematics_solver:panda_moveit_ikfast_plugin/IKFastKinematicsPluginkinematics_solver_timeout:0.005# 求解超时时间秒 kinematics_solver_attempts:3# 尝试次数对 TRAC-IK 影响不大它会自动重启 solve_type:Speed# TRAC-IK 特有追求速度(Speed)或距离最优(Distance)position_only_ik:False # 是否只计算位置忽略姿态3 编译配置文件package.xml (依赖声明):?xml version1.0?package format3namecpp_kinematics_demo/nameversion0.0.1/versiondescriptionDemoforMoveIt2 FK/IK in C/descriptionmaintainer emailuserexample.comuser/maintainerlicenseApache-2.0/licensebuildtool_dependament_cmake/buildtool_dependdependrclcpp/dependdependmoveit_ros_planning_interface/dependdependgeometry_msgs/dependexportbuild_typeament_cmake/build_type/export/packageCMakeLists.txt (编译规则):cmake_minimum_required(VERSION3.8)project(cpp_kinematics_demo)if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHESClang)add_compile_options(-Wall-Wextra-Wpedantic)endif()find_package(ament_cmake REQUIRED)find_package(rclcpp REQUIRED)find_package(moveit_ros_planning_interface REQUIRED)find_package(geometry_msgs REQUIRED)add_executable(kinematics_node src/kinematics_node.cpp)ament_target_dependencies(kinematics_node rclcpp moveit_ros_planning_interface geometry_msgs)install(TARGETS kinematics_node DESTINATION lib/${PROJECT_NAME})# 安装 config 和 launch 文件夹到 share 目录确保运行时能找到install(DIRECTORY config launch DESTINATION share/${PROJECT_NAME})ament_package()4启动文件 (launch/kinematics_demo.launch.py)启动文件负责将你的机械臂 URDF、SRDF 以及最关键的 kinematics.yaml 加载到节点中 urdf可以自己根据模型创建 xacro、srdf和kinematics.yaml可以提供moveit2图形界面根据在配置完之后生成moveit-config包的时候一起生成from launchimportLaunchDescriptionfrom launch_ros.actionsimportNodefrom moveit_configs_utilsimportMoveItConfigsBuilderdefgenerate_launch_description():# 加载 MoveIt 配置这里假设你的机械臂配置包名为 panda_moveit_config # 并明确指定加载我们自定义的 kinematics.yaml moveit_config(MoveItConfigsBuilder(panda,package_namepanda_moveit_config).robot_description(file_pathconfig/panda.urdf.xacro).robot_description_semantic(file_pathconfig/panda.srdf).robot_description_kinematics(file_pathconfig/kinematics.yaml)# 核心加载求解器配置.to_moveit_configs())kinematics_nodeNode(packagecpp_kinematics_demo,executablekinematics_node,outputscreen,parameters[moveit_config.robot_description,moveit_config.robot_description_semantic,moveit_config.robot_description_kinematics,# 传入 kinematics.yaml 参数 moveit_config.joint_limits,],)returnLaunchDescription([kinematics_node])5运行与验证将该功能包放入你的 ROS2工作空间如 colcon_ws/src。 确保你已经安装了 TRAC-IK 插件如果想测试 TRAC-IK sudo apt install ros-humble-trac-ik-kinematics-plugin 编译工作空间colcon build--packages-select cpp_kinematics_demo 启动你的机械臂基础环境例如 Panda 机械臂的 demo ros2 launch panda_moveit_config demo.launch.py 在另一个终端运行你的节点 ros2 launch cpp_kinematics_demo kinematics_demo.launch.py 通过修改 config/kinematics.yaml 中的 kinematics_solver 字段并重新运行可以在同一个 C程序中无缝体验不同 IK 求解器的效果。
ros2-moveit进行正反解进行计算及不同求解器
ros2的moveit进行正反解进行计算及不同求解器对于工作空间的某个位姿使用ros2的moveit进行正反解进行计算已及不同求解器的选择 在ros2程序中使用一个完整包体现在ROS2中要完整体现 MoveIt2的正逆运动学计算以及不同求解器的选择 最规范的做法是创建一个独立的ROS2功能包。 下面一个完整的工程结构包含C源码、编译配置文件CMakeLists.txt 和package.xml 以及控制求解器选择的配置文件kinematics.yaml。完整的 ROS 2 功能包结构假设将功能包命名为 cpp_kinematics_demo其标准目录结构如下cpp_kinematics_demo/├── CMakeLists.txt ├── package.xml ├── config/│ └── kinematics.yaml # 核心不同求解器的选择配置文件 ├── launch/│ └── kinematics_demo.launch.py # 启动文件加载配置并运行节点 └── src/└── kinematics_node.cpp # C源码实现正反解计算逻辑1C 源码 (src/kinematics_node.cpp)这段代码展示了如何通过 MoveGroupInterface 获取当前位姿正解 FK以及如何设定目标位姿触发逆解IK并规划。#includerclcpp/rclcpp.hpp#includemoveit/move_group_interface/move_group_interface.h#includegeometry_msgs/msg/pose.hppintmain(intargc,char**argv){rclcpp::init(argc,argv);rclcpp::Node::SharedPtr noderclcpp::Node::make_shared(cpp_kinematics_node);// 异步旋转器处理后台回调rclcpp::executors::SingleThreadedExecutor executor;executor.add_node(node);std::thread([executor](){executor.spin();}).detach();// 假设规划组名为 panda_arm末端连杆名为 panda_handconststd::string PLANNING_GROUPpanda_arm;conststd::string END_EFFECTOR_LINKpanda_hand;// 初始化 MoveIt 2 接口automove_groupmoveit::planning_interface::MoveGroupInterface(node,PLANNING_GROUP);RCLCPP_INFO(node-get_logger(),MoveIt 2 接口初始化成功);// 正运动学 (FK) // 获取当前末端执行器的实际位姿geometry_msgs::msg::Pose current_posemove_group.getCurrentPose(END_EFFECTOR_LINK).pose;RCLCPP_INFO(node-get_logger(),[FK] 当前末端位姿 - x: %.3f, y: %.3f, z: %.3f,current_pose.position.x,current_pose.position.y,current_pose.position.z);// 逆运动学 (IK) // 设定一个目标位姿在当前 X 基础上偏移 0.1 米geometry_msgs::msg::Pose target_posecurrent_pose;target_pose.position.x0.1;// 设置目标位姿底层会自动调用 kinematics.yaml 中配置的 IK 求解器move_group.setPoseTarget(target_pose,END_EFFECTOR_LINK);// 触发规划如果 IK 求解失败plan 会直接报错moveit::planning_interface::MoveGroupInterface::Plan my_plan;boolsuccess(move_group.plan(my_plan)moveit::core::MoveItErrorCode::SUCCESS);if(success){RCLCPP_INFO(node-get_logger(),[IK] 逆解求解与轨迹规划成功);}else{RCLCPP_ERROR(node-get_logger(),[IK] 逆解求解失败目标位姿可能超出工作空间。);}rclcpp::shutdown();return0;}2求解器配置文件 (config/kinematics.yaml)这是体现“不同求解器选择”的核心文件。程序在运行时会自动读取该文件加载指定的插件。 可以根据需求修改 kinematics_solver 的值panda_arm:# 必须与你的 MoveIt 规划组名称一致 # 求解器选择取消注释你想使用的求解器 #1.默认数值解(KDL)开箱即用但速度慢易受关节限位影响#kinematics_solver:kdl_kinematics_plugin/KDLKinematicsPlugin#2.优化数值解(TRAC-IK)推荐速度快成功率高。需提前 sudo apt install ros-humble-trac-ik-kinematics-plugin kinematics_solver:trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin #3.解析解(IKFast)最快但需提前为特定机械臂生成专属插件#kinematics_solver:panda_moveit_ikfast_plugin/IKFastKinematicsPluginkinematics_solver_timeout:0.005# 求解超时时间秒 kinematics_solver_attempts:3# 尝试次数对 TRAC-IK 影响不大它会自动重启 solve_type:Speed# TRAC-IK 特有追求速度(Speed)或距离最优(Distance)position_only_ik:False # 是否只计算位置忽略姿态3 编译配置文件package.xml (依赖声明):?xml version1.0?package format3namecpp_kinematics_demo/nameversion0.0.1/versiondescriptionDemoforMoveIt2 FK/IK in C/descriptionmaintainer emailuserexample.comuser/maintainerlicenseApache-2.0/licensebuildtool_dependament_cmake/buildtool_dependdependrclcpp/dependdependmoveit_ros_planning_interface/dependdependgeometry_msgs/dependexportbuild_typeament_cmake/build_type/export/packageCMakeLists.txt (编译规则):cmake_minimum_required(VERSION3.8)project(cpp_kinematics_demo)if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHESClang)add_compile_options(-Wall-Wextra-Wpedantic)endif()find_package(ament_cmake REQUIRED)find_package(rclcpp REQUIRED)find_package(moveit_ros_planning_interface REQUIRED)find_package(geometry_msgs REQUIRED)add_executable(kinematics_node src/kinematics_node.cpp)ament_target_dependencies(kinematics_node rclcpp moveit_ros_planning_interface geometry_msgs)install(TARGETS kinematics_node DESTINATION lib/${PROJECT_NAME})# 安装 config 和 launch 文件夹到 share 目录确保运行时能找到install(DIRECTORY config launch DESTINATION share/${PROJECT_NAME})ament_package()4启动文件 (launch/kinematics_demo.launch.py)启动文件负责将你的机械臂 URDF、SRDF 以及最关键的 kinematics.yaml 加载到节点中 urdf可以自己根据模型创建 xacro、srdf和kinematics.yaml可以提供moveit2图形界面根据在配置完之后生成moveit-config包的时候一起生成from launchimportLaunchDescriptionfrom launch_ros.actionsimportNodefrom moveit_configs_utilsimportMoveItConfigsBuilderdefgenerate_launch_description():# 加载 MoveIt 配置这里假设你的机械臂配置包名为 panda_moveit_config # 并明确指定加载我们自定义的 kinematics.yaml moveit_config(MoveItConfigsBuilder(panda,package_namepanda_moveit_config).robot_description(file_pathconfig/panda.urdf.xacro).robot_description_semantic(file_pathconfig/panda.srdf).robot_description_kinematics(file_pathconfig/kinematics.yaml)# 核心加载求解器配置.to_moveit_configs())kinematics_nodeNode(packagecpp_kinematics_demo,executablekinematics_node,outputscreen,parameters[moveit_config.robot_description,moveit_config.robot_description_semantic,moveit_config.robot_description_kinematics,# 传入 kinematics.yaml 参数 moveit_config.joint_limits,],)returnLaunchDescription([kinematics_node])5运行与验证将该功能包放入你的 ROS2工作空间如 colcon_ws/src。 确保你已经安装了 TRAC-IK 插件如果想测试 TRAC-IK sudo apt install ros-humble-trac-ik-kinematics-plugin 编译工作空间colcon build--packages-select cpp_kinematics_demo 启动你的机械臂基础环境例如 Panda 机械臂的 demo ros2 launch panda_moveit_config demo.launch.py 在另一个终端运行你的节点 ros2 launch cpp_kinematics_demo kinematics_demo.launch.py 通过修改 config/kinematics.yaml 中的 kinematics_solver 字段并重新运行可以在同一个 C程序中无缝体验不同 IK 求解器的效果。
