def __get_grasp(self, name):
try:
open_state = self.__get_state(name + "_open", "").state
closed_state = self.__get_state(name + "_closed", "").state
except Exception:
rospy.logfatal("Couldn't get grasp pose from db.")
return Grasp()
try:
self.__group.set_start_state_to_current_state()
pre_pose = self.__group.plan(open_state.joint_state)
self.__group.set_start_state(open_state)
pose = self.__group.plan(closed_state.joint_state)
except Exception:
rospy.logfatal("Couldn't plan grasp trajectories.")
return Grasp()
grasp = Grasp()
grasp.id = name
grasp.pre_grasp_posture = pre_pose.joint_trajectory
grasp.grasp_posture = pose.joint_trajectory
grasp.pre_grasp_approach.desired_distance = 0.2
grasp.pre_grasp_approach.min_distance = 0.1
grasp.pre_grasp_approach.direction.vector.x = 0
grasp.pre_grasp_approach.direction.vector.y = -1
grasp.pre_grasp_approach.direction.vector.z = 0
return grasp
def make_grasps(self, initial_pose_stamped, allowed_touch_objects):
# Initialize the grasp object
g = Grasp()
# Set the pre-grasp and grasp postures appropriately
g.pre_grasp_posture = self.make_gripper_posture(GRIPPER_OPEN)
g.grasp_posture = self.make_gripper_posture(GRIPPER_CLOSED)
# Set the approach and retreat parameters as desired
g.pre_grasp_approach = self.make_gripper_translation(0.01, 0.2, [0.0, 1.0, 0.0])
g.post_grasp_retreat = self.make_gripper_translation(0.1, 0.2, [0.0, 0.0, 1.0])
# Set the first grasp pose to the input pose
g.grasp_pose = initial_pose_stamped
# Pitch angles to try
pitch_vals = [0, 0.1, -0.1, 0.2, -0.2, 0.3, -0.3]
# Yaw angles to try
yaw_vals = [0]
# A list to hold the grasps
grasps = []
# Generate a grasp for each pitch and yaw angle
for y in yaw_vals:
for p in pitch_vals:
def graspObj(self, x, y, z, phi, theta, psi):
grasps = []
g = Grasp()
g.id = "test"
grasp_pose = PoseStamped()
grasp_pose.header.frame_id = "base_footprint"
orientation_tuple = tf.transformations.quaternion_from_euler(
phi, theta, psi)
grasp_pose.orientation.x = orientation_tuple[0]
grasp_pose.orientation.y = orientation_tuple[1]
grasp_pose.orientation.z = orientation_tuple[2]
grasp_pose.orientation.w = orientation_tuple[3]
grasp_pose.position.x = x
grasp_pose.position.y = y
grasp_pose.position.z = z
self.group.set_pose_target(grasp_pose)
self.group.go()
rospy.sleep(2)
# set the grasp pose
g.grasp_pose = grasp_pose
self.group.pick("unit_box", g)
def make_grasps(self, initial_pose_stamped, allowed_touch_objects, grasp_opening=[0]):
# Initialize the grasp object
g = Grasp()
# Set the pre-grasp and grasp postures appropriately;
# grasp_opening should be a bit smaller than target width
g.pre_grasp_posture = self.make_gripper_posture(self.gripper_opened)
g.grasp_posture = self.make_gripper_posture(grasp_opening)
# Set the approach and retreat parameters as desired
g.pre_grasp_approach = self.make_gripper_translation(0.01, 0.1, [1.0, 0.0, 0.0])
g.post_grasp_retreat = self.make_gripper_translation(0.1, 0.15, [0.0, -1.0, 1.0])
# Set the first grasp pose to the input pose
g.grasp_pose = initial_pose_stamped
# Pitch angles to try
pitch_vals = [0, 0.1, -0.1, 0.2, -0.2, 0.4, -0.4]
# Yaw angles to try; given the limited dofs of turtlebot_arm, we must calculate the heading
# from arm base to the object to pick (first we must transform its pose to arm base frame)
target_pose_arm_ref = self.tf_listener.transformPose(ARM_BASE_FRAME, initial_pose_stamped)
x = target_pose_arm_ref.pose.position.x
y = target_pose_arm_ref.pose.position.y
self.pick_yaw = atan2(y, x) # check in make_places method why we store the calculated yaw
yaw_vals = [self.pick_yaw]
# A list to hold the grasps
grasps = []
# Generate a grasp for each pitch and yaw angle
for yaw in yaw_vals:
for pitch in pitch_vals:
# Create a quaternion from the Euler angles
q = quaternion_from_euler(0, pitch, yaw)
# Set the grasp pose orientation accordinglytb_return_pose.launch
g.grasp_pose.pose.orientation.x = q[0]
g.grasp_pose.pose.orientation.y = q[1]
g.grasp_pose.pose.orientation.z = q[2]
g.grasp_pose.pose.orientation.w = q[3]
# Set and id for this grasp (simply needs to be unique)
g.id = str(len(grasps))
# Set the allowed touch objects to the input list
g.allowed_touch_objects = allowed_touch_objects
# Don't restrict contact force
g.max_contact_force = 0
# Degrade grasp quality for increasing pitch angles
g.grasp_quality = 1.0 - abs(pitch)
# Append the grasp to the list
grasps.append(deepcopy(g))
# Return the list
return grasps
def pick(robot):
# Construct the grasp
p = PoseStamped()
p.header.frame_id = robot.get_planning_frame()
p.pose.position.x = 0.34
p.pose.position.y = -0.7
p.pose.position.z = 0.5
p.pose.orientation.w = 1.0
g = Grasp()
g.grasp_pose = p
# Construct pre-grasp
g.pre_grasp_approach.direction.vector.x = 1.0
g.pre_grasp_approach.direction.header.frame_id = "r_wrist_roll_link"
g.pre_grasp_approach.min_distance = 0.2
g.pre_grasp_approach.desired_distance = 0.4
# Construct post-grasp
g.post_grasp_retreat.direction.header.frame_id = robot.get_planning_frame()
g.post_grasp_retreat.direction.vector.z = 1.0
g.post_grasp_retreat.min_distance = 0.1
g.post_grasp_retreat.desired_distance = 0.25
g.pre_grasp_posture = open_gripper(g.pre_grasp_posture)
g.grasp_posture = closed_gripper(g.grasp_posture)
grasps = []
grasps.append(g)
group = robot.get_group("right_arm")
group.set_support_surface_name("table")
group.pick("part", grasps)
def publish_poses_grasps(grasps):
rospy.loginfo("Publishing PoseArray on /grasp_pose_from_block_bla for grasp_pose")
grasp_publisher = rospy.Publisher("grasp_pose_from_block_bla", PoseArray)
graspmsg = Grasp()
grasp_PA = PoseArray()
header = Header()
header.frame_id = "base_link"
header.stamp = rospy.Time.now()
grasp_PA.header = header
for graspmsg in grasps:
print graspmsg
print type(graspmsg)
p = Pose(graspmsg.grasp_pose.pose.position, graspmsg.grasp_pose.pose.orientation)
grasp_PA.poses.append(p)
grasp_publisher.publish(grasp_PA)
rospy.loginfo("Press a to continue...")
while True:
choice = raw_input("> ")
if choice == 'a' :
print "Continuing, a pressed"
break
else:
grasp_publisher.publish(grasp_PA)
rospy.sleep(0.1)
def pickup(pickPlaceInterface, frame, graspPose_xyzrpy, objectName, supportName):
moveit_grasp = Grasp()
moveit_grasp.grasp_pose.header.frame_id = frame
moveit_grasp.grasp_pose.pose.orientation = geometry_msgs.msg.Quaternion(
*transformations.quaternion_from_euler(*graspPose_xyzrpy[3:])
)
moveit_grasp.grasp_pose.pose.position.x = graspPose_xyzrpy[0]
moveit_grasp.grasp_pose.pose.position.y = graspPose_xyzrpy[1]
moveit_grasp.grasp_pose.pose.position.z = graspPose_xyzrpy[2]
moveit_grasp.pre_grasp_approach.direction.header.frame_id = frame
moveit_grasp.pre_grasp_approach.direction.vector.x = 1.0
moveit_grasp.pre_grasp_approach.min_distance = 0.04
moveit_grasp.pre_grasp_approach.desired_distance = 0.08
moveit_grasp.post_grasp_retreat.direction.header.frame_id = frame
moveit_grasp.post_grasp_retreat.direction.vector.z = 1.0
moveit_grasp.post_grasp_retreat.min_distance = 0.1
moveit_grasp.post_grasp_retreat.desired_distance = 0.25
openGripper(moveit_grasp.pre_grasp_posture)
closeGripper(moveit_grasp.grasp_posture)
for i in range(10):
res = pickPlaceInterface.pickup(objectName, [moveit_grasp,], support_name=supportName)
# print("{}: {}".format(i, res.error_code.val))
if res.error_code.val == 1:
return True
return False
def get_grasp(self, grasp_pose, id):
grasp = Grasp()
grasp.grasp_pose = grasp_pose
grasp.grasp_pose.header.frame_id = self.frame_id
joint_name = self.gripper + '_gripper_l_finger_joint'
point = JointTrajectoryPoint()
point.positions.append(0.095)
grasp.pre_grasp_posture.joint_names.append(joint_name)
grasp.pre_grasp_posture.points.append(point)
point = JointTrajectoryPoint()
point.positions.append(-0.0125)
grasp.grasp_posture.joint_names.append(joint_name)
grasp.grasp_posture.points.append(point)
grasp.grasp_quality = 1.0
grasp.id = str(id)
grasp.post_place_retreat.desired_distance = 0.3
grasp.post_place_retreat.min_distance = 0.05
grasp.post_place_retreat.direction.header.frame_id = 'world'
grasp.post_place_retreat.direction.vector.z = 1.0
grasp.pre_grasp_approach.desired_distance = 0.3
grasp.pre_grasp_approach.min_distance = 0.05
grasp.pre_grasp_approach.direction.header.frame_id = self.gripper + "_gripper"
grasp.pre_grasp_approach.direction.vector.y = 1.0
grasp.post_grasp_retreat = grasp.post_place_retreat
return grasp
def createGrasp(grasp_pose, allowed_touch_objects=[], pre_grasp_posture=None, grasp_posture=None, pre_grasp_approach=None, post_grasp_retreat=None, id_grasp="grasp_"):
"""Create a grasp for object in grasp_pose, allowing collisions with allowed_touch_objects list,
with pre_grasp_posture and grasp_posture as positions of the hand. Also id name id_grasp."""
grasp = Grasp()
grasp.id = id_grasp
# header = Header()
# header.frame_id = "base_link"
# header.stamp = rospy.Time.now()
# grasp_pose_msg = PoseStamped(header, grasp_pose)
grasp_pose_with_offset = add_offset_reem_hand(grasp_pose)
grasp.grasp_pose = grasp_pose_with_offset
if pre_grasp_posture == None:
grasp.pre_grasp_posture = getPreGraspPosture
else:
grasp.pre_grasp_posture = pre_grasp_posture
if grasp_posture == None:
grasp.grasp_posture = getGraspPosture
else:
grasp.grasp_posture = grasp_posture
grasp.allowed_touch_objects = allowed_touch_objects # ["table", "part"]
if pre_grasp_approach != None:
grasp.pre_grasp_approach = pre_grasp_approach
if post_grasp_retreat != None:
grasp.post_grasp_retreat = post_grasp_retreat
grasp.max_contact_force = 0
#grasp.grasp_quality = 0
return grasp
def make_grasps(self, pose_stamped, mega_angle=False):
# setup defaults of grasp
g = Grasp()
g.pre_grasp_posture = self.make_gripper_posture(GRIPPER_OPEN)
g.grasp_posture = self.make_gripper_posture(GRIPPER_CLOSED)
g.pre_grasp_approach = self.make_gripper_translation(0.1, 0.15)
g.post_grasp_retreat = self.make_gripper_translation(0.1, 0.15, -1.0)
g.grasp_pose = pose_stamped
pitch_vals = [0, 0.2, -0.2, 0.4, -0.4]
if mega_angle:
pitch_vals += [0.3, -0.3, 0.5, -0.5, 0.6, -0.6]
# generate list of grasps
grasps = []
for y in [-1.57, -0.78, 0, 0.78, 1.57]:
for p in pitch_vals:
q = quaternion_from_euler(0, 1.57 - p, y)
g.grasp_pose.pose.orientation.x = q[0]
g.grasp_pose.pose.orientation.y = q[1]
g.grasp_pose.pose.orientation.z = q[2]
g.grasp_pose.pose.orientation.w = q[3]
g.id = str(len(grasps))
g.grasp_quality = 1.0 - abs(p / 2.0)
grasps.append(copy.deepcopy(g))
return grasps
def make_grasps(self, initial_pose_stamped, allowed_touch_objects):
# 初始化抓取姿态对象
g = Grasp()
# 创建夹爪张开、闭合的姿态
g.pre_grasp_posture = self.make_gripper_posture(GRIPPER_OPEN)
g.grasp_posture = self.make_gripper_posture(GRIPPER_CLOSED)
# 设置期望的夹爪靠近、撤离目标的参数
# g.pre_grasp_approach = self.make_gripper_translation(0.01, 0.05, [1.0, 0.0, 0.0])
g.post_grasp_retreat = self.make_gripper_translation(
0.01, 0.06, [0.0, 0.0, 1.0])
g.pre_grasp_approach = self.make_gripper_translation(
0.02, 0.05, self.direction_z)
# 设置抓取姿态
g.grasp_pose = initial_pose_stamped
# 抓取姿态的列表
grasps = []
# 设置抓取的唯一id号
g.id = str(len(grasps))
# 设置允许接触的物体
g.allowed_touch_objects = allowed_touch_objects
# 将本次规划的抓取放入抓取列表中
grasps.append(deepcopy(g))
# 返回抓取列表
return grasps
def _create_grasp(self, x, y, z, roll, pitch, yaw):
grasp = Grasp()
# pre_grasp
grasp.pre_grasp_posture = self._open_gripper()
grasp.pre_grasp_approach.direction.header.frame_id = self.end_effector_link
grasp.pre_grasp_approach.direction.vector.z = 1.0
grasp.pre_grasp_approach.direction.vector.y = 0.0
grasp.pre_grasp_approach.direction.vector.x = 0.0
grasp.pre_grasp_approach.min_distance = 0.05
grasp.pre_grasp_approach.desired_distance = 0.1
# grasp
grasp.grasp_posture = self._close_gripper()
grasp.grasp_pose.pose.position.x = x
grasp.grasp_pose.pose.position.y = y
grasp.grasp_pose.pose.position.z = z
q = quaternion_from_euler(roll, pitch, yaw)
grasp.grasp_pose.pose.orientation.x = q[0]
grasp.grasp_pose.pose.orientation.y = q[1]
grasp.grasp_pose.pose.orientation.z = q[2]
grasp.grasp_pose.pose.orientation.w = q[3]
grasp.grasp_pose.header.frame_id = "m1n6s200_link_base"
# post_grasp
grasp.post_grasp_retreat.direction.header.frame_id = self.end_effector_link
grasp.post_grasp_retreat.direction.vector.z = -1.0
grasp.post_grasp_retreat.direction.vector.x = 0.0
grasp.post_grasp_retreat.direction.vector.y = 0.0
grasp.post_grasp_retreat.min_distance = 0.05
grasp.post_grasp_retreat.desired_distance = 0.25
return [grasp]
def _pick_box(self):
# Construct the grasp
p = PoseStamped()
p.header.frame_id = self.robot.get_planning_frame()
# TODO: Clean this
p.pose.position.x = self.box_x
p.pose.position.y = self.box_y
p.pose.position.z = self.box_z # old value = 0.85
p.pose.orientation.w = 1.0
g = Grasp()
g.grasp_pose = p
# Construct pre-grasp
g.pre_grasp_approach.direction.vector.x = 1.0
g.pre_grasp_approach.direction.header.frame_id = "l_wrist_roll_link"
g.pre_grasp_approach.min_distance = 0.2
g.pre_grasp_approach.desired_distance = 0.4
g.pre_grasp_posture = self._open_gripper(g.pre_grasp_posture)
g.grasp_posture = self._closed_gripper(g.grasp_posture)
grasps = []
grasps.append(g)
self.group.set_support_surface_name("table")
self.group.set_planner_id("RRTkConfigDefault")
print("EXECUTING PICK")
self.group.pick("box", grasps)
rospy.sleep(3)
return True
def make_grasps(self, pose_stamped, mega_angle=False):
# setup defaults for the grasp
g = Grasp()
g.pre_grasp_posture = self.make_gripper_posture(GRIPPER_OPEN)
g.grasp_posture = self.make_gripper_posture(GRIPPER_CLOSED)
g.pre_grasp_approach = self.make_gripper_translation(0.05, 0.1)
g.post_grasp_retreat = self.make_gripper_translation(0.05, 0.1, -1.0)
g.grasp_pose = pose_stamped
pitch_vals = [0,0.4, 0.9, 1.2, 1.57, 1.8, 2.2, 2.8, 3.14]
#pitch_vals = [0]
yaw_vals = [-0.2, -0.1, 0, 0.1, 0.2]
#yaw_vals = [0]
if mega_angle:
pitch_vals += [1.57, -1.57, 0.3, -0.3, 0.5, -0.5, 0.6, -0.6]
# generate list of grasps
grasps = []
#for y in [-1.57, -0.78, 0, 0.78, 1.57]:
for y in yaw_vals:
for p in pitch_vals:
q = quaternion_from_euler(0, 1.57-p, y)
g.grasp_pose.pose.orientation.x = q[0]
g.grasp_pose.pose.orientation.y = q[1]
g.grasp_pose.pose.orientation.z = q[2]
g.grasp_pose.pose.orientation.w = q[3]
g.id = str(len(grasps))
g.allowed_touch_objects = ["part"]
g.max_contact_force = 0
#g.grasp_quality = 1.0 - abs(p/2.0)
grasps.append(copy.deepcopy(g))
return grasps
def make_grasp(arm,
gripper,
eef_grasp_pose,
approach_distance=0.2,
lift_distance=0.2):
"""Initialize the moveit_msgs/Grasp message.
Arguments:
arm {MoveGroupCommander} -- arm group
gripper {MoveGroupCommander} -- gripper group
eef_grasp_pose {PoseStamped} -- pose of the end effector in which it should attempt grasping
Keyword Arguments:
approach_distance {float} -- distance from which to approach the object (default: {0.2})
lift_distance {float} -- distance to take after a grasp has been completed (default: {0.2})
Returns:
Grasp -- message
"""
return Grasp(
id='grasp',
grasp_quality=1.0,
grasp_pose=eef_grasp_pose,
pre_grasp_posture=_make_posture(
gripper.get_named_target_values('open')),
grasp_posture=_make_posture(gripper.get_named_target_values('close')),
pre_grasp_approach=_make_gripper_translation(
arm.get_end_effector_link(), [0, 0, 1], approach_distance),
post_grasp_retreat=_make_gripper_translation(arm.get_planning_frame(),
[0, 0, 1], lift_distance),
post_place_retreat=_make_gripper_translation(
arm.get_end_effector_link(), [0, 0, -1], approach_distance),
max_contact_force=10.0,
allowed_touch_objects=[])
def create_grasp(grasp_pose,
allowed_touch_objects=[],
pre_grasp_posture=None,
grasp_posture=None,
pre_grasp_approach=None,
post_grasp_retreat=None,
id_grasp="grasp_"):
grasp = Grasp()
grasp.id = id_grasp
grasp.grasp_pose = grasp_pose
if pre_grasp_posture is None:
grasp.pre_grasp_posture = get_grasp_posture(True)
else:
grasp.pre_grasp_posture = pre_grasp_posture
if grasp_posture is None:
grasp.grasp_posture = get_grasp_posture(False)
else:
grasp.grasp_posture = grasp_posture
grasp.allowed_touch_objects = allowed_touch_objects
if pre_grasp_approach is not None:
grasp.pre_grasp_approach = pre_grasp_approach
if post_grasp_retreat is not None:
grasp.post_grasp_retreat = post_grasp_retreat
grasp.max_contact_force = 0
return grasp
def __init__(self):
self.scene = PlanningSceneInterface("base_link")
self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True)
self.move_group = MoveGroupInterface("arm", "base_link")
grasp_topic = "fetch_grasp_planner_node/plan"
rospy.loginfo("Waiting for %s..." % grasp_topic)
self.grasp_planner_client = actionlib.SimpleActionClient(
grasp_topic, GraspPlanningAction)
wait = self.grasp_planner_client.wait_for_server(rospy.Duration(5))
if (wait):
print("successfully connected to grasp server")
else:
print("failed to connect to grasp server")
rospy.Subscriber("fetch_fruit_harvest/apple_pose", Pose,
self.apple_pose_callback)
self.pub = rospy.Publisher("fetch_fruit_harvest/grasp_msg",
String,
queue_size=10)
self.object = Object()
self.grasps = Grasp()
self.ready_for_grasp = False
self.pick_place_finished = False
self.first_time_grasp = True
self.scene.addBox("ground", 300, 300, 0.02, 0, 0, 0)
self.tuck()
def pick(move_group):
"""Pick object.
Parameters
----------
Group : moveit_commander.RobotCommander
Moveit_commander move group.
"""
## - BEGIN_SUB_TUTORIAL pick1 - ##
## Create a vector of grasps to be attempted, currently only creating single grasp. ##
# This is essentially useful when using a grasp generator to generate and test multiple grasps.
grasps = [Grasp() for i in range(1)]
## Setting grasp pose ##
# This is the pose of panda_link8. |br|
# From panda_link8 to the palm of the eef the distance is 0.058, the cube starts 0.01 before 5.0 (half of the length
# of the cube). |br|
# Therefore, the position for panda_link8 = 5 - (length of cube/2 - distance b/w panda_link8 and palm of eef - some
# extra padding)
grasps[0].grasp_pose.header.frame_id = "panda_link0"
orientation = quaternion_from_euler(-math.pi / 2, -math.pi / 4,
-math.pi / 2)
grasps[0].grasp_pose.pose.orientation.x = orientation[0]
grasps[0].grasp_pose.pose.orientation.y = orientation[1]
grasps[0].grasp_pose.pose.orientation.z = orientation[2]
grasps[0].grasp_pose.pose.orientation.w = orientation[3]
grasps[0].grasp_pose.pose.position.x = 0.415
grasps[0].grasp_pose.pose.position.y = 0
grasps[0].grasp_pose.pose.position.z = 0.5
## Setting pre-grasp approach ##
# Defined with respect to frame_id
grasps[0].pre_grasp_approach.direction.header.frame_id = "panda_link0"
# Direction is set as positive x axis
grasps[0].pre_grasp_approach.direction.vector.x = 1.0
grasps[0].pre_grasp_approach.min_distance = 0.095
grasps[0].pre_grasp_approach.desired_distance = 0.115
## Setting post-grasp retreat ##
# Defined with respect to frame_id
grasps[0].post_grasp_retreat.direction.header.frame_id = "panda_link0"
# Direction is set as positive z axis
grasps[0].post_grasp_retreat.direction.vector.z = 1.0
grasps[0].post_grasp_retreat.min_distance = 0.1
grasps[0].post_grasp_retreat.desired_distance = 0.25
## Setting posture of eef before grasp ##
openGripper(grasps[0].pre_grasp_posture)
## - END_SUB_TUTORIAL - ##
## - BEGIN_SUB_TUTORIAL pick2 - ##
## Setting posture of eef during grasp ##
closedGripper(grasps[0].grasp_posture)
## Set support surface as table1. ##
move_group.set_support_surface_name("table1")
# Call pick to pick up the object using the grasps given
move_group.pick("object", grasps)
def make_grasps(self, initial_pose_stamped, allowed_touch_objects):
# Initialize the grasp object
g = Grasp()
# Set the pre-grasp and grasp postures appropriately
g.pre_grasp_posture = self.make_gripper_posture(GRIPPER_OPEN)
g.grasp_posture = self.make_gripper_posture(GRIPPER_CLOSED)
# Set the approach and retreat parameters as desired
g.pre_grasp_approach = self.make_gripper_translation(0.01, 0.065, [1.0, 0.0, 0.0])
g.post_grasp_retreat = self.make_gripper_translation(0.01, 0.1, [0.0, 0.0, 1.0])
# Set the first grasp pose to the input pose
g.grasp_pose = initial_pose_stamped
# Pitch angles to try
pitch_vals = [0, 0.1, -0.1, 0.2, -0.2, 0.3, -0.3]
# Yaw angles to try
yaw_vals = [-1.57]
# A list to hold the grasps
grasps = []
# Generate a grasp for each pitch and yaw angle
for y in yaw_vals:
# for p in pitch_vals:
for i in range(0,3):
p = (float)(i / 1000.0)
print("pitch", p)
# Create a quaternion from the Euler angles
q = quaternion_from_euler(-1.57, p, y)
# q = quaternion_from_euler(-1.5708, p, y)
# Set the grasp pose orientation accordingly
g.grasp_pose.pose.orientation.x = q[0]
g.grasp_pose.pose.orientation.y = q[1]
g.grasp_pose.pose.orientation.z = q[2]
g.grasp_pose.pose.orientation.w = q[3]
# Set and id for this grasp (simply needs to be unique)
g.id = str(len(grasps))
# Set the allowed touch objects to the input list
g.allowed_touch_objects = allowed_touch_objects
# Don't restrict contact force
g.max_contact_force = 0
# Degrade grasp quality for increasing pitch angles
g.grasp_quality = 1.0 - abs(p)
# Append the grasp to the list
grasps.append(deepcopy(g))
# print("grasp_pose_pose", g)
# Return the list
return grasps
def generate_grasp_msgs(self, grasps):
formatted_grasps = []
for i in range(0, len(grasps)): #dimitris, take out self. !
g = Grasp()
g.id = "dupa"
gp = PoseStamped()
gp.header.frame_id = "camera_color_optical_frame"
quat = self.trans_matrix_to_quaternion(grasps[i])
# Move grasp back for given offset
gp.pose.position.x = grasps[
i].surface.x + self.grasp_offset * grasps[i].approach.x
gp.pose.position.y = grasps[
i].surface.y + self.grasp_offset * grasps[i].approach.y
gp.pose.position.z = grasps[
i].surface.z + self.grasp_offset * grasps[i].approach.z
gp.pose.orientation.x = float(quat.elements[1])
gp.pose.orientation.y = float(quat.elements[2])
gp.pose.orientation.z = float(quat.elements[3])
gp.pose.orientation.w = float(quat.elements[0])
g.grasp_pose = gp
g.pre_grasp_approach.direction.header.frame_id = "hand_link"
g.pre_grasp_approach.direction.vector.x = 0.0
g.pre_grasp_approach.direction.vector.y = 0.0
g.pre_grasp_approach.direction.vector.z = 1.0
g.pre_grasp_approach.min_distance = 0.05
g.pre_grasp_approach.desired_distance = 0.1
# g.pre_grasp_posture.joint_names = ["gripper_right_finger_joint", "gripper_left_finger_joint"]
g.pre_grasp_posture.joint_names = ["joint_6"]
g.pre_grasp_posture.header.frame_id = "hand_link"
pos = JointTrajectoryPoint()
pos.positions.append(0)
# pos.positions.append(0.1337)
g.pre_grasp_posture.points.append(pos)
# g.grasp_posture.joint_names = ["gripper_right_finger_joint", "gripper_left_finger_joint"]
# g.grasp_posture.joint_names = ["joint_6"]
# pos = JointTrajectoryPoint()
# pos.positions.append(0.0)
# pos.positions.append(0.0)
# pos.accelerations.append(0.0)
# pos.accelerations.append(0.0)
# g.grasp_posture.points.append(pos)
# g.grasp_posture.header.frame_id = "hand_link"
g.allowed_touch_objects = ["<octomap>", "obj"]
g.max_contact_force = 0.0
#g.grasp_quality = grasps[0].score.data perche 0 e non i????
g.grasp_quality = grasps[i].score.data
formatted_grasps.append(g)
return formatted_grasps
def load_grasps(self, filename):
f = open(filename)
args = yaml.load(f)
grasps = []
for arg in args:
grasp = Grasp()
arg["id"] = str(arg["id"])
genpy.message.fill_message_args(grasp, arg)
grasps.append(grasp)
return grasps
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
def make_grasps(initial_pose_stamped, allowed_touch_objects, grasp_opening=[0]): # INicio el grasp g = Grasp() # Asigno las posiciones de pre-grasp y grasp postures; g.pre_grasp_posture = make_gripper_posture(GRIPPER_OPEN) g.grasp_posture = make_gripper_posture(grasp_opening) # Defino los parametros de aproximación y alejar deseados g.pre_grasp_approach = make_gripper_translation(0.01, 0.1, [1.0, 0.0, 0.0]) g.post_grasp_retreat = make_gripper_translation(0.1, 0.15, [0.0, -1.0, 1.0]) # Defino la primera grasp pose g.grasp_pose = initial_pose_stamped # Pitch angulos a probar: POR AHORA SOLO UNO pitch_vals = [0] # Yaw angles to try: POR AHORA SOLO UNO yaw_vals = [0] # A list to hold the grasps grasps = [] # Genero un grasp para cada angulo pitch y yaw for y in yaw_vals: for p in pitch_vals: # Creo un quaternion de Euler angles, con un roll de pi (cenital) q = quaternion_from_euler(pi, p, y) # asigno grasppose acorde al quaternio g.grasp_pose.pose.orientation.x = q[0] g.grasp_pose.pose.orientation.y = q[1] g.grasp_pose.pose.orientation.z = q[2] g.grasp_pose.pose.orientation.w = q[3] # EStablezco la id de este grasp g.id = str(len(grasps)) # Defino los objetos que se pueden tocar g.allowed_touch_objects = allowed_touch_objects # no elimino la fuerza de contacto g.max_contact_force = 0 g.grasp_quality = 1.0 - abs(p) # Adjunto la lista de grasp grasps.append(deepcopy(g)) #Me devuelve la lista return grasps
def pick(self):
##-----------GRAPS---------------##
#set all grasp poses
#first create an array where all graps are going to be saved
grasps = []
#graps 1, box
#create object grasp
g = Grasp()
g.id="box_grasp"
#GRASP POSE: position of object in end efector seen from world
grasp_pose = PoseStamped()
grasp_pose.header.frame_id = "panda_link0"
grasp_pose.pose.position.x = 0.4 #(box position- (half box lenght- distance distance b/w panda_link8 and palm of eef (0.058) - some extra padding)))
grasp_pose.pose.position.y = 0
grasp_pose.pose.position.z = 0.5
#orientation in quaternion
#orientation in quaternion
q = quaternion_from_euler(-pi/2,-pi/4,-pi/2)
grasp_pose.pose.orientation.x = q[0]
grasp_pose.pose.orientation.y = q[1]
grasp_pose.pose.orientation.z = q[2]
grasp_pose.pose.orientation.w = q[3]
g.grasp_pose = grasp_pose #set grasp pose
#pre-grasp approach
g.pre_grasp_approach.direction.header.frame_id = "panda_link0"
g.pre_grasp_approach.direction.vector.x = 1.0
g.pre_grasp_approach.min_distance = 0.095
g.pre_grasp_approach.desired_distance = 0.115
self.openGripperPose(g.pre_grasp_posture) #posture before grasp
#posture during grasp
self.closeGripperPose(g.grasp_posture,0.02)
#post-grasp retreat
g.post_grasp_retreat.direction.header.frame_id = "panda_link0"
g.post_grasp_retreat.direction.vector.z = 1.0
g.post_grasp_retreat.min_distance = 0.1
g.post_grasp_retreat.desired_distance= 0.25
#surface in contact with object
#g.allowed_touch_objects = ["table1","box","table2"]
self.move_group_arm.set_support_surface_name("table1")
# append the grasp to the list of grasps
grasps.append(g)
rospy.sleep(2)
# pick the object
self.move_group_arm.pick("box", grasps)
def make_grasps(self, initial_pose_stamped, allowed_touch_objects):
# 初始化抓取姿态对象
g = Grasp()
# 创建夹爪张开、闭合的姿态
g.pre_grasp_posture = self.make_gripper_posture(GRIPPER_OPEN)
g.grasp_posture = self.make_gripper_posture(GRIPPER_CLOSED)
# 设置期望的夹爪靠近、撤离目标的参数
g.pre_grasp_approach = self.make_gripper_translation(
0.05, 0.1, [0.0, 0.0, -1.0])
g.post_grasp_retreat = self.make_gripper_translation(
0.05, 0.1, [0.0, 0.0, 1.0])
# 设置抓取姿态
g.grasp_pose = initial_pose_stamped
rot = (initial_pose_stamped.pose.orientation.x,
initial_pose_stamped.pose.orientation.y,
initial_pose_stamped.pose.orientation.z,
initial_pose_stamped.pose.orientation.w)
# 需要尝试改变姿态的数据列表
yaw_vals = [0, 0.1, -0.1, 0.2, -0.2, 0.3, -0.3]
p = euler_from_quaternion(rot)
# 抓取姿态的列表
grasps = []
# 改变姿态，生成抓取动作
for Y in yaw_vals:
g.grasp_pose.pose.position.z += 0.18
# 欧拉角到四元数的转换
q = quaternion_from_euler(pi, 0, p[2] + Y)
# 设置抓取的姿态
g.grasp_pose.pose.orientation.x = q[0]
g.grasp_pose.pose.orientation.y = q[1]
g.grasp_pose.pose.orientation.z = q[2]
g.grasp_pose.pose.orientation.w = q[3]
# 设置抓取的唯一id号
g.id = str(len(grasps))
# 设置允许接触的物体
g.allowed_touch_objects = allowed_touch_objects
# 将本次规划的抓取放入抓取列表中
grasps.append(deepcopy(g))
# 返回抓取列表
return grasps
def handle_graps(req):
#this will create an element of type grasp
object_name=req.object_name
object_pose=Pose()
object_pose=Query_object_Pose(object_name)
object_dimensions=[0,0,0]
#get object dimensions
if object_name=="block":
object_dimensions=[0.045, 0.045, 0.2]
print object_dimensions
#graps 1
#create object grasp
g = Grasp()
g.id=object_name
#GRASP POSE: position of object in end efector seen from world
grasp_pose = PoseStamped()
grasp_pose.header.frame_id = "panda_link0"
grasp_pose.pose.position.x = object_pose.position.x -(object_dimensions[0]/2) -0.058-0.02 #(box position- (half box lenght- distance distance b/w panda_link8 and palm of eef (0.058) - some extra padding)))
grasp_pose.pose.position.y = object_pose.position.y
grasp_pose.pose.position.z = object_pose.position.z
#orientation in quaternion
q = quaternion_from_euler(-pi/2,-pi/4,-pi/2)
grasp_pose.pose.orientation.x = q[0]
grasp_pose.pose.orientation.y = q[1]
grasp_pose.pose.orientation.z = q[2]
grasp_pose.pose.orientation.w = q[3]
g.grasp_pose = grasp_pose #set grasp pose
#pre-grasp approach
g.pre_grasp_approach.direction.header.frame_id = "panda_link0"
g.pre_grasp_approach.direction.vector.x = 1.0
g.pre_grasp_approach.min_distance = 0.095
g.pre_grasp_approach.desired_distance = 0.115
openGripperPose(g.pre_grasp_posture) #posture before grasp
#posture during grasp
closeGripperPose(g.grasp_posture,object_dimensions[0])
#post-grasp retreat
g.post_grasp_retreat.direction.header.frame_id = "panda_link0"
g.post_grasp_retreat.direction.vector.z = 1.0
g.post_grasp_retreat.min_distance = 0.1
g.post_grasp_retreat.desired_distance= 0.25
#print "Returning grasp [%s , %s , %s]"%(g.grasp_pose.pose.position.x, g.grasp_pose.pose.position.y, g.grasp_pose.pose.position.z)
return g
def generate_grasps(self, pose, width):
grasps = []
now = rospy.Time.now()
for angle in numpy.arange(0.0, numpy.deg2rad(360.0),
numpy.deg2rad(90.0)):
# Create place location:
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)
q = quaternion_from_euler(0.0, numpy.deg2rad(90.0), angle)
grasp.grasp_pose.pose.orientation = Quaternion(*q)
# 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
grasp.max_contact_force = 100
grasp.pre_grasp_posture.joint_names.append("gripper_finger1_joint")
traj = JointTrajectoryPoint()
traj.positions.append(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()
traj.positions.append(0.55)
#traj.effort.append(100)
traj.time_from_start = rospy.Duration.from_sec(1.0)
grasp.grasp_posture.points.append(traj)
grasps.append(grasp)
return grasps
def createGrasp(grasp_pose):
grasp = Grasp()
grasp.id = "test"
grasp.grasp_pose = grasp_pose
# grasp.pre_grasp_approach.direction.header.frame_id = "base_link"
# grasp.pre_grasp_approach.direction.header.stamp = rospy.Time.now()
# grasp.pre_grasp_approach.direction.vector.x = 1.0
# grasp.pre_grasp_approach.direction.vector.y = 0.0
# grasp.pre_grasp_approach.direction.vector.z = 0.0
# grasp.pre_grasp_approach.desired_distance = 0.05
# grasp.pre_grasp_approach.min_distance = 0.01
grasp.pre_grasp_posture.header.frame_id = "base_link" # what link do i need here?
grasp.pre_grasp_posture.header.stamp = rospy.Time.now()
grasp.pre_grasp_posture.joint_names = [
"hand_right_thumb_joint", "hand_right_index_joint",
"hand_right_middle_joint"
]
pos = JointTrajectoryPoint() # pre-grasp with thumb down and fingers open
pos.positions.append(2.0)
pos.positions.append(0.0)
pos.positions.append(0.0)
grasp.pre_grasp_posture.points.append(pos)
grasp.grasp_posture.header.frame_id = "base_link"
grasp.grasp_posture.header.stamp = rospy.Time.now()
grasp.grasp_posture.joint_names = [
"hand_right_thumb_joint", "hand_right_index_joint",
"hand_right_middle_joint"
]
pos = JointTrajectoryPoint() # grasp with all closed
pos.positions.append(2.0)
pos.positions.append(2.0)
pos.positions.append(2.0)
grasp.grasp_posture.points.append(pos)
# grasp.post_grasp_retreat.direction.header.frame_id = "base_link"
# grasp.post_grasp_retreat.direction.header.stamp = rospy.Time.now()
# grasp.post_grasp_retreat.direction.vector.x = 0.0
# grasp.post_grasp_retreat.direction.vector.y = 0.0
# grasp.post_grasp_retreat.direction.vector.z = 1.0
# grasp.post_grasp_retreat.desired_distance = 0.1
# grasp.post_grasp_retreat.min_distance = 0.01
# grasp.allowed_touch_objects = ["table", "part"]
grasp.max_contact_force = 0
return grasp
def load_grasps(resourcepath):
"""
load and fill a list of Grasp.msg from a yaml file
"""
grasp_yaml = yaml.load(open(resourcepath + 'grasps/grasps.yaml'))
grasps = {}
for g in grasp_yaml:
grasp = Grasp()
genpy.message.fill_message_args(grasp, g)
if grasp.id is None or grasp.id == "":
raise Exception("Grasp has no id")
else:
grasps[grasp.id] = grasp
return grasps
def publish_grasps_as_poses(grasps):
rospy.loginfo(
"Publishing PoseArray on /grasp_pose_from_block_bla for grasp_pose")
graspmsg = Grasp()
grasp_PA = PoseArray()
header = Header()
header.frame_id = "base_link"
header.stamp = rospy.Time.now()
grasp_PA.header = header
for graspmsg in grasps:
p = Pose(graspmsg.grasp_pose.pose.position,
graspmsg.grasp_pose.pose.orientation)
grasp_PA.poses.append(p)
grasp_publisher.publish(grasp_PA)
rospy.loginfo('Published ' + str(len(grasp_PA.poses)) + ' poses')
rospy.sleep(2)
