class PickAndPlace:
def __init__(self, robot_name="panda_arm", frame="panda_link0"):
try:
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node(name="pick_place_test")
self.scene = PlanningSceneInterface()
self.scene_pub = rospy.Publisher('planning_scene',
PlanningScene,
queue_size=10)
# region Robot initial
self.robot = MoveGroupCommander(robot_name)
self.robot.set_goal_joint_tolerance(0.00001)
self.robot.set_goal_position_tolerance(0.00001)
self.robot.set_goal_orientation_tolerance(0.01)
self.robot.set_goal_tolerance(0.00001)
self.robot.allow_replanning(True)
self.robot.set_pose_reference_frame(frame)
self.robot.set_planning_time(3)
# endregion
self.gripper = MoveGroupCommander("hand")
self.gripper.set_joint_value_target(GRIPPER_CLOSED)
self.gripper.go()
self.gripper.set_joint_value_target(GRIPPER_OPEN)
self.gripper.go()
self.gripper.set_joint_value_target(GRIPPER_CLOSED)
self.gripper.go()
# Robot go home
self.robot.set_named_target("home")
self.robot.go()
# clear all object in world
self.clear_all_object()
table_pose = un.Pose(0, 0, -10, 0, 0, 0)
table_color = un.Color(255, 255, 0, 100)
self.add_object_box("table", table_pose, table_color, frame,
(2000, 2000, 10))
bearing_pose = un.Pose(250, 250, 500, -90, 45, -90)
bearing_color = un.Color(255, 0, 255, 255)
bearing_file_name = "../stl/bearing.stl"
self.add_object_mesh("bearing", bearing_pose, bearing_color, frame,
bearing_file_name)
obpose = self.scene.get_object_poses(["bearing"])
# self.robot.set_support_surface_name("table")
g = Grasp()
# Create gripper position open or close
g.pre_grasp_posture = self.open_gripper()
g.grasp_posture = self.close_gripper()
g.pre_grasp_approach = self.make_gripper_translation(
0.01, 0.1, [0, 1.0, 0])
g.post_grasp_retreat = self.make_gripper_translation(
0.01, 0.9, [0, 1.0, 0])
p = PoseStamped()
p.header.frame_id = "panda_link0"
p.pose.orientation = obpose["bearing"].orientation
p.pose.position = obpose["bearing"].position
g.grasp_pose = p
g.allowed_touch_objects = ["bearing"]
a = []
a.append(g)
result = self.robot.pick(object_name="bearing", grasp=a)
print(result)
except Exception as ex:
print(ex)
moveit_commander.roscpp_shutdown()
moveit_commander.roscpp_shutdown()
def make_gripper_translation(self, min_dist, desired, vector):
g = GripperTranslation()
g.direction.vector.x = vector[0]
g.direction.vector.y = vector[1]
g.direction.vector.z = vector[2]
g.direction.header.frame_id = "panda_link0"
g.min_distance = min_dist
g.desired_distance = desired
return g
def open_gripper(self):
t = JointTrajectory()
t.joint_names = ['panda_finger_joint1', 'panda_finger_joint2']
tp = JointTrajectoryPoint()
tp.positions = [0.04, 0.04]
tp.time_from_start = rospy.Duration(secs=1)
t.points.append(tp)
return t
def close_gripper(self):
t = JointTrajectory()
t.joint_names = ['panda_finger_joint1', 'panda_finger_joint2']
tp = JointTrajectoryPoint()
tp.positions = [0.0, 0.0]
tp.time_from_start = rospy.Duration(secs=1)
t.points.append(tp)
return t
def clear_all_object(self):
for world_object in (self.scene.get_known_object_names()):
self.scene.remove_world_object(world_object)
for attached_object in (self.scene.get_attached_objects()):
self.scene.remove_attached_object(attached_object)
def add_object_box(self, object_name, pose, color, frame, size):
"""
add object in rviz
:param object_name: name of object
:param pose: pose of object. (xyz) in mm,(abc) in degree
:param color: color of object.(RGBA)
:param frame: reference_frame
:param size: size of object(mm)
:return: None
"""
# Add object
object_pose = PoseStamped()
object_pose.header.frame_id = frame
object_pose.pose.position.x = pose.X / 1000.0
object_pose.pose.position.y = pose.Y / 1000.0
object_pose.pose.position.z = pose.Z / 1000.0
quaternion = quaternion_from_euler(np.deg2rad(pose.A),
np.deg2rad(pose.B),
np.deg2rad(pose.C))
object_pose.pose.orientation.x = quaternion[0]
object_pose.pose.orientation.y = quaternion[1]
object_pose.pose.orientation.z = quaternion[2]
object_pose.pose.orientation.w = quaternion[3]
self.scene.add_box(name=object_name,
pose=object_pose,
size=(size[0] / 1000.0, size[1] / 1000.0,
size[2] / 1000.0))
# Add object color
object_color = ObjectColor()
object_color.id = object_name
object_color.color.r = color.R / 255.00
object_color.color.g = color.G / 255.00
object_color.color.b = color.B / 255.00
object_color.color.a = color.A / 255.00
p = PlanningScene()
p.is_diff = True
p.object_colors.append(object_color)
self.scene_pub.publish(p)
def add_object_mesh(self, object_name, pose, color, frame, file_name):
"""
add object in rviz
:param object_name: name of object
:param pose: pose of object. (xyz) in mm,(abc) in degree
:param color: color of object.(RGBA)
:param frame: reference_frame
:param file_name: mesh file name
:return: None
"""
# Add object
object_pose = PoseStamped()
object_pose.header.frame_id = frame
object_pose.pose.position.x = pose.X / 1000.0
object_pose.pose.position.y = pose.Y / 1000.0
object_pose.pose.position.z = pose.Z / 1000.0
quaternion = quaternion_from_euler(np.deg2rad(pose.A),
np.deg2rad(pose.B),
np.deg2rad(pose.C))
object_pose.pose.orientation.x = quaternion[0]
object_pose.pose.orientation.y = quaternion[1]
object_pose.pose.orientation.z = quaternion[2]
object_pose.pose.orientation.w = quaternion[3]
self.scene.add_mesh(name=object_name,
pose=object_pose,
filename=file_name,
size=(0.001, 0.001, 0.001))
# Add object color
object_color = ObjectColor()
object_color.id = object_name
object_color.color.r = color.R / 255.00
object_color.color.g = color.G / 255.00
object_color.color.b = color.B / 255.00
object_color.color.a = color.A / 255.00
p = PlanningScene()
p.is_diff = True
p.object_colors.append(object_color)
self.scene_pub.publish(p)
