class PlanningSceneHelper:
def __init__(self, package=None, mesh_folder_path=None):
# interface to the planning and collision scenes
self.psi = PlanningSceneInterface()
# self.scene_pub = Publisher("/collision_object", CollisionObject,
# queue_size=1, latch=True)
self.vis_pub = Publisher("/collision_scene",
MarkerArray,
queue_size=10)
self.marker_array = MarkerArray()
sleep(1.0)
if package is not None and mesh_folder_path is not None:
# Build folder path for use in load
rospack = RosPack()
self.package = package
self.mesh_folder_path = mesh_folder_path
self.package_path = rospack.get_path(package)
self.folder_path = os.path.join(self.package_path, mesh_folder_path)\
+ os.sep
else:
loginfo(
"Missing package or mesh folder path supplied to planning_scene_helper; no meshes can be added"
)
# Create dict of objects, for removing markers later
self.objects = {}
self.next_id = 1
# Loads the selected mesh file into collision scene at the desired location.
def add_mesh(self,
object_id,
frame,
filename,
visual=None,
pose=None,
position=None,
orientation=None,
rpy=None,
color=None):
if self.package is None:
logwarn("No package was provided; no meshes can be loaded.")
if visual is None:
visual = filename
filename = self.folder_path + filename
stamped_pose = self.make_stamped_pose(frame=frame,
pose=pose,
position=position,
orientation=orientation,
rpy=rpy)
try:
self.psi.add_mesh(object_id, stamped_pose, filename)
loginfo("Loaded " + object_id + " from" + filename +
" as collision object.")
marker = self.make_marker_stub(stamped_pose, color=color)
marker.type = Marker.MESH_RESOURCE
# marker.mesh_resource = "file:/" + self.folder_path + visual
marker.mesh_resource = "package://" + self.package + os.sep + self.mesh_folder_path + os.sep + visual
self.marker_array = self.make_new_marker_array_msg()
self.marker_array.markers.append(marker)
self.vis_pub.publish(self.marker_array)
self.objects[object_id] = marker
loginfo("Loaded " + object_id + " from " + marker.mesh_resource +
" as marker.")
except ROSException as e:
loginfo("Problem loading " + object_id + " collision object: " +
str(e))
except AssimpError as ae:
loginfo("Problem loading " + object_id + " collision object: " +
str(ae))
def add_cylinder(self,
object_id,
frame,
size,
pose=None,
position=None,
orientation=None,
rpy=None,
color=None):
try:
stamped_pose = self.make_stamped_pose(frame=frame,
pose=pose,
position=position,
orientation=orientation,
rpy=rpy)
# Cylinders are special - they are not supported directly by
# moveit_commander. It must be published manually to collision scene
cyl = CollisionObject()
cyl.operation = CollisionObject.ADD
cyl.id = object_id
cyl.header = stamped_pose.header
prim = SolidPrimitive()
prim.type = SolidPrimitive.CYLINDER
prim.dimensions = [size[0], size[1]]
cyl.primitives = [prim]
cyl.primitive_poses = [stamped_pose.pose]
# self.scene_pub.publish(cyl)
marker = self.make_marker_stub(stamped_pose,
[size[1] * 2, size[2] * 2, size[0]],
color=color)
marker.type = Marker.CYLINDER
self.publish_marker(object_id, marker)
sleep(0.5)
logdebug("Loaded " + object_id +
" as cylindrical collision object.")
except ROSException as e:
loginfo("Problem loading " + object_id + " collision object: " +
str(e))
def add_sphere(self,
object_id,
frame,
size,
pose=None,
position=None,
orientation=None,
rpy=None,
color=None):
try:
stamped_pose = self.make_stamped_pose(frame=frame,
pose=pose,
position=position,
orientation=orientation,
rpy=rpy)
self.psi.add_sphere(object_id, stamped_pose, size[0])
loginfo("got past adding collision scene object")
marker = self.make_marker_stub(
stamped_pose, [size[0] * 2, size[1] * 2, size[2] * 2],
color=color)
marker.type = Marker.SPHERE
self.publish_marker(object_id, marker)
sleep(0.5)
loginfo("Loaded " + object_id + " as collision object.")
except ROSException as e:
loginfo("Problem loading " + object_id + " collision object: " +
str(e))
def add_box(self,
object_id,
frame,
size,
pose=None,
position=None,
orientation=None,
rpy=None,
color=None):
try:
stamped_pose = self.make_stamped_pose(frame=frame,
pose=pose,
position=position,
orientation=orientation,
rpy=rpy)
self.psi.add_box(object_id, stamped_pose, size)
marker = self.make_marker_stub(stamped_pose, size, color=color)
marker.type = Marker.CUBE
self.publish_marker(object_id, marker)
sleep(0.5)
loginfo("Loaded " + object_id + " as collision object.")
except ROSException as e:
loginfo("Problem loading " + object_id + " collision object: " +
str(e))
def attach_box(self,
link,
object_id,
frame,
size,
attach_to_link,
pose=None,
position=None,
orientation=None,
rpy=None):
"""TODO: color is not yet supported, since it's not internally
supported by psi.attach_box. Basically duplicate this method
but with color support."""
try:
stamped_pose = self.make_stamped_pose(frame=frame,
pose=pose,
position=position,
orientation=orientation,
rpy=rpy),
self.psi.attach_box(link, object_id, stamped_pose, size,
attach_to_link)
sleep(0.5)
loginfo("Attached " + object_id + " as collision object.")
except ROSException as e:
loginfo("Problem attaching " + object_id + " collision object: " +
str(e))
@staticmethod
def make_stamped_pose(frame,
pose=None,
position=None,
orientation=None,
rpy=None):
if orientation is not None and rpy is not None:
logwarn("Collision object has both orientation (quaternion) and "
"Rotation (rpy) defined! Defaulting to quaternion "
"representation")
stamped_pose = PoseStamped()
stamped_pose.header.frame_id = frame
stamped_pose.header.stamp = Time.now()
# use a pose if one is provided, otherwise, make your own from the
# position and orientation.
if pose is not None:
stamped_pose.pose = pose
else:
stamped_pose.pose.position.x = position[0]
stamped_pose.pose.position.y = position[1]
stamped_pose.pose.position.z = position[2]
# for orientation, allow either quaternion or RPY specification
if orientation is not None:
stamped_pose.pose.orientation.x = orientation[0]
stamped_pose.pose.orientation.y = orientation[1]
stamped_pose.pose.orientation.z = orientation[2]
stamped_pose.pose.orientation.w = orientation[3]
elif rpy is not None:
quaternion = transformations.quaternion_from_euler(
rpy[0], rpy[1], rpy[2])
stamped_pose.pose.orientation.x = quaternion[0]
stamped_pose.pose.orientation.y = quaternion[1]
stamped_pose.pose.orientation.z = quaternion[2]
stamped_pose.pose.orientation.w = quaternion[3]
else:
stamped_pose.pose.orientation.w = 1 # make basic quaternion
return stamped_pose
# Fill a ROS Marker message with the provided data
def make_marker_stub(self, stamped_pose, size=None, color=None):
if color is None:
color = (0.5, 0.5, 0.5, 1.0)
if size is None:
size = (1, 1, 1)
marker = Marker()
marker.header = stamped_pose.header
# marker.ns = "collision_scene"
marker.id = self.next_id
marker.lifetime = Time(0) # forever
marker.action = Marker.ADD
marker.pose = stamped_pose.pose
marker.color.r = color[0]
marker.color.g = color[1]
marker.color.b = color[2]
if len(color) == 4:
alpha = color[3]
else:
alpha = 1.0
marker.color.a = alpha
marker.scale.x = size[0]
marker.scale.y = size[1]
marker.scale.z = size[2]
self.next_id += 1
return marker
def make_new_marker_array_msg(self):
ma = MarkerArray()
return ma
# publish a single marker
def publish_marker(self, object_id, marker):
loginfo("Publishing marker for object {}".format(object_id))
self.marker_array = self.make_new_marker_array_msg()
self.marker_array.markers.append(marker)
self.vis_pub.publish(self.marker_array)
self.objects[object_id] = marker
# Remove the provided object from the world.
def remove(self, object_id):
# if object_id not in self.objects:
# logwarn("PlanningSceneHelper was not used to create object with id "
# + object_id + ". Attempting to remove it anyway.")
try:
# first remove the actual collision object
self.psi.remove_world_object(object_id)
if object_id in self.objects:
# then remove the marker associated with it
marker = self.objects[object_id]
marker.action = Marker.DELETE
self.publish_marker(object_id, marker)
self.objects.pop(object_id)
logdebug("Marker for collision object " + object_id + " removed.")
except ROSException as e:
loginfo("Problem removing " + object_id +
" from collision scene:" + str(e))
return
except KeyError as e:
loginfo("Problem removing " + object_id +
" from collision scene:" + str(e))
return
loginfo("Model " + object_id + " removed from collision scene.")
# Remove the provided attached collision object.
def remove_attached(self, link, object_id):
try:
self.psi.remove_attached_object(link=link, name=object_id)
loginfo("Attached object '" + object_id +
"' removed from collision scene.")
except:
loginfo("Problem attached object '" + object_id +
"' removing from collision scene.")
class Pick_Place:
def __init__(self):
self.object_list = {}
self.arm = moveit_commander.MoveGroupCommander("irb_120")
self.gripper = moveit_commander.MoveGroupCommander("robotiq_85")
self.arm.set_goal_tolerance(0.05)
self.gripper.set_goal_tolerance(0.02)
self.scene = PlanningSceneInterface()
self.robot = RobotCommander()
rospy.sleep(1)
self.add_objects()
self.add_table()
#self.add_ground()
self.approach_retreat_desired_dist = 0.2
self.approach_retreat_min_dist = 0.1
rospy.sleep(1.0)
# pick up object in pose
def pickup(self, object_name, pose):
grasps = self.generate_grasps(object_name, pose)
self.arm.pick(object_name, grasps)
#self.gripper.stop()
rospy.loginfo('Pick up successfully')
self.arm.detach_object(object_name)
self.clean_scene(object_name)
#rospy.sleep(1)
# place object to goal pose
def place(self, pose):
self.move_pose_arm(pose)
rospy.sleep(1)
# pose.position.z -= 0.1
# self.move_pose_arm(pose)
waypoints = []
wpose = self.arm.get_current_pose().pose
wpose.position.z -= 0.15
waypoints.append(copy.deepcopy(wpose))
(plan, fraction) = self.arm.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
self.arm.execute(plan, wait=True)
self.move_joint_hand(0)
rospy.sleep(1)
# pose.position.z += 0.1
# self.move_pose_arm(pose)
waypoints = []
wpose = self.arm.get_current_pose().pose
wpose.position.z += 0.15
waypoints.append(copy.deepcopy(wpose))
(plan, fraction) = self.arm.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
self.arm.execute(plan, wait=True)
rospy.loginfo('Place successfully')
def get_object_pose(self, object_name):
pose = self.object_list[object_name].pose
return pose
def clean_scene(self, object_name):
self.scene.remove_world_object(object_name)
def add_ground(self):
self.clean_scene("ground")
p = PoseStamped()
p.header.frame_id = self.robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
p.pose.position.x = 0.0
p.pose.position.y = 0.0
p.pose.position.z = -0.01
size = (5, 5, 0.02)
#self.scene.add_box("ground",p, size)
rospy.sleep(2)
def add_table(self):
self.clean_scene("table")
p = PoseStamped()
p.header.frame_id = self.robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
p.pose.position.x = 0.8
p.pose.position.y = 0.0
p.pose.position.z = 0.1
size = (0.8, 1.5, 0.028)
self.scene.add_box("table", p, size)
rospy.sleep(2)
def add_objects(self):
p = PoseStamped()
p.header.frame_id = self.robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
# add box
name = "box"
self.clean_scene(name)
p.pose.position.x = 0.5
p.pose.position.y = 0.0
p.pose.position.z = 0.025 + 0.115
q = quaternion_from_euler(0.0, 0.0, 0.0)
p.pose.orientation = Quaternion(*q)
size = (0.05, 0.05, 0.05)
self.scene.add_box(name, p, size)
height = 0.05
width = 0.05
shape = "cube"
color = "red"
self.object_list[name] = Object(p.pose, height, width, shape, color)
print("add box")
rospy.sleep(1)
p = PoseStamped()
p.header.frame_id = self.robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
# add cylinder
name = "cylinder"
self.clean_scene(name)
p.pose.position.x = 0.5
p.pose.position.y = 0.2
p.pose.position.z = 0.05 + 0.115
q = quaternion_from_euler(0.0, 0.0, 0.0)
p.pose.orientation = Quaternion(*q)
height = 0.1
radius = 0.03
self.scene.add_cylinder(name, p, height, radius)
height = 0.1
width = 0.03
shape = "cylinder"
color = "green"
self.object_list[name] = Object(p.pose, height, width, shape, color)
print("add cylinder")
rospy.sleep(1)
p = PoseStamped()
p.header.frame_id = self.robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
# add sphere
name = "ball"
self.clean_scene(name)
p.pose.position.x = 0.5
p.pose.position.y = -0.2
p.pose.position.z = 0.03 + 0.115
q = quaternion_from_euler(0.0, 0.0, 0.0)
p.pose.orientation = Quaternion(*q)
radius = 0.03
self.scene.add_sphere(name, p, radius)
height = 0.03
width = 0.03
shape = "sphere"
color = "red"
self.object_list[name] = Object(p.pose, height, width, shape, color)
print("add ball")
rospy.sleep(1)
#print(self.object_list)
def pose2msg(self, roll, pitch, yaw, x, y, z):
pose = geometry_msgs.msg.Pose()
quat = quaternion_from_euler(roll, pitch, yaw)
pose.orientation.x = quat[0]
pose.orientation.y = quat[1]
pose.orientation.z = quat[2]
pose.orientation.w = quat[3]
pose.position.x = x
pose.position.y = y
pose.position.z = z
return pose
def msg2pose(self, pose):
x = pose.position.x
y = pose.position.y
z = pose.position.z
quaternion = (pose.orientation.x, pose.orientation.y,
pose.orientation.z, pose.orientation.w)
euler = euler_from_quaternion(quaternion)
roll = euler[0]
pitch = euler[1]
yaw = euler[2]
return roll, pitch, yaw, x, y, z
def back_to_home(self):
self.move_joint_arm(0, 0, 0, 0, 0, 0)
self.move_joint_hand(0)
rospy.sleep(1)
# Forward Kinematics (FK): move the arm by axis values
def move_joint_arm(self, joint_0, joint_1, joint_2, joint_3, joint_4,
joint_5):
joint_goal = self.arm.get_current_joint_values()
joint_goal[0] = joint_0
joint_goal[1] = joint_1
joint_goal[2] = joint_2
joint_goal[3] = joint_3
joint_goal[4] = joint_4
joint_goal[5] = joint_5
self.arm.go(joint_goal, wait=True)
self.arm.stop() # To guarantee no residual movement
# Inverse Kinematics: Move the robot arm to desired pose
def move_pose_arm(self, pose_goal):
self.arm.set_pose_target(pose_goal)
self.arm.go(wait=True)
self.arm.stop() # To guarantee no residual movement
self.arm.clear_pose_targets()
# Move the Robotiq gripper by master axis
def move_joint_hand(self, gripper_finger1_joint):
joint_goal = self.gripper.get_current_joint_values()
joint_goal[2] = gripper_finger1_joint # Gripper master axis
self.gripper.go(joint_goal, wait=True)
self.gripper.stop() # To guarantee no residual movement
def generate_grasps(self, name, pose):
grasps = []
now = rospy.Time.now()
angle = 0
grasp = Grasp()
grasp.grasp_pose.header.stamp = now
grasp.grasp_pose.header.frame_id = self.robot.get_planning_frame()
grasp.grasp_pose.pose = copy.deepcopy(pose)
# Setting pre-grasp approach
grasp.pre_grasp_approach.direction.header.stamp = now
grasp.pre_grasp_approach.direction.header.frame_id = self.robot.get_planning_frame(
)
grasp.pre_grasp_approach.direction.vector.z = -0.5
grasp.pre_grasp_approach.min_distance = self.approach_retreat_min_dist
grasp.pre_grasp_approach.desired_distance = self.approach_retreat_desired_dist
# Setting post-grasp retreat
grasp.post_grasp_retreat.direction.header.stamp = now
grasp.post_grasp_retreat.direction.header.frame_id = self.robot.get_planning_frame(
)
grasp.post_grasp_retreat.direction.vector.z = 0.5
grasp.post_grasp_retreat.min_distance = self.approach_retreat_min_dist
grasp.post_grasp_retreat.desired_distance = self.approach_retreat_desired_dist
q = quaternion_from_euler(0.0, numpy.deg2rad(90.0), angle)
grasp.grasp_pose.pose.orientation = Quaternion(*q)
grasp.max_contact_force = 1000
grasp.pre_grasp_posture.joint_names.append("gripper_finger1_joint")
traj = JointTrajectoryPoint()
traj.positions.append(0.0)
traj.time_from_start = rospy.Duration.from_sec(0.5)
grasp.pre_grasp_posture.points.append(traj)
grasp.grasp_posture.joint_names.append("gripper_finger1_joint")
traj = JointTrajectoryPoint()
if name == "box":
traj.positions.append(0.4)
elif name == "ball":
traj.positions.append(0.3)
elif name == "cylinder":
traj.positions.append(0.3)
#traj.velocities.append(0.2)
#traj.effort.append(100)
traj.time_from_start = rospy.Duration.from_sec(5.0)
grasp.grasp_posture.points.append(traj)
grasps.append(grasp)
return grasps
# Implement the main algorithm here
def MyAlgorithm(self):
self.back_to_home()
# pick cylinder
object_name = "cylinder"
pose = self.get_object_pose(object_name)
print(pose.position.y)
pose.position.z += 0.16
self.pickup(object_name, pose)
roll = 0.0
pitch = numpy.deg2rad(90.0)
yaw = 0.0
x = 0
y = 0.6
z = pose.position.z + 0.01
place_pose = self.pose2msg(roll, pitch, yaw, x, y, z)
self.place(place_pose)
self.back_to_home()
# pick box
object_name = "box"
pose = self.get_object_pose(object_name)
print(pose.position.y)
pose.position.z += 0.15
self.pickup(object_name, pose)
roll = 0.0
pitch = numpy.deg2rad(90.0)
yaw = 0.0
x = 0.1
y = 0.6
z = pose.position.z + 0.01
place_pose = self.pose2msg(roll, pitch, yaw, x, y, z)
self.place(place_pose)
self.back_to_home()
# pick ball
object_name = "ball"
pose = self.get_object_pose(object_name)
print(pose.position.y)
pose.position.z += 0.14
self.pickup(object_name, pose)
roll = 0.0
pitch = numpy.deg2rad(90.0)
yaw = 0.0
x = -0.1
y = 0.6
z = pose.position.z + 0.01
place_pose = self.pose2msg(roll, pitch, yaw, x, y, z)
self.place(place_pose)
self.back_to_home()
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_obstacles_demo')
# 初始化场景对象
scene = PlanningSceneInterface()
# 创建一个发布场景变化信息的发布者
self.scene_pub = rospy.Publisher('planning_scene',
PlanningScene,
queue_size=5)
rospy.Subscriber("chatter", Float64MultiArray, callback)
# 创建一个存储物体颜色的字典对象
self.colors = dict()
# 等待场景准备就绪
rospy.sleep(1)
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander('arm')
gripper = MoveGroupCommander('gripper')
# 获取终端link的名称
end_effector_link = arm.get_end_effector_link()
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.01)
gripper.set_goal_position_tolerance(0.01)
arm.set_goal_orientation_tolerance(0.2)
# 当运动规划失败后，允许重新规划
arm.allow_replanning(True)
# 设置目标位置所使用的参考坐标系
reference_frame = 'base_link'
arm.set_pose_reference_frame(reference_frame)
# 设置每次运动规划的时间限制：5s
arm.set_planning_time(5)
# 设置场景物体的名称
table_id = 'table'
# cy_id = 'cy'
box1_id = 'box1'
box2_id = 'box2'
box3_id = 'box3'
sphere_id = 'sphere'
# 移除场景中之前运行残留的物体
scene.remove_world_object(table_id)
scene.remove_world_object(box1_id)
scene.remove_world_object(box2_id)
scene.remove_world_object(box3_id)
scene.remove_world_object(sphere_id)
rospy.sleep(1)
#控制机械臂先回到初始化位置
arm.set_named_target('init')
arm.go()
rospy.sleep(1)
# arm.set_named_target('start')
# arm.go()
# rospy.sleep(1)
gripper.set_joint_value_target([0.05])
gripper.go()
rospy.sleep(0)
# 设置桌面的高度
table_ground = 0.37
# 设置table、box1和box2的三维尺寸
table_size = [0.2, 0.3, 0.01]
box1_size = [0.01, 0.01, 0.19]
box2_size = [0.01, 0.01, 0.19]
box3_size = [0.005, 0.01, 0.3]
sphere_R = 0.01
error = 0.03
# 将三个物体加入场景当中
table_pose = PoseStamped()
table_pose.header.frame_id = reference_frame
table_pose.pose.position.x = -table_size[0] / 2.0
table_pose.pose.position.y = 0.0
table_pose.pose.position.z = table_ground + table_size[2] / 2.0
table_pose.pose.orientation.w = 1.0
scene.add_box(table_id, table_pose, table_size)
#scene.add_cylinder
box1_pose = PoseStamped()
box1_pose.header.frame_id = reference_frame
box1_pose.pose.position.x = -0.09
box1_pose.pose.position.y = table_size[0] / 2.0
box1_pose.pose.position.z = 0.18 + box1_size[2] / 2.0
box1_pose.pose.orientation.w = 1.0
scene.add_box(box1_id, box1_pose, box1_size)
box2_pose = PoseStamped()
box2_pose.header.frame_id = reference_frame
box2_pose.pose.position.x = -0.09
box2_pose.pose.position.y = -table_size[0] / 2.0
box2_pose.pose.position.z = 0.18 + box1_size[2] / 2.0
box2_pose.pose.orientation.w = 1.0
scene.add_box(box2_id, box2_pose, box2_size)
# box3_pose = PoseStamped()
# box3_pose.header.frame_id = reference_frame
# box3_pose.pose.position.x = pos_aim[0]
# box3_pose.pose.position.y = pos_aim[1]
# box3_pose.pose.position.z = box3_size[2]/2.0+0.1
# box3_pose.pose.orientation.w = 1.0
# scene.add_box(box3_id, box3_pose, box3_size)
sphere_pose = PoseStamped()
sphere_pose.header.frame_id = reference_frame
sphere_pose.pose.position.x = pos_aim[0] + 0
sphere_pose.pose.position.y = pos_aim[1]
sphere_pose.pose.position.z = pos_aim[2]
sphere_pose.pose.orientation.w = 1.0
scene.add_sphere(sphere_id, sphere_pose, sphere_R)
# 将桌子设置成红色，两个box设置成橙色
self.setColor(table_id, 0, 0, 0, 1)
# self.setColor(table_id, 0.8, 0, 0, 1.0)
# self.setColor(box1_id, 0.8, 0.4, 0, 1.0)
self.setColor(box1_id, 1, 1, 1, 1.0)
self.setColor(box2_id, 1, 1, 1, 1.0)
self.setColor(box3_id, 1, 1, 1, 1.0)
self.setColor(sphere_id, 0.8, 0, 0, 1.0)
# 将场景中的颜色设置发布
self.sendColors()
# rospy.INFO("waiting...")
# if(Recive_FLAG==1):
# rospy.INFO("OK！")
# 设置机械臂的运动目标位置
target_pose = PoseStamped()
target_pose.header.frame_id = reference_frame
target_pose.pose.position.x = pos_aim[0] - error
target_pose.pose.position.y = pos_aim[1]
target_pose.pose.position.z = pos_aim[2]
# target_pose.pose.orientation.w = 1.0
#####0.3
# 0.2 0.2 0.2 0.15 0.15
# 0.12 0.11 0.12 0.15 0.15
# 0.30 0.245 0.35 0.35 0.25
# 控制机械臂运动到目标位置
arm.set_pose_target(target_pose, end_effector_link)
arm.go()
rospy.sleep(1)
gripper.set_joint_value_target([0.03])
gripper.go()
rospy.sleep(1)
# # 控制机械臂终端向x移动5cm
arm.shift_pose_target(0, 0.01, end_effector_link)
arm.go()
rospy.sleep(1)
gripper.set_joint_value_target([0.05])
gripper.go()
rospy.sleep(1)
# # 设置机械臂的运动目标位置，进行避障规划
# target_pose2 = PoseStamped()
# target_pose2.header.frame_id = reference_frame
# target_pose2.pose.position.x = 0.15
# target_pose2.pose.position.y = 0 #-0.25
# target_pose2.pose.position.z = table_pose.pose.position.z + table_size[2] + 0.05
# target_pose2.pose.orientation.w = 1.0
# # 控制机械臂运动到目标位置
# arm.set_pose_target(target_pose2, end_effector_link)
# arm.go()
# rospy.sleep(2)
#控制机械臂回到初始化位置
arm.set_named_target('init')
arm.go()
# 关闭并退出moveit
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
rospy.spin()
class Pick_Place:
def __init__(self):
self.objects_name = ["cylinder", "box", "sphere"]
self.object_width = 0.03
self.arm = moveit_commander.MoveGroupCommander("irb_120")
self.gripper = moveit_commander.MoveGroupCommander("robotiq_85")
self.arm.set_goal_tolerance(0.2)
self.gripper.set_goal_tolerance(0.05)
self.scene = PlanningSceneInterface()
self.robot = RobotCommander()
self.add_ground()
rospy.sleep(1.0)
for i in range(3):
rospy.loginfo("Moving arm to HOME point")
self.move_pose_arm(0, 1.57, 0, 0.4, 0, 0.6)
rospy.sleep(1)
self.object_name = self.objects_name[i]
self.scene.remove_world_object(self.object_name)
self.pose_object, dx, dy, dz = self.add_object(self.object_name)
rospy.sleep(1.0)
self.pose_object.position.x += dx
self.pose_object.position.y += dy
self.pose_object.position.z += dz
print(self.objects_name[i], self.pose_object.position)
self.approach_retreat_desired_dist = 0.2
self.approach_retreat_min_dist = 0.1
print("start pick and place")
# Pick
grasps = self.generate_grasps(self.object_name, self.pose_object)
self.arm.pick(self.object_name, grasps)
self.gripper.stop()
rospy.loginfo('Pick up successfully')
self.arm.detach_object(self.object_name)
self.clean_scene(self.object_name)
rospy.sleep(1)
curr_pose = self.arm.get_current_pose().pose
x = curr_pose.position.x
y = curr_pose.position.y
z = curr_pose.position.z
# quaternion = (curr_pose.orientation.x,
# curr_pose.orientation.y,
# curr_pose.orientation.z,
# curr_pose.orientation.w)
# euler = euler_from_quaternion(quaternion)
# roll = euler[0]
# pitch = euler[1]
# yaw = euler[2]
roll = 0.0
pitch = numpy.deg2rad(90.0)
yaw = 0.0
z += 0.1
self.move_pose_arm(roll, pitch, yaw, x, y, z)
x = -curr_pose.position.y
y = 0.6
z -= 0.1
#z = self.pose_object.position.z+0.05
# if self.object_name == "cylinder":
# yaw = numpy.deg2rad(90.0)
#z+= 0.001
self.move_pose_arm(roll, pitch, yaw, x, y, z)
rospy.sleep(0.5)
z -= 0.1
self.move_pose_arm(roll, pitch, yaw, x, y, z)
self.move_joint_hand(0)
#if self.object_name == "cylinder":
z += 0.1
self.move_pose_arm(roll, pitch, yaw, x, y, z)
# target_pose = curr_pose
# target_pose.position.x = x
# target_pose.position.y = y
# target_pose.position.z = z
# q = quaternion_from_euler(roll, pitch, yaw)
# target_pose.orientation = Quaternion(*q)
# place = self.generate_places(target_pose)
# self.arm.place(self.object_name, place)
# self.arm.detach_object(self.object_name)
# self.clean_scene(self.object_name)
rospy.loginfo('Place successfully')
rospy.loginfo("Moving arm to HOME point")
self.move_pose_arm(0, 0.8, 0, 0.4, 0, 0.6)
rospy.sleep(1)
def clean_scene(self, object_name):
self.scene.remove_world_object(object_name)
def add_ground(self):
p = PoseStamped()
p.header.frame_id = self.robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
p.pose.position.x = 0.0
p.pose.position.y = 0.0
p.pose.position.z = -0.01
size = (5, 5, 0.02)
self.scene.add_box("ground", p, size)
def add_object(self, name):
p = PoseStamped()
p.header.frame_id = self.robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
dx = 0
dy = 0
dz = 0
if name == "box":
p.pose.position.x = 0.5
p.pose.position.y = 0.0
p.pose.position.z = 0.015 + 0.115
q = quaternion_from_euler(0.0, 0.0, 0.0)
p.pose.orientation = Quaternion(*q)
size = (0.03, 0.03, 0.03)
self.scene.add_box(name, p, size)
dz = 0.16
elif name == "cylinder":
p.pose.position.x = 0.5
p.pose.position.y = 0.2
p.pose.position.z = 0.05 + 0.115
q = quaternion_from_euler(0.0, 0.0, 0.0)
p.pose.orientation = Quaternion(*q)
height = 0.1
radius = 0.03
self.scene.add_cylinder(name, p, height, radius)
dz = 0.16
#dx = -0.135
elif name == "sphere":
p.pose.position.x = 0.5
p.pose.position.y = -0.2
p.pose.position.z = 0.03 + 0.115
q = quaternion_from_euler(0.0, 0.0, 0.0)
p.pose.orientation = Quaternion(*q)
radius = 0.03
self.scene.add_sphere(name, p, radius)
dz = 0.15
return p.pose, dx, dy, dz
def move_pose_arm(self, roll, pitch, yaw, x, y, z):
pose_goal = geometry_msgs.msg.Pose()
quat = quaternion_from_euler(roll, pitch, yaw)
pose_goal.orientation.x = quat[0]
pose_goal.orientation.y = quat[1]
pose_goal.orientation.z = quat[2]
pose_goal.orientation.w = quat[3]
pose_goal.position.x = x
pose_goal.position.y = y
pose_goal.position.z = z
self.arm.set_pose_target(pose_goal)
self.arm.go(wait=True)
self.arm.stop() # To guarantee no residual movement
self.arm.clear_pose_targets()
# Move the Robotiq gripper by master axis
def move_joint_hand(self, gripper_finger1_joint):
joint_goal = self.gripper.get_current_joint_values()
joint_goal[2] = gripper_finger1_joint # Gripper master axis
self.gripper.go(joint_goal, wait=True)
self.gripper.stop() # To guarantee no residual movement
def generate_grasps(self, name, pose):
grasps = []
now = rospy.Time.now()
#for angle in numpy.arange(0.0, numpy.deg2rad(360.0), numpy.deg2rad(10.0)):
# Create place location:
angle = 0
grasp = Grasp()
grasp.grasp_pose.header.stamp = now
grasp.grasp_pose.header.frame_id = self.robot.get_planning_frame()
grasp.grasp_pose.pose = copy.deepcopy(pose)
# Setting pre-grasp approach
grasp.pre_grasp_approach.direction.header.stamp = now
grasp.pre_grasp_approach.direction.header.frame_id = self.robot.get_planning_frame(
)
grasp.pre_grasp_approach.direction.vector.z = -0.2
grasp.pre_grasp_approach.min_distance = self.approach_retreat_min_dist
grasp.pre_grasp_approach.desired_distance = self.approach_retreat_desired_dist
# Setting post-grasp retreat
grasp.post_grasp_retreat.direction.header.stamp = now
grasp.post_grasp_retreat.direction.header.frame_id = self.robot.get_planning_frame(
)
grasp.post_grasp_retreat.direction.vector.z = 0.2
grasp.post_grasp_retreat.min_distance = self.approach_retreat_min_dist
grasp.post_grasp_retreat.desired_distance = self.approach_retreat_desired_dist
# if name != "cylinder":
q = quaternion_from_euler(0.0, numpy.deg2rad(90.0), angle)
grasp.grasp_pose.pose.orientation = Quaternion(*q)
# else:
# #grasp.pre_grasp_approach.direction.vector.z = -0.05
# grasp.pre_grasp_approach.direction.vector.x = 0.1
# #grasp.post_grasp_retreat.direction.vector.z = 0.05
# grasp.post_grasp_retreat.direction.vector.x = -0.1
grasp.max_contact_force = 100
grasp.pre_grasp_posture.joint_names.append("gripper_finger1_joint")
traj = JointTrajectoryPoint()
traj.positions.append(0.03)
traj.time_from_start = rospy.Duration.from_sec(0.5)
grasp.pre_grasp_posture.points.append(traj)
grasp.grasp_posture.joint_names.append("gripper_finger1_joint")
traj = JointTrajectoryPoint()
if name == "box":
traj.positions.append(0.57)
elif name == "sphere":
traj.positions.append(0.3)
else:
traj.positions.append(0.3)
#traj.velocities.append(0.2)
traj.effort.append(800)
traj.time_from_start = rospy.Duration.from_sec(5.0)
grasp.grasp_posture.points.append(traj)
grasps.append(grasp)
return grasps
def generate_places(self, target):
#places = []
now = rospy.Time.now()
# for angle in numpy.arange(0.0, numpy.deg2rad(360.0), numpy.deg2rad(30.0)):
# Create place location:
place = PlaceLocation()
place.place_pose.header.stamp = now
place.place_pose.header.frame_id = self.robot.get_planning_frame()
# Set target position:
place.place_pose.pose = copy.deepcopy(target)
# Generate orientation (wrt Z axis):
# q = quaternion_from_euler(0.0, 0, 0.0)
# place.place_pose.pose.orientation = Quaternion(*q)
# Generate pre place approach:
place.pre_place_approach.desired_distance = self.approach_retreat_desired_dist
place.pre_place_approach.min_distance = self.approach_retreat_min_dist
place.pre_place_approach.direction.header.stamp = now
place.pre_place_approach.direction.header.frame_id = self.robot.get_planning_frame(
)
place.pre_place_approach.direction.vector.x = 0
place.pre_place_approach.direction.vector.y = 0
place.pre_place_approach.direction.vector.z = -0.2
# Generate post place approach:
place.post_place_retreat.direction.header.stamp = now
place.post_place_retreat.direction.header.frame_id = self.robot.get_planning_frame(
)
place.post_place_retreat.desired_distance = self.approach_retreat_desired_dist
place.post_place_retreat.min_distance = self.approach_retreat_min_dist
place.post_place_retreat.direction.vector.x = 0
place.post_place_retreat.direction.vector.y = 0
place.post_place_retreat.direction.vector.z = 0.2
place.allowed_touch_objects.append(self.object_name)
place.post_place_posture.joint_names.append("gripper_finger1_joint")
traj = JointTrajectoryPoint()
traj.positions.append(0)
#traj.effort.append(0)
traj.time_from_start = rospy.Duration.from_sec(1.0)
place.post_place_posture.points.append(traj)
# Add place:
#places.append(place)
return place
class ModelManager:
def __init__(self):
rospy.wait_for_service("gazebo/spawn_sdf_model")
self.spawn_model_srv = rospy.ServiceProxy("gazebo/spawn_sdf_model",
SpawnModel)
rospy.wait_for_service("gazebo/spawn_sdf_model")
self.delete_model_srv = rospy.ServiceProxy("gazebo/delete_model",
DeleteModel)
self.scene = PlanningSceneInterface()
self.robot = RobotCommander()
rospy.sleep(1)
self.object_list = {}
def pose2msg(self, x, y, z, roll, pitch, yaw):
pose = Pose()
pose.position.x = x
pose.position.y = y
pose.position.z = z
quat = quaternion_from_euler(roll, pitch, yaw)
pose.orientation.x = quat[0]
pose.orientation.y = quat[1]
pose.orientation.z = quat[2]
pose.orientation.w = quat[3]
return pose
def msg2pose(self, pose):
x = pose.position.x
y = pose.position.y
z = pose.position.z
quaternion = (pose.orientation.x, pose.orientation.y,
pose.orientation.z, pose.orientation.w)
euler = euler_from_quaternion(quaternion)
roll = euler[0]
pitch = euler[1]
yaw = euler[2]
return roll, pitch, yaw, x, y, z
def spawn_model(self, model_name, model_pose):
with open(
os.path.join(
rospkg.RosPack().get_path('irb120_robotiq85_gazebo'),
'models', model_name, 'model.sdf'), "r") as f:
model_xml = f.read()
self.spawn_model_srv(model_name, model_xml, "", model_pose, "world")
def delete_model(self, model_name):
self.delete_model_srv(model_name)
def clean_scene(self, object_name):
self.scene.remove_world_object(object_name)
def respawn_all_objects(self):
objects_name = self.object_list.keys()
for object_name in objects_name:
this_object = self.object_list[object_name]
print("Respawning {}".format(object_name))
# remove old objects in Gazebo
self.delete_model(object_name)
# respawn new objects in Gazebo
roll, pitch, yaw, x, y, z = self.msg2pose(this_object.abs_pose)
object_pose = self.pose2msg(x, y, z, roll, pitch, yaw)
self.spawn_model(object_name, object_pose)
# respawn objects in Rviz
p = PoseStamped()
p.header.frame_id = self.robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
self.clean_scene(object_name)
p.pose = copy.copy(this_object.relative_pose)
shape = this_object.shape
if shape == "box":
x = this_object.length
y = this_object.width
z = this_object.height
size = (x, y, z)
self.scene.add_box(object_name, p, size)
elif shape == "cylinder":
height = this_object.height
radius = this_object.width / 2
self.scene.add_cylinder(object_name, p, height, radius)
elif shape == "sphere":
radius = this_object.width / 2
self.scene.add_sphere(object_name, p, radius)
rospy.sleep(0.5)
rospy.loginfo("All objects are respawned")
def spawn_all_model(self):
filename = os.path.join(
rospkg.RosPack().get_path('rqt_industrial_robot'), 'src',
'rqt_kinematics', 'interfaces', 'models_info.yaml')
with open(filename) as file:
objects_info = yaml.load(file)
robot_x = objects_info["robot"]["pose"]["x"]
robot_y = objects_info["robot"]["pose"]["y"]
robot_z = objects_info["robot"]["pose"]["z"]
robot_roll = objects_info["robot"]["pose"]["roll"]
robot_pitch = objects_info["robot"]["pose"]["pitch"]
robot_yaw = objects_info["robot"]["pose"]["yaw"]
rospy.loginfo("Spawning Objects in Gazebo and planning scene")
objects = objects_info["objects"]
objects_name = objects.keys()
for object_name in objects_name:
name = object_name
shape = objects[name]["shape"]
color = objects[name]["color"]
# add object in Gazebo
# self.delete_model(object_name)
x = objects[name]["pose"]["x"]
y = objects[name]["pose"]["y"]
z = objects[name]["pose"]["z"]
roll = objects[name]["pose"]["roll"]
pitch = objects[name]["pose"]["pitch"]
yaw = objects[name]["pose"]["yaw"]
object_pose = self.pose2msg(x, y, z, roll, pitch, yaw)
self.spawn_model(object_name, object_pose)
## add object in planning scene(Rviz)
p = PoseStamped()
p.header.frame_id = self.robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
self.clean_scene(name)
p.pose.position.x = x - robot_x
p.pose.position.y = y - robot_y
p.pose.position.z = z - robot_z
q = quaternion_from_euler(roll, pitch, yaw)
p.pose.orientation = Quaternion(*q)
if shape == "box":
x = objects[name]["size"]["x"]
y = objects[name]["size"]["y"]
z = objects[name]["size"]["z"]
p.pose.position.z += z / 2
size = (x, y, z)
self.scene.add_box(name, p, size)
height = z
width = y
length = x
self.object_list[name] = Object(p.pose, object_pose,
height, width, length,
shape, color)
elif shape == "cylinder":
height = objects[name]["size"]["height"]
radius = objects[name]["size"]["radius"]
p.pose.position.z += height / 2
self.scene.add_cylinder(name, p, height, radius)
self.object_list[name] = Object(p.pose, object_pose,
height, radius * 2,
radius * 2, shape, color)
elif shape == "sphere":
radius = objects[name]["size"]
p.pose.position.z += radius
self.scene.add_sphere(name, p, radius)
self.object_list[name] = Object(p.pose, object_pose,
radius * 2, radius * 2,
radius * 2, shape, color)
rospy.sleep(0.5)
rospy.loginfo("Spawning Obstacles in planning scene")
obstacles = objects_info["obstacles"]
obstacles_name = obstacles.keys()
for obstacle_name in obstacles_name:
name = obstacle_name
p = PoseStamped()
p.header.frame_id = self.robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
self.clean_scene(name)
p.pose.position.x = obstacles[name]["pose"]["x"] - robot_x
p.pose.position.y = obstacles[name]["pose"]["y"] - robot_y
p.pose.position.z = obstacles[name]["pose"]["z"] - robot_z
q = quaternion_from_euler(robot_roll, robot_pitch, robot_yaw)
p.pose.orientation = Quaternion(*q)
x = obstacles[name]["size"]["x"]
y = obstacles[name]["size"]["y"]
z = obstacles[name]["size"]["z"]
size = (x, y, z)
self.scene.add_box(name, p, size)
rospy.sleep(0.5)
print ">>>>> add scenes"
# publish a demo scene
p = PoseStamped()
p.header.frame_id = robot.get_planning_frame()
p.pose.position.x = 0
p.pose.position.y = -1
p.pose.position.z = 0.2
p.pose.orientation.w = 1.0
scene.add_box("pole", p, (0.4, 0.1, 0.4))
p.pose.position.x = 0
p.pose.position.y = 0.8
p.pose.position.z = 0.1
p.pose.orientation.w = 1.0
scene.add_sphere("ball", p, 0.1)
p.pose.position.x = 0
p.pose.position.y = 0
p.pose.position.z = -0.1
scene.add_box("table", p, (1.0, 2.6, 0.2))
p.pose.position.x = 0
p.pose.position.y = 0
p.pose.position.z = -0.4
scene.add_plane("ground_plane", p)
rospy.sleep(20)
## We can get the name of the reference frame for this robot
print ">>>>> Reference frame: %s" % group.get_planning_frame()
class Pick_Place:
def __init__(self):
# rospy.init_node('pick_and_place')
self.scene = PlanningSceneInterface()
self.robot = RobotCommander()
filename = os.path.join(
rospkg.RosPack().get_path('rqt_industrial_robot'), 'src',
'rqt_mobile_manipulator', 'joints_setup.yaml')
with open(filename) as file:
joints_setup = yaml.load(file)
jointslimit = joints_setup["joints_limit"]
home_value = joints_setup["home_value"]
j1 = home_value["joint_1"]
j2 = home_value["joint_2"]
j3 = home_value["joint_3"]
j4 = home_value["joint_4"]
j5 = home_value["joint_5"]
j6 = home_value["joint_6"]
g = home_value["gripper"]
self.set_home_value([j1, j2, j3, j4, j5, j6, g])
home_value = joints_setup["pick_place_home_value"]
j1 = home_value["joint_1"]
j2 = home_value["joint_2"]
j3 = home_value["joint_3"]
j4 = home_value["joint_4"]
j5 = home_value["joint_5"]
j6 = home_value["joint_6"]
g = home_value["gripper"]
self.set_pick_place_home_value([j1, j2, j3, j4, j5, j6, g])
self.object_list = {}
self.obstacle_list = {}
filename = os.path.join(
rospkg.RosPack().get_path('rqt_industrial_robot'), 'src',
'rqt_mobile_manipulator', 'interfaces', 'models_info.yaml')
with open(filename) as file:
objects_info = yaml.load(file)
rospy.loginfo("Spawning Objects in Gazebo and planning scene")
objects = objects_info["objects"]
objects_name = objects.keys()
for object_name in objects_name:
name = object_name
shape = objects[name]["shape"]
color = objects[name]["color"]
x = objects[name]["pose"]["x"]
y = objects[name]["pose"]["y"]
z = objects[name]["pose"]["z"]
roll = objects[name]["pose"]["roll"]
pitch = objects[name]["pose"]["pitch"]
yaw = objects[name]["pose"]["yaw"]
object_pose = self.pose2msg(x, y, z, roll, pitch, yaw)
p = PoseStamped()
p.header.frame_id = self.robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
p.pose.position.x = x
p.pose.position.y = y
p.pose.position.z = z
q = quaternion_from_euler(roll, pitch, yaw)
p.pose.orientation = Quaternion(*q)
if shape == "box":
x = objects[name]["size"]["x"]
y = objects[name]["size"]["y"]
z = objects[name]["size"]["z"]
p.pose.position.z += z / 2
height = z
width = y
length = x
self.object_list[name] = Object(p.pose, p.pose, height,
width, length, shape,
color)
elif shape == "cylinder":
height = objects[name]["size"]["height"]
radius = objects[name]["size"]["radius"]
p.pose.position.z += height / 2
self.object_list[name] = Object(p.pose, p.pose, height,
radius * 2, radius * 2,
shape, color)
elif shape == "sphere":
radius = objects[name]["size"]
p.pose.position.z += radius
self.object_list[name] = Object(p.pose, p.pose, radius * 2,
radius * 2, radius * 2,
shape, color)
objects = objects_info["objects"]
obstacles = objects_info["obstacles"]
obstacles_name = obstacles.keys()
for object_name in obstacles_name:
name = object_name
x = obstacles[name]["pose"]["x"]
y = obstacles[name]["pose"]["y"]
z = obstacles[name]["pose"]["z"]
roll = obstacles[name]["pose"]["roll"]
pitch = obstacles[name]["pose"]["pitch"]
yaw = obstacles[name]["pose"]["yaw"]
object_pose = self.pose2msg(x, y, z, roll, pitch, yaw)
p = PoseStamped()
p.header.frame_id = self.robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
p.pose.position.x = x
p.pose.position.y = y
p.pose.position.z = z
x = obstacles[name]["size"]["x"]
y = obstacles[name]["size"]["y"]
z = obstacles[name]["size"]["z"]
p.pose.position.z += z / 2
height = z
width = y
length = x
self.obstacle_list[name] = Obstacle(p.pose, p.pose, height,
width, length)
# self.object_list = object_list
self.goal_list = {}
self.set_target_info()
self.gripper_width = {}
self.set_gripper_width_relationship()
self.arm = moveit_commander.MoveGroupCommander("ur10_manipulator")
self.arm.set_goal_tolerance(0.01)
self.arm.set_pose_reference_frame("ur10_base_link")
self.gripperpub = rospy.Publisher("gripper_controller/command",
JointTrajectory,
queue_size=0)
self.transform_arm_to_baselink = Point()
self.get_arm_to_baselink()
self.gripper_length = 0.33
self.get_workspace()
self.message_pub = rospy.Publisher("/gui_message",
String,
queue_size=0)
self.updatepose_pub = rospy.Publisher("/updatepose",
Bool,
queue_size=0)
self.robot_pose = Pose()
self.odom_sub = rospy.Subscriber("/odom", Odometry,
self.robot_pose_callback)
def send_message(self, message):
msg = String()
msg.data = message
self.message_pub.publish(msg)
def updatepose_trigger(self, value):
msg = Bool()
msg.data = value
self.updatepose_pub.publish(msg)
def clean_scene(self, object_name):
self.scene.remove_world_object(object_name)
def clean_all_objects_in_scene(self):
objects_name = self.object_list.keys()
for object_name in objects_name:
self.clean_scene(object_name)
def spawn_all_objects(self):
objects_name = self.object_list.keys()
for object_name in objects_name:
self.spawn_object_rviz(object_name)
def spawn_object_rviz(self, object_name):
self.clean_scene(object_name)
this_object = self.object_list[object_name]
p = PoseStamped()
p.header.frame_id = self.robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
p.pose = copy.deepcopy(this_object.abs_pose)
robot_pose = self.get_robot_pose()
p.pose.position.x -= robot_pose.position.x
p.pose.position.y -= robot_pose.position.y
quaternion = (robot_pose.orientation.x, robot_pose.orientation.y,
robot_pose.orientation.z, robot_pose.orientation.w)
euler = euler_from_quaternion(quaternion)
roll = euler[0]
pitch = euler[1]
yaw = -euler[2]
quat = quaternion_from_euler(roll, pitch, yaw)
p.pose.orientation.x = quat[0]
p.pose.orientation.y = quat[1]
p.pose.orientation.z = quat[2]
p.pose.orientation.w = quat[3]
self.object_list[object_name].relative_pose = self.baselink2arm(p.pose)
shape = this_object.shape
if shape == "box":
x = this_object.length
y = this_object.width
z = this_object.height
size = (x, y, z)
self.scene.add_box(object_name, p, size)
elif shape == "cylinder":
height = this_object.height
radius = this_object.width / 2
self.scene.add_cylinder(object_name, p, height, radius)
elif shape == "sphere":
radius = this_object.width / 2
self.scene.add_sphere(object_name, p, radius)
rospy.sleep(0.5)
def spawn_obstacle_rviz(self, obstacle_name):
self.clean_scene(obstacle_name)
this_object = self.obstacle_list[obstacle_name]
p = PoseStamped()
p.header.frame_id = self.robot.get_planning_frame()
p.header.stamp = rospy.Time.now()
p.pose = copy.deepcopy(this_object.abs_pose)
robot_pose = self.get_robot_pose()
# p.pose.position.x -= robot_pose.position.x
# p.pose.position.y -= robot_pose.position.y
quaternion = (robot_pose.orientation.x, robot_pose.orientation.y,
robot_pose.orientation.z, robot_pose.orientation.w)
euler = euler_from_quaternion(quaternion)
roll = euler[0]
pitch = euler[1]
yaw = -euler[2]
quat = quaternion_from_euler(roll, pitch, yaw)
p.pose.orientation.x = quat[0]
p.pose.orientation.y = quat[1]
p.pose.orientation.z = quat[2]
p.pose.orientation.w = quat[3]
x = p.pose.position.x - robot_pose.position.x
y = p.pose.position.y - robot_pose.position.y
p.pose.position.x = math.cos(yaw) * x - math.sin(yaw) * y
p.pose.position.y = math.sin(yaw) * x + math.cos(yaw) * y
x = this_object.length
y = this_object.width
z = this_object.height
size = (x, y, z)
self.scene.add_box(obstacle_name, p, size)
rospy.sleep(0.5)
def set_target_info(self):
filename = os.path.join(
rospkg.RosPack().get_path('rqt_industrial_robot'), 'src',
'rqt_mobile_manipulator', 'interfaces', 'models_info.yaml')
with open(filename) as file:
objects_info = yaml.load(file)
robot_x = objects_info["robot"]["pose"]["x"]
robot_y = objects_info["robot"]["pose"]["y"]
robot_z = objects_info["robot"]["pose"]["z"]
targets = objects_info["targets"]
target_name = targets.keys()
for name in target_name:
position = Point()
position.x = targets[name]["x"] - robot_x
position.y = targets[name]["y"] - robot_y
position.z = targets[name]["z"] - robot_z
self.goal_list[name] = position
def set_gripper_width_relationship(self):
filename = os.path.join(
rospkg.RosPack().get_path('rqt_industrial_robot'), 'src',
'rqt_mobile_manipulator', 'interfaces', 'models_info.yaml')
with open(filename) as file:
objects_info = yaml.load(file)
gripper_joint_value = objects_info["gripper_joint_value"]
objects_width = gripper_joint_value.keys()
for object_width in objects_width:
self.gripper_width[object_width] = gripper_joint_value[
object_width]
def get_object_list(self):
return self.object_list.keys()
def get_target_list(self):
return self.goal_list.keys()
def get_object_pose(self, object_name):
return copy.deepcopy(self.object_list[object_name].relative_pose)
def get_object_info(self, object_name):
this_object = copy.deepcopy(self.object_list[object_name])
pose = this_object.relative_pose
height = this_object.height
width = this_object.width
length = this_object.length
shape = this_object.shape
color = this_object.color
return pose, height, width, length, shape, color
def get_target_position(self, target_name):
position = copy.deepcopy(self.goal_list[target_name])
# print("before transformation",position)
p = Pose()
p.position = position
robot_pose = self.get_robot_pose()
quaternion = (robot_pose.orientation.x, robot_pose.orientation.y,
robot_pose.orientation.z, robot_pose.orientation.w)
euler = euler_from_quaternion(quaternion)
yaw = -euler[2]
x = position.x - robot_pose.position.x
y = position.y - robot_pose.position.y
position.x = math.cos(yaw) * x - math.sin(yaw) * y
position.y = math.sin(yaw) * x + math.cos(yaw) * y
# print("after transformation",position, yaw)
position = self.baselink2arm(p).position
return position
def robot_pose_callback(self, msg):
self.robot_pose.position = msg.pose.pose.position
self.robot_pose.orientation = msg.pose.pose.orientation
def get_robot_pose(self):
# try:
# listener = tf.TransformListener()
# (trans,rot) = listener.lookupTransform('/map', "/base_link", rospy.Time(0))
# except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
# rospy.loginfo("no transformation")
# return
# pose = Pose()
# pose.position.x = trans[0]
# pose.position.y = trans[1]
# pose.position.z = trans[2]
# pose.orientation.x = rot[0]
# pose.orientation.y = rot[1]
# pose.orientation.z = rot[2]
# pose.orientation.w = rot[3]
return self.robot_pose
def get_arm_to_baselink(self):
# try:
# listener = tf.TransformListener()
# (trans,rot) = listener.lookupTransform('/base_link', "/ur10_base_link", rospy.Time(0))
# except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
# rospy.loginfo("no transformation")
# return
# self.transform_arm_to_baselink.x = trans[0]
# self.transform_arm_to_baselink.y = trans[1]
# self.transform_arm_to_baselink.z = trans[2]
# print(self.transform_arm_to_baselink)
self.transform_arm_to_baselink.x = 0.205
self.transform_arm_to_baselink.y = 0
self.transform_arm_to_baselink.z = 0.802
def get_workspace(self):
filename = os.path.join(
rospkg.RosPack().get_path('rqt_industrial_robot'), 'src',
'rqt_mobile_manipulator', 'joints_setup.yaml')
with open(filename) as file:
joints_setup = yaml.load(file)
workspace = joints_setup["workspace"]
x = workspace["center"]["x"]
y = workspace["center"]["y"]
z = workspace["center"]["z"]
min_r = workspace["r"]["min"]
max_r = workspace["r"]["max"]
min_z = workspace["min_z"]
self.workspace = WorkSpace(x, y, z, min_r, max_r, min_z)
# check if the position is inside workspace
def is_inside_workspace(self, x, y, z):
if z > self.workspace.min_z:
dx = x - self.workspace.x
dy = y - self.workspace.y
dz = z - self.workspace.z
r = math.sqrt(dx**2 + dy**2 + dz**2)
if self.workspace.min_r < r < self.workspace.max_r:
return True
return False
# move one joint of the arm to value
def set_arm_joint(self, joint_id, value):
joint_goal = self.arm.get_current_joint_values()
joint_goal[joint_id - 1] = value
self.arm.go(joint_goal, wait=True)
self.arm.stop()
# Forward Kinematics (FK): move the arm by axis values
def move_joint_arm(self, joint_0, joint_1, joint_2, joint_3, joint_4,
joint_5):
joint_goal = self.arm.get_current_joint_values()
joint_goal[0] = joint_0
joint_goal[1] = joint_1
joint_goal[2] = joint_2
joint_goal[3] = joint_3
joint_goal[4] = joint_4
joint_goal[5] = joint_5
self.arm.go(joint_goal, wait=True)
self.arm.stop() # To guarantee no residual movement
def get_joint_value(self, joint_id):
joints = self.arm.get_current_joint_values()
return joints[joint_id - 1]
def get_joints_value(self):
joints = self.arm.get_current_joint_values()
return joints
def get_arm_pose(self):
pose = self.arm.get_current_pose().pose
pose = self.baselink2arm(pose)
pose = self.TCP2gripper(pose, self.gripper_length)
# print(pose)
return self.msg2pose(pose)
def set_random_pose(self):
self.arm.set_random_target()
def set_gripper_length(self, length):
self.gripper_length = length
def set_home_value(self, home_value):
self.home_value = home_value
def back_to_home(self, move_gripper=True):
j1, j2, j3, j4, j5, j6, g = self.home_value
self.move_joint_arm(j1, j2, j3, j4, j5, j6)
if move_gripper:
self.move_joint_hand(g)
def set_pick_place_home_value(self, home_value):
self.pick_place_home_value = home_value
def move_to_pick_place_home(self, move_gripper=True):
j1, j2, j3, j4, j5, j6, g = self.pick_place_home_value
self.move_joint_arm(j1, j2, j3, j4, j5, j6)
if move_gripper:
self.move_joint_hand(g)
def fold_robot_arm(self, ):
self.move_joint_arm(0, 0, 0, 0, 0, 0)
self.move_joint_hand(0)
def gripper2TCP(self, pose, length=0):
roll, pitch, yaw, x, y, z = self.msg2pose(pose)
T = euler_matrix(roll, pitch, yaw)
T[0:3, 3] = numpy.array([x, y, z])
pos_gripper_tcp = numpy.array([0, -length, 0, 1])
pos_tcp = T.dot(pos_gripper_tcp)
pose.position.x = pos_tcp[0]
pose.position.y = pos_tcp[1]
pose.position.z = pos_tcp[2]
return pose
def TCP2gripper(self, pose, length=0):
roll, pitch, yaw, x, y, z = self.msg2pose(pose)
T = euler_matrix(roll, pitch, yaw)
T[0:3, 3] = numpy.array([x, y, z])
pos_gripper_tcp = numpy.array([0, length, 0, 1])
pos_tcp = T.dot(pos_gripper_tcp)
pose.position.x = pos_tcp[0]
pose.position.y = pos_tcp[1]
pose.position.z = pos_tcp[2]
return pose
def arm2baselink(self, in_pose):
pose = copy.deepcopy(in_pose)
pose.position.x += self.transform_arm_to_baselink.x
pose.position.y += self.transform_arm_to_baselink.y
pose.position.z += self.transform_arm_to_baselink.z
return pose
def baselink2arm(self, in_pose):
pose = copy.deepcopy(in_pose)
pose.position.x -= self.transform_arm_to_baselink.x
pose.position.y -= self.transform_arm_to_baselink.y
pose.position.z -= self.transform_arm_to_baselink.z
return pose
# Inverse Kinematics (IK): move TCP to given position and orientation
def move_pose_arm(self, pose_goal):
# pose_goal = self.pose2msg(roll, pitch, yaw, x, y, z)
# pose_goal = self.gripper2TCP(pose_goal, self.gripper_length)
x = pose_goal.position.x
y = pose_goal.position.y
z = pose_goal.position.z
if not self.is_inside_workspace(x, y, z):
rospy.loginfo('***** GOAL POSE IS OUT OF ROBOT WORKSPACE *****')
return False
self.arm.set_pose_target(pose_goal)
self.arm.go(wait=True)
self.arm.stop() # To guarantee no residual movement
self.arm.clear_pose_targets()
return True
# Move gripper
def move_joint_hand(self, finger1_goal, finger2_goal=10, finger3_goal=10):
if finger2_goal == 10 and finger3_goal == 10:
finger2_goal, finger3_goal = finger1_goal, finger1_goal
jointtrajectory = JointTrajectory()
jointtrajectory.header.stamp = rospy.Time.now()
jointtrajectory.joint_names.extend(["H1_F1J3", "H1_F2J3", "H1_F3J3"])
joint = JointTrajectoryPoint()
joint.positions.extend([finger1_goal, finger2_goal, finger3_goal])
joint.time_from_start = rospy.Duration(1)
jointtrajectory.points.append(joint)
self.gripperpub.publish(jointtrajectory)
def pose2msg(self, roll, pitch, yaw, x, y, z):
pose = geometry_msgs.msg.Pose()
quat = quaternion_from_euler(roll, pitch, yaw)
pose.orientation.x = quat[0]
pose.orientation.y = quat[1]
pose.orientation.z = quat[2]
pose.orientation.w = quat[3]
pose.position.x = x
pose.position.y = y
pose.position.z = z
return pose
def msg2pose(self, pose):
x = pose.position.x
y = pose.position.y
z = pose.position.z
quaternion = (pose.orientation.x, pose.orientation.y,
pose.orientation.z, pose.orientation.w)
euler = euler_from_quaternion(quaternion)
roll = euler[0]
pitch = euler[1]
yaw = euler[2]
return roll, pitch, yaw, x, y, z
def set_grasp_distance(self, min_distance, desired_distance):
self.approach_retreat_min_dist = min_distance
self.approach_retreat_desired_dist = desired_distance
def count_gripper_width(self, object_name):
object_width = self.object_list[object_name].width
if object_width in self.gripper_width:
return self.gripper_width[object_width]
else:
rospy.loginfo(
"Cannot find suitable gripper joint value for this object width"
)
return 0
# pick object to goal position
def pickup(self, object_name, position, width, distance=0.12):
if not self.is_inside_workspace(position.x, position.y, position.z):
rospy.loginfo('***** GOAL POSE IS OUT OF ROBOT WORKSPACE *****')
rospy.loginfo('Stop placing')
return
pose = Pose()
pose.position = position
q = quaternion_from_euler(numpy.deg2rad(-90), 0, numpy.deg2rad(180))
pose.orientation = Quaternion(*q)
# transform from gripper to TCP
pose = self.gripper2TCP(pose, self.gripper_length)
pose.position.z += distance
rospy.loginfo('Start picking')
self.move_pose_arm(pose)
rospy.sleep(1)
# move down
waypoints = []
wpose = self.arm.get_current_pose().pose
wpose = self.baselink2arm(wpose)
print(wpose)
wpose.position.z -= distance
waypoints.append(copy.deepcopy(wpose))
(plan, fraction) = self.arm.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
self.arm.execute(plan, wait=True)
self.updatepose_trigger(True)
# pick
rospy.sleep(0.5)
self.move_joint_hand(width)
rospy.sleep(3)
# if object_name != "yellow_ball":
# self.arm.attach_object(object_name)
# move up
waypoints = []
wpose = self.arm.get_current_pose().pose
wpose = self.baselink2arm(wpose)
wpose.position.z += distance
waypoints.append(copy.deepcopy(wpose))
(plan, fraction) = self.arm.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
self.arm.execute(plan, wait=True)
self.updatepose_trigger(True)
rospy.loginfo('Pick finished')
# place object to goal position
def place(self, object_name, position, width=-0.2, distance=0.12):
if not self.is_inside_workspace(position.x, position.y, position.z):
rospy.loginfo('***** GOAL POSE IS OUT OF ROBOT WORKSPACE *****')
rospy.loginfo('Stop placing')
return
pose = Pose()
pose.position = position
q = quaternion_from_euler(numpy.deg2rad(-90), 0, numpy.deg2rad(180))
pose.orientation = Quaternion(*q)
# transform from gripper to TCP
pose = self.gripper2TCP(pose, self.gripper_length)
pose.position.z += distance
rospy.loginfo('Start placing')
self.move_pose_arm(pose)
rospy.sleep(1)
# move down
waypoints = []
wpose = self.arm.get_current_pose().pose
wpose = self.baselink2arm(wpose)
print(wpose)
wpose.position.z -= distance
waypoints.append(copy.deepcopy(wpose))
(plan, fraction) = self.arm.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
self.arm.execute(plan, wait=True)
self.updatepose_trigger(True)
# place
self.move_joint_hand(width)
rospy.sleep(3)
# if object_name != "yellow_ball":
# self.arm.detach_object(object_name)
# self.clean_scene(object_name)
self.object_list.pop(object_name)
# move up
waypoints = []
wpose = self.arm.get_current_pose().pose
wpose = self.baselink2arm(wpose)
wpose.position.z += distance
waypoints.append(copy.deepcopy(wpose))
(plan, fraction) = self.arm.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
self.arm.execute(plan, wait=True)
self.updatepose_trigger(True)
rospy.loginfo('Place finished')
