moveit2的机械臂物体抓取

最近在用moveit2来写物体抓取python版，发现资料好少啊，现有的代码几乎也都是官网上教程上的代码，直接搬过来。而且网上好多方法都行不通

[objectDetection.py-8] RuntimeError: Failed to load planning pipelines from parameter server

查了好多资料，发现好多人都有这个问题，但都没有正确的解决办法，手动配置更是报错。尝试了很多方法，最后用了MoveItPy这个。下面附上我运行成功的代码，首先是逆运动学，通过目标位姿运行代码

这个官网完整源码链接在：

https://github.com/moveit/moveit2_tutorials/tree/main/doc/examples/motion_planning_python_api

这是motion_planning_python_api_tutorial.launch.py文件，这个文件我没有进行改动，直接用的官网的

import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import DeclareLaunchArgument, ExecuteProcess
from launch.substitutions import LaunchConfiguration
from moveit_configs_utils import MoveItConfigsBuilder


def generate_launch_description():
    moveit_config = (
        MoveItConfigsBuilder(
            robot_name="jetarm_6dof", package_name="piper_with_gripper_moveit" #revise package name, robot name,
        )
        .robot_description(file_path="config/piper.urdf.xacro")   #probot_anno.urdf.xacro, jetarm_6dof.urdf.xacro #!!!
        .trajectory_execution(file_path="config/moveit_controllers.yaml")
        .moveit_cpp(
            file_path=get_package_share_directory("motion_planning_python_api")
            + "/config/motion_planning_python_api_tutorial.yaml"
        )
        .to_moveit_configs()
    )

    example_file = DeclareLaunchArgument(
        "example_file",
        default_value="motion_planning_python_api_tutorial.py",
        description="Python API tutorial file name",
    )

    moveit_py_node = Node(
        name="moveit_py",
        package="motion_planning_python_api",
        executable=LaunchConfiguration("example_file"),
        output="both",
        parameters=[moveit_config.to_dict()],
    )

这是motion_planning_python_api_tutorial.py

import time

import numpy as np

# generic ros libraries
import rclpy
from rclpy.logging import get_logger

# moveit python library
from moveit.core.robot_state import RobotState
from scipy.spatial.transform import Rotation as R
from moveit.planning import (
    MoveItPy,
    MultiPipelinePlanRequestParameters,
)

from rclpy.node import Node
from geometry_msgs.msg import PoseStamped, Pose, Quaternion
from std_msgs.msg import Header

def plan_and_execute(
    robot,
    planning_component,
    logger,
    single_plan_parameters=None,
    multi_plan_parameters=None,
    sleep_time=0.0,
):
    """Helper function to plan and execute a motion."""
    # plan to goal
    logger.info("Planning trajectory")
    if multi_plan_parameters is not None:
        plan_result = planning_component.plan(
            multi_plan_parameters=multi_plan_parameters
        )
    elif single_plan_parameters is not None:
        plan_result = planning_component.plan(
            single_plan_parameters=single_plan_parameters
        )
    else:
        plan_result = planning_component.plan()

    # execute the plan
    if plan_result:
        logger.info("Executing plan")
        robot_trajectory = plan_result.trajectory
        robot.execute(robot_trajectory, controllers=[])
    else:
        logger.error("Planning failed----------------------------------------------------------------------------------plan_result")
        print("plan_result:::::::::::::::::::::::", plan_result)

    time.sleep(sleep_time)

def main():
    from moveit.planning import MoveItPy
    from geometry_msgs.msg import Pose

    rclpy.init()
    logger = get_logger("moveit_py.pose_goal")

    # instantiate MoveItPy instance and get planning component
    panda = MoveItPy(node_name="moveit_py")
    panda_arm = panda.get_planning_component("arm")
    logger.info("MoveItPy instance created")
    robot_model = panda.get_robot_model()
    robot_state = RobotState(robot_model)
 panda_arm.set_start_state_to_current_state()
    print("panda_arm----------",panda_arm)

    # set pose goal with PoseStamped message
    pose_goal = PoseStamped()
    pose_goal.header.frame_id = "base_link"
    # 2. 姿态：角度 → 弧度，再转四元数
    rx = np.radians(178.819)
    ry = np.radians(-3.500)
    rz = np.radians(-178.377)

    quat = R.from_euler('xyz', [rx, ry, rz]).as_quat()  # [qx, qy, qz, qw]
    pose_goal.pose.orientation.x = quat[0]
    pose_goal.pose.orientation.y = quat[1]
    pose_goal.pose.orientation.z = quat[2]
    pose_goal.pose.orientation.w = quat[3]
    pose_goal.pose.position.x = 0.433
    pose_goal.pose.position.y = 0.036
    pose_goal.pose.position.z = 0.132
    panda_arm.set_goal_state(pose_stamped_msg=pose_goal, pose_link="link6")


    # plan to goal
    plan_and_execute(panda, panda_arm, logger, sleep_time=3.0)
if __name__ == "__main__":
    main()

我是用来控制真机的，所以是以下步骤：

1.激活can接口

2.运行机械臂控制节点

3.运行moveit中的demo文件

4.运行这个launch文件