Moveit Noetic的运动规划组python接口

Moveit Noetic的运动规划组python接口

记录学习如何使用moveit的python API进行机械臂的控制

🚀 环境配置 🚀

• ubuntu 20.04
• ros noetic
• moveit noetic
• 法奥机械臂 frcobot_fr5

1. 设置moveit功能包

可以参考这篇文章：【Moveit2】使用moveit_setup_assistant配置自己的机械臂功能包，moveit2和moveit1功能包的配置非常相似，这里就不再赘述。

配置好moveit功能包之后，我们查看一下当前的文件目录

.
├── build
├── devel
└── src
    └── frcobot_ros
        ├── fr5_gripper_moveit_config
        ├── fr_planning
            └── scripts
        ...

其中fr5_gripper_moveit_config是我使用moveit_setup_assasitant配置的moveit功能包，而fr_planning是我用于存放控制脚本的功能包。

我们将脚本文件存放于./scripts目录下，命名为moveitServer.py，相应的配置文件要做如下修改

CMakeLists.txt

find_package(catkin REQUIRED COMPONENTS
  actionlib
  control_msgs 
  diagnostic_msgs
  geometry_msgs
  moveit_msgs
  moveit_ros_perception
  moveit_ros_planning_interface
  roscpp
  rospy
  sensor_msgs
  shape_msgs
  std_msgs
  std_srvs
  tf
  trajectory_msgs
  message_generation
)

catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES fr_planning
   CATKIN_DEPENDS roscpp rospy std_msgs message_runtime
#  DEPENDS system_lib
)

catkin_install_python(PROGRAMS
  scripts/moveitServer.py
  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

packages.xml

<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>rospy</build_depend>
<build_depend>std_msgs</build_depend>

<!-- add -->
<build_depend>message_generation</build_depend>
<build_depend>actionlib</build_depend>
<build_depend>control_msgs</build_depend>
<build_depend>diagnostic_msgs</build_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>manipulation_msgs</build_depend>
<build_depend>moveit_msgs</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>shape_msgs</build_depend>
<build_depend>std_srvs</build_depend>
<build_depend>tf</build_depend>
<build_depend>trajectory_msgs</build_depend>

<build_export_depend>roscpp</build_export_depend>
<build_export_depend>rospy</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>

<exec_depend>roscpp</exec_depend>
<exec_depend>rospy</exec_depend>
<exec_depend>std_msgs</exec_depend>

<!-- add -->
<exec_depend>message_runtime</exec_depend>
<exec_depend>actionlib</exec_depend>
<exec_depend>control_msgs</exec_depend>
<exec_depend>diagnostic_msgs</exec_depend>
<exec_depend>geometry_msgs</exec_depend>
<exec_depend>manipulation_msgs</exec_depend>
<exec_depend>moveit_msgs</exec_depend>
<exec_depend>sensor_msgs</exec_depend>
<exec_depend>shape_msgs</exec_depend>
<exec_depend>std_srvs</exec_depend>
<exec_depend>tf</exec_depend>
<exec_depend>trajectory_msgs</exec_depend>

2. 编写MoveItController

这部分主要参考官网给出的API示例

具体就不解读了，相信能看懂

moveitServer.py

#!/usr/bin/env python
# -*- coding: utf-8 -*-
# 导入基本ros和moveit库
import rospy, sys
import moveit_commander
from moveit_commander import MoveGroupCommander, PlanningSceneInterface, RobotCommander
from moveit_msgs.msg import  PlanningScene, ObjectColor,CollisionObject, AttachedCollisionObject,Constraints,OrientationConstraint
from geometry_msgs.msg import PoseStamped, Pose
from tf.transformations import quaternion_from_euler
from copy import deepcopy
import numpy as np
import math
from copy import deepcopy


class MoveIt_Controller:

    def __init__(self, is_use_gripper: bool = False):
        # init ros moveit config
        moveit_commander.roscpp_initialize(sys.argv)

        # init ros node
        rospy.init_node('moveit_control_server', anonymous=False)

        # set the name of MoveItGroup
        self.arm = moveit_commander.MoveGroupCommander("arm")
        # set the tolerance of joint
        self.arm.set_goal_joint_tolerance(0.001)
        self.arm.set_goal_position_tolerance(0.001)
        self.arm.set_goal_orientation_tolerance(0.01)
        # get the link of end effector
        self.end_effector_link = self.arm.get_end_effector_link()

        # set the reference frame of MoveItGroup
        self.reference_frame = "base_link"
        self.arm.set_pose_reference_frame(self.reference_frame)

        # set the maximum of planning time and other parameters
        self.arm.set_planning_time(10)
        self.arm.allow_replanning(True)
        self.arm.set_planner_id("RRTstar")

        # set the maximum acceleration and velocity (set to 0-1)
        self.arm.set_max_acceleration_scaling_factor(1)
        self.arm.set_max_velocity_scaling_factor(1)

        # set the arm to "up" pose
        self.go_up()

        # pub scene
        # self.set_scene()    # set table
    
    #=============================== tool function ===============================
    def close(self):
        moveit_commander.roscpp_shutdown()
        moveit_commander.os._exit(0)
    
    def setColor(self, name, r, g, b, a=0.9):
        # init color object of moveit
        color = ObjectColor()
        # set color
        color.id = name
        color.color.r = r
        color.color.g = g
        color.color.b = b
        color.color.a = a
        # update the color dict
        self.colors[name] = color
    
    def sendColors(self):
        # send the color to the planning scene
        # init
        p = PlanningScene()
        # need color to be different
        p.is_diff = True
        # get the color from color dict
        for color in self.colors.values():
            p.object_colors.append(color)
        # publish the color to scene
        self.scene_pub.publish(p)
    
    def set_scene(self):
        # init scene interface
        self.scene = PlanningSceneInterface()
        # init scene publisher
        self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=5)
        self.colors: dict = {}
        rospy.sleep(1)

        # set a table under the arm
        table_id = "table"
        self.scene.remove_world_object(table_id)
        rospy.sleep(1)
        table_size = [2, 2, 0.01]
        # set the pose of the obj
        table_pose = PoseStamped()
        # set the frame of the obj
        table_pose.header.frame_id = self.reference_frame
        table_pose.pose.position.x = 0.0
        table_pose.pose.position.y = 0.0
        table_pose.pose.position.z = -table_size[2]/2 - 0.02
        # w=1表明物体的姿态没有任何旋转，它与世界坐标系对齐。
        table_pose.pose.orientation.w = 1.0
        self.scene.add_box(table_id, table_pose, table_size)
        self.setColor(table_id, 0, 1, 0, 1.0)
        self.sendColors()


    #=============================== move function ===============================
    def go_up(self, a=1, v=1):
        self.arm.set_max_acceleration_scaling_factor(a)
        self.arm.set_max_velocity_scaling_factor(v)
        # "up" is the custom position set by myself
        self.arm.set_named_target('up')
        self.arm.go()
        rospy.sleep(1)

    def go_home(self, a=1, v=1):
        self.arm.set_max_acceleration_scaling_factor(a)
        self.arm.set_max_velocity_scaling_factor(v)
        # "home" is the custom position set by myself
        self.arm.set_named_target('home')
        self.arm.go()
        rospy.sleep(1)
    
    # 关节规划运动，输入6个关节角度(unit: radian)
    def move_j(self, joint_configuration=None, a=1, v=1):
        if joint_configuration == None:
            joint_configuration = [0, -np.pi/2, 0, -np.pi/2, 0, 0]
        self.arm.set_max_acceleration_scaling_factor(a)
        self.arm.set_max_velocity_scaling_factor(v)
        rospy.loginfo("move_j:" + str(joint_configuration))
        self.arm.set_joint_value_target(joint_configuration)
        self.arm.go()
        rospy.sleep(1)
    
    # reference frame下的空间规划，输入xyzRPY
    def move_p(self, pose_configuration=None, a=1, v=1):
        if pose_configuration == None:
            pose_configuration = [0.3, 0, 0.3, 0, -np.pi/2, 0]
        self.arm.set_max_acceleration_scaling_factor(a)
        self.arm.set_max_velocity_scaling_factor(v)

        target_pose = PoseStamped()
        # set reference frame 
        target_pose.header.frame_id = self.reference_frame
        target_pose.header.stamp = rospy.Time.now()
        target_pose.pose.position.x = pose_configuration[0]
        target_pose.pose.position.y = pose_configuration[1]
        target_pose.pose.position.z = pose_configuration[2]
        q = quaternion_from_euler(pose_configuration[3], 
                                    pose_configuration[4],
                                    pose_configuration[5])
        target_pose.pose.orientation.x = q[0]
        target_pose.pose.orientation.y = q[1]
        target_pose.pose.orientation.z = q[2]
        target_pose.pose.orientation.w = q[3]

        # set current pose as the start pose
        rospy.loginfo("move_p:" + str(pose_configuration))
        self.arm.set_start_state_to_current_state()
        self.arm.set_pose_target(target_pose, self.end_effector_link)
        self.arm.go(wait=True)
        # traj = self.arm.plan()
        # self.arm.execute(traj)

        # clean the target for next time plan
        self.arm.clear_pose_targets()
        rospy.sleep(1)
    
    # 空间直线运动,输入连续的(x,y,z,R,P,Y, x2,y2,z2,R2,P2,Y2, ...)
    # 默认只执行一个点位，需要指定传入点位的数量
    def move_l(self, tool_configuration, waypoints_number=1, a=0.5, v=0.5):
        if tool_configuration == None:
            tool_configuration = [0.3, 0, 0.3, 0, -np.pi/2, 0]
        self.arm.set_max_acceleration_scaling_factor(a)
        self.arm.set_max_velocity_scaling_factor(v)

        waypoints = []
        for i in range(waypoints_number):
            target_pose = PoseStamped()
            target_pose.header.frame_id = self.reference_frame
            target_pose.header.stamp = rospy.Time.now()
            target_pose.pose.position.x = tool_configuration[6*i + 0]
            target_pose.pose.position.y = tool_configuration[6*i + 1]
            target_pose.pose.position.z = tool_configuration[6*i + 2]
            q = quaternion_from_euler(tool_configuration[6*i + 3],
                                        tool_configuration[6*i + 4],
                                        tool_configuration[6*i + 5])
            target_pose.pose.orientation.x = q[0]
            target_pose.pose.orientation.y = q[1]
            target_pose.pose.orientation.z = q[2]
            target_pose.pose.orientation.w = q[3]
            waypoints.append(deepcopy(target_pose.pose))
        rospy.loginfo("move_l:" + str(tool_configuration))
        fraction = 0.0      # 路径规划覆盖率
        max_tries = 100     # 最大尝试规划次数
        attempts = 0        # 已经尝试规划次数

        self.arm.set_start_state_to_current_state()
        while fraction < 1.0 and attempts < max_tries:
            (plan, fraction) = self.arm.compute_cartesian_path(
                waypoints,  # waypoint poses, 路径点列表
                0.001,      # eef_step, 终端步进值
                0.00,       # jump_threshold, 跳跃阈值
                True)       # avoid_collisions, 避障规划
            attempts += 1
        if fraction == 1.0:
            rospy.loginfo("Path computed successfully. Moving the arm.")
            self.arm.execute(plan, wait=True)
            rospy.loginfo("Path execution complete.")
        else:
            rospy.loginfo("Path planning failed with only " + str(fraction) + 
                            "success after " + str(max_tries) + " attempts.")
        rospy.sleep(1)

    #=============================== test ===============================
    def test_move_functions(self):
        try:
            print("Test for robot...")
            # self.go_home()
            # self.move_j([0.3, -1.5, 1.2, 0.0, -1, 0.454125],a=0.5,v=0.5)
            # rospy.sleep(2)
            self.move_p([0.4, 0.4, 0.4, -np.pi/2, 0, 0])
            rospy.sleep(2)
            self.move_p([-0.2, -0.3, 0.5, np.pi/4, 0, 0])
            rospy.sleep(2)
            # self.move_l([0.4, 0.1, 0.4, -np.pi, 0, 0] )
            #rospy.sleep(2)
            self.move_l([0.4, 0.1, 0.4, -np.pi, 0, 0,
                         0.3, 0.4, 0.4, np.pi, 0, 0,],waypoints_number=2)
            rospy.sleep(2)
            print("Test success!")
            # self.close_gripper()
            # self.open_gripper()
            # self.grasp([0.4,0.2,0 ],[-np.pi, 0, 0])
            # self.move_p([-0.3, 0, 0.3, 0, -np.pi / 2, 0])
            # self.go_home()
            # waypoints=[]
            # start_pose = self.arm.get_current_pose(self.end_effector_link).pose
            # pose1=deepcopy(start_pose)
            # pose1.position.z +=0.1
            # waypoints.append(deepcopy(pose1))
            # self.move_l(waypoints)
            # self.go_home()
            #self.some_useful_function_you_may_use()
        except:
            print("Test fail! ")

if __name__ == "__main__":
    moveit_server = MoveIt_Controller(is_use_gripper=False)
    # test move functions
    moveit_server.test_move_functions()

然后启动rviz，再运行这个节点就能够让机械臂末端运动到指定的位姿了。

roslaunch fr5_gripper_moveit_config demo.launch 

rosrun fr_planning moveitServer.py 

如图所示

Reference

[1]Move Group Python Interface
[2]【ROS机械臂入门教程】第6讲-Moveit基础（python）