UR10+gazebo+moveit吸盘抓取搬运demo
使用ur10+gazebo开发了一个简易吸盘抓取箱子码垛的仿真过程，机械臂控制使用的是moveit配置。 本博客对部分关键的代码进行解释。
代码运行环境：支持ubuntu16、 18、 20， ros版本是ros1（经过测试）。
1、搬运场景
场景的搭建过程大致就是先用sw建模，然后导出stl模型到gazebo世界中，另存为sdf文件，再把所有模型文件拖入gazebo世界中制作world文件。导入ur机械臂就不再赘述。
2、world文件 carry_box.world
<sdf version='1.6'>
<world name='default'>
<light name='sun' type='directional'>
<cast_shadows>1</cast_shadows>
<pose frame=''>0 0 10 0 -0 0</pose>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.2 0.2 0.2 1</specular>
<attenuation>
<range>1000</range>
<constant>0.9</constant>
<linear>0.01</linear>
<quadratic>0.001</quadratic>
</attenuation>
<direction>-0.5 0.1 -0.9</direction>
</light>
<model name='ground_plane'>
<static>1</static>
<link name='link'>
<collision name='collision'>
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
<surface>
<contact>
<collide_bitmask>65535</collide_bitmask>
<ode/>
</contact>
<friction>
<ode>
<mu>100</mu>
<mu2>50</mu2>
</ode>
<torsional>
<ode/>
</torsional>
</friction>
<bounce/>
</surface>
<max_contacts>10</max_contacts>
</collision>
<visual name='visual'>
<cast_shadows>0</cast_shadows>
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/LightGrey</name>
</script>
</material>
</visual>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
</model>
<gravity>0 0 -9.8</gravity>
<magnetic_field>6e-06 2.3e-05 -4.2e-05</magnetic_field>
<atmosphere type='adiabatic'/>
<physics name='default_physics' default='0' type='ode'>
<max_step_size>0.001</max_step_size>
<real_time_factor>1</real_time_factor>
<real_time_update_rate>1000</real_time_update_rate>
</physics>
<scene>
<ambient>0.4 0.4 0.4 1</ambient>
<background>0.7 0.7 0.7 1</background>
<shadows>0</shadows>
</scene>
<audio>
<device>default</device>
</audio>
<wind/>
<spherical_coordinates>
<surface_model>EARTH_WGS84</surface_model>
<latitude_deg>0</latitude_deg>
<longitude_deg>0</longitude_deg>
<elevation>0</elevation>
<heading_deg>0</heading_deg>
</spherical_coordinates>
<state world_name='default'>
<sim_time>486 140000000</sim_time>
<real_time>4 327300810</real_time>
<wall_time>1696174243 353467065</wall_time>
<iterations>4291</iterations>
<model name='desk_plane'>
<pose frame=''>-0.25 -0.25 0.35 0 -0 0</pose>
<scale>1 1 1</scale>
<link name='link_0'>
<pose frame=''>-0.25 -0.25 0.35 0 -0 0</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>-2.9e-05 -0 4.55499 -3.14158 -0.458701 -3.14159</acceleration>
<wrench>-2.9e-05 -0 4.55499 0 -0 0</wrench>
</link>
</model>
<model name='ground_plane'>
<pose frame=''>0 0 0 0 -0 0</pose>
<scale>1 1 1</scale>
<link name='link'>
<pose frame=''>0 0 0 0 -0 0</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
</model>
<model name='pidai'>
<pose frame=''>0.3 0.45 0.3 0 -7.3e-05 1.57809</pose>
<scale>1 1 1</scale>
<link name='link_1'>
<pose frame=''>0.3 0.45 0.3 0 -7.3e-05 1.57809</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>5.3e-05 0.004276 -0 -2.24965 -0.428859 0</acceleration>
<wrench>5.3e-05 0.004276 -0 0 -0 0</wrench>
</link>
</model>
<model name='tuopan'>
<pose frame=''>0.2 0.5 0.07 -3.14159 0 0</pose>
<scale>1 1 1</scale>
<link name='link_3'>
<pose frame=''>0.2 0.5 0.07 -3.14159 0 0</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>-0.030733 -2.32282 -0.311017 1.76562 -0.44259 0.000324</acceleration>
<wrench>-0.030733 -2.32282 -0.311017 0 -0 0</wrench>
</link>
</model>
<light name='sun'>
<pose frame=''>0 0 10 0 -0 0</pose>
</light>
</state>
<model name='pidai'>
<link name='link_1'>
<inertial>
<mass>1</mass>
<pose frame=''>1 0.3 0 0 -0 0</pose>
<inertia>
<ixx>0.166667</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.166667</iyy>
<iyz>0</iyz>
<izz>0.166667</izz>
</inertia>
</inertial>
<pose frame=''>0 0 0 0 -0 0</pose>
<visual name='visual'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<uri>/home/lzw/now_work/ur_ws/src/ur_carry_box/ur10sence/pidai.stl</uri>
<scale>1 1 1</scale>
</mesh>
</geometry>
<material>
<lighting>1</lighting>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Green</name>
</script>
</material>
<transparency>0</transparency>
<cast_shadows>1</cast_shadows>
</visual>
<collision name='collision'>
<laser_retro>0</laser_retro>
<max_contacts>10</max_contacts>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<uri>/home/lzw/now_work/ur_ws/src/ur_carry_box/ur10sence/pidai.stl</uri>
<scale>1 1 1</scale>
</mesh>
</geometry>
<surface>
<friction>
<ode>
<mu>1</mu>
<mu2>1</mu2>
<fdir1>0 0 0</fdir1>
<slip1>0</slip1>
<slip2>0</slip2>
</ode>
<torsional>
<coefficient>1</coefficient>
<patch_radius>0</patch_radius>
<surface_radius>0</surface_radius>
<use_patch_radius>1</use_patch_radius>
<ode>
<slip>0</slip>
</ode>
</torsional>
</friction>
<bounce>
<restitution_coefficient>0</restitution_coefficient>
<threshold>1e+06</threshold>
</bounce>
<contact>
<collide_without_contact>0</collide_without_contact>
<collide_without_contact_bitmask>1</collide_without_contact_bitmask>
<collide_bitmask>1</collide_bitmask>
<ode>
<soft_cfm>0</soft_cfm>
<soft_erp>0.2</soft_erp>
<kp>1e+13</kp>
<kd>1</kd>
<max_vel>0.01</max_vel>
<min_depth>0</min_depth>
</ode>
<bullet>
<split_impulse>1</split_impulse>
<split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
<soft_cfm>0</soft_cfm>
<soft_erp>0.2</soft_erp>
<kp>1e+13</kp>
<kd>1</kd>
</bullet>
</contact>
</surface>
</collision>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<static>1</static>
<allow_auto_disable>1</allow_auto_disable>
<pose frame=''>-1.09292 1.16271 0 0 -0 0</pose>
</model>
<model name='tuopan'>
<link name='link_3'>
<inertial>
<mass>10</mass>
<pose frame=''>0.6 0.5 0 0 -0 0</pose>
<inertia>
<ixx>0.166667</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.166667</iyy>
<iyz>0</iyz>
<izz>0.166667</izz>
</inertia>
</inertial>
<pose frame=''>-0 -0 0 0 -0 0</pose>
<visual name='visual'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<uri>/home/lzw/now_work/ur_ws/src/ur_carry_box/ur10sence/tuopan.stl</uri>
<scale>1 1 1</scale>
</mesh>
</geometry>
<material>
<lighting>1</lighting>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Wood</name>
</script>
</material>
<transparency>0</transparency>
<cast_shadows>1</cast_shadows>
</visual>
<collision name='collision'>
<laser_retro>0</laser_retro>
<max_contacts>10</max_contacts>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<uri>/home/lzw/now_work/ur_ws/src/ur_carry_box/ur10sence/tuopan.stl</uri>
<scale>1 1 1</scale>
</mesh>
</geometry>
<surface>
<friction>
<ode>
<mu>1</mu>
<mu2>1</mu2>
<fdir1>0 0 0</fdir1>
<slip1>0</slip1>
<slip2>0</slip2>
</ode>
<torsional>
<coefficient>1</coefficient>
<patch_radius>0</patch_radius>
<surface_radius>0</surface_radius>
<use_patch_radius>1</use_patch_radius>
<ode>
<slip>0</slip>
</ode>
</torsional>
</friction>
<bounce>
<restitution_coefficient>0</restitution_coefficient>
<threshold>1e+06</threshold>
</bounce>
<contact>
<collide_without_contact>0</collide_without_contact>
<collide_without_contact_bitmask>1</collide_without_contact_bitmask>
<collide_bitmask>1</collide_bitmask>
<ode>
<soft_cfm>0</soft_cfm>
<soft_erp>0.2</soft_erp>
<kp>1e+13</kp>
<kd>1</kd>
<max_vel>0.01</max_vel>
<min_depth>0</min_depth>
</ode>
<bullet>
<split_impulse>1</split_impulse>
<split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
<soft_cfm>0</soft_cfm>
<soft_erp>0.2</soft_erp>
<kp>1e+13</kp>
<kd>1</kd>
</bullet>
</contact>
</surface>
</collision>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<static>1</static>
<allow_auto_disable>1</allow_auto_disable>
<pose frame=''>0.845494 -0.422476 0 0 -0 0</pose>
</model>
<gui fullscreen='0'>
<camera name='user_camera'>
<pose frame=''>3.17031 -0.472188 0.896836 -0 0.368 2.88159</pose>
<view_controller>orbit</view_controller>
<projection_type>perspective</projection_type>
</camera>
</gui>
<model name='desk_plane'>
<link name='link_0'>
<inertial>
<mass>1</mass>
<pose frame=''>0.25 0.25 0 0 -0 0</pose>
<inertia>
<ixx>0.166667</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.166667</iyy>
<iyz>0</iyz>
<izz>0.166667</izz>
</inertia>
</inertial>
<pose frame=''>-0 -0 0 0 -0 0</pose>
<visual name='visual'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<uri>/home/lzw/now_work/ur_ws/src/ur_carry_box/ur10sence/desk_plane.stl</uri>
<scale>1 1 1</scale>
</mesh>
</geometry>
<material>
<lighting>1</lighting>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/DarkYellow </name>
</script>
</material>
<transparency>0</transparency>
<cast_shadows>1</cast_shadows>
</visual>
<collision name='collision'>
<laser_retro>0</laser_retro>
<max_contacts>10</max_contacts>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<uri>/home/lzw/now_work/ur_ws/src/ur_carry_box/ur10sence/desk_plane.stl</uri>
<scale>1 1 1</scale>
</mesh>
</geometry>
<surface>
<friction>
<ode>
<mu>1</mu>
<mu2>1</mu2>
<fdir1>0 0 0</fdir1>
<slip1>0</slip1>
<slip2>0</slip2>
</ode>
<torsional>
<coefficient>1</coefficient>
<patch_radius>0</patch_radius>
<surface_radius>0</surface_radius>
<use_patch_radius>1</use_patch_radius>
<ode>
<slip>0</slip>
</ode>
</torsional>
</friction>
<bounce>
<restitution_coefficient>0</restitution_coefficient>
<threshold>1e+06</threshold>
</bounce>
<contact>
<collide_without_contact>0</collide_without_contact>
<collide_without_contact_bitmask>1</collide_without_contact_bitmask>
<collide_bitmask>1</collide_bitmask>
<ode>
<soft_cfm>0</soft_cfm>
<soft_erp>0.2</soft_erp>
<kp>1e+13</kp>
<kd>1</kd>
<max_vel>0.01</max_vel>
<min_depth>0</min_depth>
</ode>
<bullet>
<split_impulse>1</split_impulse>
<split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
<soft_cfm>0</soft_cfm>
<soft_erp>0.2</soft_erp>
<kp>1e+13</kp>
<kd>1</kd>
</bullet>
</contact>
</surface>
</collision>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<static>1</static>
<allow_auto_disable>1</allow_auto_disable>
<pose frame=''>0.852331 0.937325 0 0 -0 0</pose>
</model>
</world>
</sdf>
需要修改的地方：
把类似于/home/lzw/now_work/ur_ws/src/ur_carry_box/ur10sence/pidai.stl绝对地址，改成自己本机上的文件地址，所有这些军需要替换。例如自己电脑的文件夹地址是/home/A/ur_ws,则改为/home/A/ur_ws/src/ur_carry_box/ur10sence/pidai.stl。
3、生成box的python代码
# -*- coding: utf-8 -*-
#!/usr/bin/env python
import os
import rospy
from gazebo_msgs.msg import ModelState
from gazebo_msgs.srv import DeleteModel, SpawnModel
from std_msgs.msg import Header
from geometry_msgs.msg import Pose, Point, Quaternion
from math import pi,sin,cos
def rpy2quaternion(roll, pitch, yaw):
x=sin(pitch/2)*sin(yaw/2)*cos(roll/2)+cos(pitch/2)*cos(yaw/2)*sin(roll/2)
y=sin(pitch/2)*cos(yaw/2)*cos(roll/2)+cos(pitch/2)*sin(yaw/2)*sin(roll/2)
z=cos(pitch/2)*sin(yaw/2)*cos(roll/2)-sin(pitch/2)*cos(yaw/2)*sin(roll/2)
w=cos(pitch/2)*cos(yaw/2)*cos(roll/2)-sin(pitch/2)*sin(yaw/2)*sin(roll/2)
return Quaternion(x,y,z,w)
# 定义生成模型的函数
def spawn_aruco_cubo_hover(name='1'):
model_name = "box" + name
model_path = "/home/lzw/now_work/ur_ws/src/ur_carry_box/sdf/box/model.sdf"
initial_pose = Pose(position=Point(x=-0.0, y=1, z=0.6), orientation = rpy2quaternion(0,0,1.57))
# 从文件加载模型
with open(model_path, "r") as f:
model_xml = f.read()
# 调用Gazebo的SpawnModel服务
spawn_model = rospy.ServiceProxy('/gazebo/spawn_sdf_model', SpawnModel)
resp_sdf = spawn_model(model_name, model_xml, "", initial_pose, "world")
if resp_sdf.success:
rospy.loginfo("模型 '{}' 生成成功。".format(model_name))
else:
rospy.logerr("模型 '{}' 生成失败。".format(model_name))
# 调用生成模型的函数
if __name__ == '__main__':
try:
# 初始化ROS节点
rospy.init_node('spawn_box', anonymous=True)
spawn_aruco_cubo_hover()
except rospy.ROSInterruptException:
pass
需要修改的地方：
仍然是地址，原代码中 model_path = "/home/lzw/now_work/ur_ws/src/ur_carry_box/sdf/box/model.sdf"表示的是每次在gazebo场景中生成的物体模型，这里改为自己电脑的模型地址。
4、机械臂控制python程序
#!/usr/bin/env python
# -*- coding:utf-8 -*-
from multiprocessing import current_process
from select import select
import sys
import turtle
# sys.path.append('/home/lzw/now_work/ur_ws/src/ur_carry_box')
import rospy
from std_msgs.msg import Float64
import moveit_commander
import moveit_msgs.msg
import geometry_msgs.msg
from geometry_msgs.msg import PoseStamped,Pose
import math
from math import pi,sin,cos
import numpy as np
import time
from moveit_commander.conversions import pose_to_list
# from pyquaternion import Quaternion
import add_box_in_sence
from std_srvs.srv import Empty
def all_close(goal, actual, tolerance):
"""
Convenience method for testing if a list of values are within a tolerance of their counterparts in another list
@param: goal A list of floats, a Pose or a PoseStamped
@param: actual A list of floats, a Pose or a PoseStamped
@param: tolerance A float
@returns: bool
"""
all_equal = True
if type(goal) is list:
for index in range(len(goal)):
if abs(actual[index] - goal[index]) > tolerance:
return False
elif type(goal) is geometry_msgs.msg.PoseStamped:
return all_close(goal.pose, actual.pose, tolerance)
elif type(goal) is geometry_msgs.msg.Pose:
return all_close(pose_to_list(goal), pose_to_list(actual), tolerance)
return True
def eulerAngles2rotationMat(theta, format='degree'):
# RPY角，是ZYX欧拉角，依次绕定轴XYZ转动[rx, ry, rz]
if format is 'degree':
theta = [i * math.pi / 180.0 for i in theta]
R_x = np.array([[1, 0, 0],
[0, math.cos(theta[0]), -math.sin(theta[0])],
[0, math.sin(theta[0]), math.cos(theta[0])]
])
R_y = np.array([[math.cos(theta[1]), 0, math.sin(theta[1])],
[0, 1, 0],
[-math.sin(theta[1]), 0, math.cos(theta[1])]
])
R_z = np.array([[math.cos(theta[2]), -math.sin(theta[2]), 0],
[math.sin(theta[2]), math.cos(theta[2]), 0],
[0, 0, 1]
])
R = np.dot(R_z, np.dot(R_y, R_x))
return R
# 旋转矩阵转换为四元数
# def rotateToQuaternion(rotateMatrix):
# q = Quaternion(matrix=rotateMatrix)
# return q
def rpy2quaternion(roll, pitch, yaw):
x=sin(pitch/2)*sin(yaw/2)*cos(roll/2)+cos(pitch/2)*cos(yaw/2)*sin(roll/2)
y=sin(pitch/2)*cos(yaw/2)*cos(roll/2)+cos(pitch/2)*sin(yaw/2)*sin(roll/2)
z=cos(pitch/2)*sin(yaw/2)*cos(roll/2)-sin(pitch/2)*cos(yaw/2)*sin(roll/2)
w=cos(pitch/2)*cos(yaw/2)*cos(roll/2)-sin(pitch/2)*sin(yaw/2)*sin(roll/2)
return w, x, y, z
class MoveGroupPythonIntefaceTutorial(object):
"""MoveGroupPythonIntefaceTutorial"""
def __init__(self):
super(MoveGroupPythonIntefaceTutorial, self).__init__()
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('move_group_python_interface_tutorial',
anonymous=True)
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
group_name = "arm"
group = moveit_commander.MoveGroupCommander(group_name)
display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
planning_frame = group.get_planning_frame()
print("============ Reference frame: %s" % planning_frame)
# We can also print the name of the end-effector link for this group:
eef_link = group.get_end_effector_link()
print("============ End effector: %s" % eef_link)
arm_links = robot.get_link_names(group=group_name)
print("============ arm links: %s" % arm_links)
# We can get a list of all the groups in the robot:
group_names = robot.get_group_names()
# print("============ Robot Groups:", robot.get_group_names())
# Sometimes for debugging it is useful to print the entire state of the
# robot:
# print("============ Printing robot state")
# print(robot.get_current_state())
## END_SUB_TUTORIAL
# Misc variables
self.box_name = ''
self.robot = robot
self.scene = scene
self.group = group
self.display_trajectory_publisher = display_trajectory_publisher
self.planning_frame = planning_frame
self.eef_link = eef_link
self.group_names = group_names
self.arm_links = arm_links
def go_to_joint_state(self, joints):
group = self.group
joint_goal = group.get_current_joint_values()
for i in range(6):
print("joint "+str(i)+" angle is ", joint_goal[i])
# joint_goal[0] = 1.57
# joint_goal[1] = -1.57
# joint_goal[2] = 1.57
# joint_goal[3] = 1.57
# joint_goal[4] = -1.57
# joint_goal[5] = 0.0
joint_goal = joints
group.go(joint_goal, wait=True)
group.stop()
current_joints = self.group.get_current_joint_values()
return all_close(joint_goal, current_joints, 0.01)
def go_to_pose_goal(self, goal):
'''
goal : list [x,y,z, R, P, Y]
'''
group = self.group
current_pose = group.get_current_pose()
print('current pose is {}'.format(current_pose.pose))
group.set_start_state_to_current_state()
group.set_pose_target(goal, self.eef_link)
traj = group.plan()
group.execute(traj)
group.clear_pose_targets()
print("============ Move success.")
def go_to_pose_goal_interp(self, goal):
'''
goal : list [x,y,z, R, P, Y]
'''
group = self.group
current_pose = group.get_current_pose()
print('current pose is {}'.format(current_pose.pose))
num_step = 4
x_step = abs(current_pose.pose.position.x-goal[0]) / num_step
y_step = abs(current_pose.pose.position.y-goal[1]) / num_step
z_step = abs(current_pose.pose.position.z-goal[2]) / num_step
x_list = list(np.arange(current_pose.pose.position.x, goal[0], x_step if current_pose.pose.position.x<goal[0] else -x_step))
y_list = list(np.arange(current_pose.pose.position.y, goal[1], y_step if current_pose.pose.position.y<goal[1] else -y_step))
z_list = list(np.arange(current_pose.pose.position.z, goal[2], z_step if current_pose.pose.position.z<goal[2] else -z_step))
waypoints= []
for i in range(max([len(x_list), len(y_list), len(z_list)]) + 1):
temp = []
temp.append(x_list[i] if i < len(x_list) else goal[0])
temp.append(y_list[i] if i < len(y_list) else goal[1])
temp.append(z_list[i] if i < len(z_list) else goal[2])
temp.append(goal[3])
temp.append(goal[4])
temp.append(goal[5])
waypoints.append(temp)
print('###current_pose={}'.format(current_pose))
print('###target ={}'.format(goal))
print('###waypoints={}'.format(waypoints))
group.set_start_state_to_current_state()
group.set_pose_targets(waypoints)
traj = group.plan()
print('###traj={}'.format(traj))
group.execute(traj)
group.clear_pose_targets()
print("============ Move success.")
def main():
try:
tutorial = MoveGroupPythonIntefaceTutorial()
rospy.wait_for_service('/vacuum_gripper/on', 1)
rospy.wait_for_service('/vacuum_gripper/off', 1)
_on = rospy.ServiceProxy('/vacuum_gripper/on', Empty)
_off = rospy.ServiceProxy('/vacuum_gripper/off', Empty)
put_position = [[1.15,-0.225],[1.15,0.225],[0.8,-0.225],[0.8,0.225],[0.45,-0.225],[0.45,0.225]]
for i in range(len(put_position)):
# add box
add_box_in_sence.spawn_aruco_cubo_hover(str(i+1))
# get box
tutorial.go_to_joint_state(joints=[1.57,-1.57,1.57,-1.57,-1.57,0])
tutorial.go_to_pose_goal([-0.002, 1.010, 0.368, -pi/2,0,0])
time.sleep(1)
_on()
time.sleep(1)
# put box
tutorial.go_to_pose_goal([0.5, 0.25, 0.7, -pi/2,0,0])
tutorial.go_to_pose_goal([put_position[i][0], put_position[i][1], 0.5, -pi/2,0,0])
tutorial.go_to_pose_goal([put_position[i][0], put_position[i][1], 0.126, -pi/2,0,0])
_off()
time.sleep(1)
tutorial.go_to_pose_goal([1,0,0.7, -pi/2,0,0])
except rospy.ROSInterruptException:
return
except KeyboardInterrupt:
return
if __name__ == '__main__':
main()
这个没什么需要改的，贴出来供其他道友参考。
5、其他报错
大部分报错均是由于环境没有配置好，请根据报错内容仔细检查本机环境缺失内容，根据报错安装缺失项。
例如报错Could not load controller ‘arm_controller’ because controller type ‘position_controllers/JointTrajectoryController’ does not exist.，则安装：
sudo apt install ros-melodic-joint-trajectory-controller
参考：https://blog.csdn.net/gezongbo/article/details/120028916
若是ubuntu20系统，可能由于movet版本区别，则arm_control_by_moveit.py程序中，traj = group.plan()需要改为ubuntu20的moveit支持的函数输出格式，改为plan_success, traj, planning_time, error_code = arm.plan()，参考https://blog.csdn.net/LWLGZY/article/details/121106934.