Moveit Noetic的运动规划组python接口
记录学习如何使用moveit的python API进行机械臂的控制
🚀 环境配置 🚀
- ubuntu 20.04
- ros noetic
- moveit noetic
- 法奥机械臂 frcobot_fr5
文章目录
- Moveit Noetic的运动规划组python接口
- 1. 设置moveit功能包
- 2. 编写MoveItController
- Reference
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）
