使用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 pythonimport 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,cosdef 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" + namemodel_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 Emptydef 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 = Trueif type(goal) is list:for index in range(len(goal)):if abs(actual[index] - goal[index]) > tolerance:return Falseelif 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 Truedef 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 qdef 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, zclass 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 variablesself.box_name = ''self.robot = robotself.scene = sceneself.group = groupself.display_trajectory_publisher = display_trajectory_publisherself.planning_frame = planning_frameself.eef_link = eef_linkself.group_names = group_namesself.arm_links = arm_linksdef go_to_joint_state(self, joints):group = self.groupjoint_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.0joint_goal = jointsgroup.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.groupcurrent_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.groupcurrent_pose = group.get_current_pose()print('current pose is {}'.format(current_pose.pose))num_step = 4x_step = abs(current_pose.pose.position.x-goal[0]) / num_stepy_step = abs(current_pose.pose.position.y-goal[1]) / num_stepz_step = abs(current_pose.pose.position.z-goal[2]) / num_stepx_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 boxadd_box_in_sence.spawn_aruco_cubo_hover(str(i+1))# get boxtutorial.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 boxtutorial.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:returnexcept KeyboardInterrupt:returnif __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.
