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 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 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):
# 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:
# Create a quaternion from the Euler angles
# q = quaternion_from_euler(0, 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]
q = quaternion_from_euler(0, 0, -1.57079633)
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))
# Return the list
return 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 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 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_GRASP)
# Set the approach and retreat parameters as desired
#g.pre_grasp_approach = self.make_gripper_translation(0.01, 0.1, [1.0, 1.0, -1.0])
#g.post_grasp_retreat = self.make_gripper_translation(0.1, 0.15, [-1.0, -1.0, 1.0])
# Set the first grasp pose to the input pose
g.grasp_pose = initial_pose_stamped
ideal_roll = 0
ideal_pitch = 0
ideal_yaw = -1.57
# A list to hold the grasps
grasps = []
q = quaternion_from_euler(ideal_roll, ideal_pitch, ideal_yaw)
# 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(ideal_pitch)
# Append the grasp to the list
grasps.append(deepcopy(g))
# Return the list
return grasps
def mk_grasp(joints, pre_joints=None, fix_j0=False):
"""
Generate a moveit_msgs/Grasp from a set of joint angles given as a dict
of joint_name -> position.
"""
if pre_joints is None:
pre_joints = {}
sr_joint_names = _sr_joint_names
if fix_j0:
_fix_j0(joints)
_fix_j0(pre_joints)
sr_joint_names = _sr_joint_names_j0
now = rospy.Time.now()
grasp = Grasp()
grasp.grasp_quality = 0.001
grasp.grasp_pose.header.frame_id = "forearm"
grasp.grasp_pose.header.stamp = now
grasp.grasp_pose.pose.position.x = 0.01
grasp.grasp_pose.pose.position.y = -0.045
grasp.grasp_pose.pose.position.z = 0.321
grasp.grasp_pose.pose.orientation.x = 0
grasp.grasp_pose.pose.orientation.y = 0
grasp.grasp_pose.pose.orientation.z = 0
grasp.grasp_pose.pose.orientation.w = 0
# pre-grasp (just zero all for now)
grasp.pre_grasp_posture.header.stamp = now
grasp.pre_grasp_posture.joint_names = sr_joint_names
jtp = JointTrajectoryPoint()
for jname in sr_joint_names:
if jname in pre_joints:
jtp.positions.append(pre_joints[jname])
else:
jtp.positions.append(0.0)
grasp.pre_grasp_posture.points.append(jtp)
# grasp
grasp.grasp_posture.header.stamp = now
grasp.grasp_posture.joint_names = sr_joint_names
jtp = JointTrajectoryPoint()
for jname in sr_joint_names:
if jname in joints:
jtp.positions.append(joints[jname])
else:
jtp.positions.append(0.0)
grasp.grasp_posture.points.append(jtp)
return grasp
def make_grasps(self,
initial_pose_stamped,
allowed_touch_objects,
pre,
post,
set_rpy=0):
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(
pre[0], pre[1], pre[2])
g.post_grasp_retreat = self.make_gripper_translation(
post[0], post[1], post[2])
g.grasp_pose = initial_pose_stamped
roll_vals = [1.57]
yaw_vals = [0, 0.2, -0.2, 0.4, -0.4, 0.6, -0.6]
pitch_vals = [-1.57, -1.47, -1.67, -1.37, -1.77]
z_vals = [0]
grasps = []
for y in yaw_vals:
for p in pitch_vals:
for z in z_vals:
for r in roll_vals:
if set_rpy:
q = quaternion_from_euler(r, 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.grasp_pose.pose.position.z = initial_pose_stamped.pose.position.z + z
g.id = str(len(grasps))
g.allowed_touch_objects = allowed_touch_objects
g.max_contact_force = 0
g.grasp_quality = 1.0 - abs(p)
grasps.append(deepcopy(g))
return grasps
def make_grasps(self, initial_pose, allow_touch_objects):
# initialise a grasp object
g = Grasp()
g.pre_grasp_posture = self.make_grab_posture(self.open_joint_values)
g.grasp_posture = self.make_grab_posture(self.closed_joint_values)
g.pre_grasp_approach = self.make_grab_translation(0.01, 0.1, [0.0, 0.0, 1.0])
g.post_grasp_retreat = self.make_grab_translation(0.1, 0.15, [0.0, 0.0, 1.0])
g.grasp_pose = initial_pose
# pitch_vals = [0, 0.1, -0.1, 0.2, -0.2, 0.4, -0.4]
#
# target_pose_arm_ref = self.tf_buffer.transform(initial_pose,'hand_iiwa_link_0')
# x = target_pose_arm_ref.pose.position.x
# y = target_pose_arm_ref.pose.position.y
# yaw_vals = [math.atan2(y,x) + inc for inc in [0, 0.1, -0.1]]
#
grasps = []
#
# for yaw in yaw_vals:
# for pitch in pitch_vals:
# q = quaternion_from_euler(0,pitch,yaw)
#
# 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 = allow_touch_objects
g.max_contact_force = 0
g.grasp_quality = 1.0 #- abs(pitch)
grasps.append(g)
return grasps
def generate_grasp_msgs(self, selected_grasps):
self.grasps = []
formatted_grasps = []
self.grasps_cartesian = []
formatted_grasps_cartesian = []
tot_grasps = len(selected_grasps)
cont = 0
filtered_orientation = 0
for i in range(0, len(selected_grasps)):
z_axis_unit = (0, 0, -1)
ap_axis = (selected_grasps[i].approach.x, selected_grasps[i].approach.y, selected_grasps[i].approach.z)
angle = numpy.dot(z_axis_unit, ap_axis)
if (angle <= 0):
# filter it out, because grasp coming from below the ground
filtered_orientation += 1
print(repr(filtered_orientation) + " Grasp filtered because coming from underneath the ground")
continue
g = Grasp()
g.id = "dupa"
gp = PoseStamped()
gp.header.frame_id = "summit_xl_base_footprint"
org_q = self.trans_matrix_to_quaternion(selected_grasps[i])
quat = org_q
gp.pose.position.x = selected_grasps[i].surface.x + self.grasp_offset * selected_grasps[i].approach.x
gp.pose.position.y = selected_grasps[i].surface.y + self.grasp_offset * selected_grasps[i].approach.y
gp.pose.position.z = selected_grasps[i].surface.z + self.grasp_offset * selected_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 = "arm_ee_link"
g.pre_grasp_approach.direction.vector.x = 1.0
g.pre_grasp_approach.min_distance = 0.06
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 = ["arm_tool0"]
# g.pre_grasp_posture.header.frame_id = "arm_wrist_3_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.allowed_touch_objects = ["obj"]
# g.max_contact_force = 0.0
g.grasp_quality = selected_grasps[0].score.data
formatted_grasps.append(g)
#Added code lines for cartesian pick
gp_cartesian = PoseStamped()
gp_cartesian.header.frame_id = "summit_xl_base_footprint"
gp_cartesian.pose.position.x = selected_grasps[i].surface.x + self.grasp_offset_cartesian * selected_grasps[i].approach.x
gp_cartesian.pose.position.y = selected_grasps[i].surface.y + self.grasp_offset_cartesian * selected_grasps[i].approach.y
gp_cartesian.pose.position.z = selected_grasps[i].surface.z + self.grasp_offset_cartesian * selected_grasps[i].approach.z
gp_cartesian.pose.orientation.x = float(quat.elements[1])
gp_cartesian.pose.orientation.y = float(quat.elements[2])
gp_cartesian.pose.orientation.z = float(quat.elements[3])
gp_cartesian.pose.orientation.w = float(quat.elements[0])
g_cartesian = Grasp ()
g_cartesian.id = "cart"
g_cartesian.grasp_pose = gp_cartesian
g_cartesian.allowed_touch_objects = ["obj"]
formatted_grasps_cartesian.append(g_cartesian)
# print(repr(cont) + " grasps out of " + repr(tot_grasps) + " removed because of no IK_SOLUTION error")
# sort grasps using z (get higher grasps first)
formatted_grasps.sort(key=lambda grasp: grasp.grasp_pose.pose.position.z, reverse=True)
formatted_grasps_cartesian.sort(key=lambda grasp: grasp.grasp_pose.pose.position.z, reverse=True)
return formatted_grasps, formatted_grasps_cartesian
pos_y = item_translation[1] - pos
(dx, dy, dz,
dyaw) = onine_arm.get_valid_pose(pos_x, pos_y, aim_z, 0)
g.grasp_pose.pose.position.x = dx
g.grasp_pose.pose.position.y = dy
g.grasp_pose.pose.position.z = dz - h
q = quaternion_from_euler(0.0, -p, dyaw)
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
g.allowed_touch_objects = ["target"]
grasps.append(copy.deepcopy(g))
debugging_pose.poses.append(
copy.deepcopy(
Pose(g.grasp_pose.pose.position,
g.grasp_pose.pose.orientation)))
debugging_pose.header.frame_id = robot.get_planning_frame()
debugging_pose.header.stamp = rospy.Time.now()
debugging_pose_pub.publish(debugging_pose)
rospy.sleep(10)
result = None
n_attempts = 0
max_pick_attempts = 10
def grasp_array(cls, msg, obj):
# for me...
for i in range(msg['_length_grasp']):
gsp = Grasp()
# direct descendants of grasp:
gsp.id = msg["%s_id" % i]
gsp.grasp_quality = msg["%s_grasp_quality" % i]
gsp.max_contact_force = msg["%s_max_contact_force" % i]
gsp.allowed_touch_objects = msg["%s_allowed_touch_objects" % i]
# descendants of pre_grasp_posture:
gsp.pre_grasp_posture.header = \
decode.header(msg, gsp.pre_grasp_posture.header, "%s_pre" % i)
gsp.pre_grasp_posture.joint_names = msg['%s_prejoint_names' % i]
for j in range(msg['%s_length_points1' % i]):
jtp1 = JointTrajectoryPoint()
jtp1 = decode.grasp_points(msg, jtp1, "%s_%s_pre" % (i, j))
gsp.pre_grasp_posture.points.append(jtp1)
# descendants of grasp_posture:
gsp.grasp_posture.header = \
decode.header(msg, gsp.grasp_posture.header, "%s_posture" % i)
gsp.grasp_posture.joint_names = msg["%s_joint_names" % i]
for j in range(msg['%s_length_points2' % i]):
jtp2 = JointTrajectoryPoint()
jtp2 = decode.grasp_points(msg, jtp2, "%s_%s_posture" % (i, j))
gsp.grasp_posture.points.append(jtp2)
# descendants of grasp_pose:
gsp.grasp_pose.header = \
decode.header(msg, gsp.grasp_pose.header, "%s_pose" % i)
gsp.grasp_pose.pose.position = \
decode.position(msg, gsp.grasp_pose.pose.position,
"%s_pose" % i)
gsp.grasp_pose.pose.orientation = \
decode.orientation(msg, gsp.grasp_pose.pose.orientation,
"%s_pose" % i)
# descendants of pre_grasp_approach:
gsp.pre_grasp_approach.direction.header = \
decode.header(msg, gsp.pre_grasp_approach.direction.header,
"%s_app" % i)
gsp.pre_grasp_approach.direction.vector = \
decode.vector(msg, gsp.pre_grasp_approach.direction.vector,
"%s_app" % i)
gsp.pre_grasp_approach.desired_distance = \
msg['%s_appdesired_distance' % i]
gsp.pre_grasp_approach.min_distance = msg['%s_appmin_distance' % i]
# descendants of post_grasp_retreat:
gsp.post_grasp_retreat.direction.header = \
decode.header(msg, gsp.post_grasp_retreat.direction.header,
"%s_ret" % i)
gsp.post_grasp_retreat.direction.vector = \
decode.vector(msg, gsp.post_grasp_retreat.direction.vector,
"%s_ret" % i)
gsp.post_grasp_retreat.desired_distance = \
msg['%s_retdesired_distance' % i]
gsp.post_grasp_retreat.min_distance = msg['%s_retmin_distance' % i]
# descendants of post_place_retreat
gsp.post_place_retreat.direction.header = \
decode.header(msg, gsp.post_place_retreat.direction.header,
'%s_place' % i)
gsp.post_place_retreat.direction.vector = \
decode.vector(msg, gsp.post_place_retreat.direction.vector,
'%s_place' % i)
gsp.post_place_retreat.desired_distance = \
msg['%s_pldesired_distance' % i]
gsp.post_place_retreat.min_distance = msg['%s_plmin_distance' % i]
# append Grasp to GraspArray:
obj.grasps.append(gsp)
return(obj)
the_grasp.grasp_pose.pose.orientation.z = -0.00061982980 #q[2]
the_grasp.grasp_pose.pose.orientation.w = 0.92954083826657420 #q[3]
# the arm movement direction to grasp the object
the_grasp.pre_grasp_approach.direction.header.frame_id = 'base_link'
the_grasp.pre_grasp_approach.direction.vector.x = -1.00
the_grasp.pre_grasp_approach.min_distance = 0.01
the_grasp.pre_grasp_approach.desired_distance = 0.01
# the arm movement direction after the grasp
the_grasp.post_grasp_retreat.direction.header.frame_id = 'base_link'
the_grasp.post_grasp_retreat.direction.vector.x = 1.00
the_grasp.post_grasp_retreat.min_distance = 0.01
the_grasp.post_grasp_retreat.desired_distance = 0.01
the_grasp.grasp_quality = 0.8
# Pickup is failing. Probabily due to:
# https://github.com/ros-planning/moveit_ros/issues/577
# https://groups.google.com/forum/#!topic/moveit-users/-Eie-wLDbu0
# -----------------------------
# Open gripper
#------------------------------
robot.get_current_state()
robot.pincher_arm.set_start_state(RobotState())
robot.pincher_arm.pick('box', the_grasp)
print('pick')
print(robot.pincher_arm.get_goal_orientation_tolerance())
def make_grasps(self, initial_pose_stamped, allowed_touch_objects):
# Initialize the grasp object
g = Grasp()
#g.grasp_pose = initial_pose_stamped
#q0 = g.grasp_pose.pose.orientation.w
#q1 = g.grasp_pose.pose.orientation.x
#q2 = g.grasp_pose.pose.orientation.y
#q3 = g.grasp_pose.pose.orientation.z
#eulerR = atan2(2*(q0*q1+q2*q3),1-2*(q1*q1+q2*q2))
#eulerP = asin(2*(q0*q2-q1*q3))
#eulerY = atan2(2*(q0*q3+q1*q2),1-2*(q2*q2+q3*q3))
#print(eulerR)
#print(eulerY)
#print(eulerP)
# 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.1, [1.0, 0.0, 0.0])
g.post_grasp_retreat = self.make_gripper_translation(
0.1, 0.15, [0.0, 0, 1.7])
# 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, 0.5, 0.4, 0.6]
# Yaw angles to try
yaw_vals = [0]
# Roll angles to try
roll_vals = [-3.14, 0]
# A list to hold the grasps
grasps = []
# Generate a grasp for each pitch and yaw angle\
for r in roll_vals:
for y in yaw_vals:
for p in pitch_vals:
# Create a quaternion from the Euler angles
q = quaternion_from_euler(r, 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))
# Return the list
return grasps
def generate_grasp_msgs(self, selected_grasps):
self.grasps = []
formatted_grasps = []
self.grasps_cartesian = []
formatted_grasps_cartesian = []
tot_grasps = len(selected_grasps)
cont = 0
filtered_orientation = 0
for i in range(0, len(selected_grasps)):
z_axis_unit = (0, 0, -1)
ap_axis = (selected_grasps[i].approach.x,
selected_grasps[i].approach.y,
selected_grasps[i].approach.z)
angle = numpy.dot(z_axis_unit, ap_axis)
if (angle >= 0):
# filter it out, because grasp coming from below the ground
filtered_orientation += 1
perror(
repr(filtered_orientation) +
" Grasp filtered because coming from underneath the ground"
)
continue
tf_listener_.waitForTransform('/arm_camera_depth_optical_frame',
'/summit_xl_base_footprint',
rospy.Time(), rospy.Duration(2.0))
g = Grasp()
g.id = "dupa"
gp = PoseStamped()
gp.header.frame_id = "arm_camera_depth_optical_frame"
org_q = self.trans_matrix_to_quaternion(selected_grasps[i])
quat = org_q
gp.pose.position.x = selected_grasps[
i].surface.x + self.grasp_offset * selected_grasps[i].approach.x
gp.pose.position.y = selected_grasps[
i].surface.y + self.grasp_offset * selected_grasps[i].approach.y
gp.pose.position.z = selected_grasps[
i].surface.z + self.grasp_offset * selected_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])
translated_pose = tf_listener_.transformPose(
"summit_xl_base_footprint", gp)
g.grasp_pose = translated_pose #gp
g.pre_grasp_approach.direction.header.frame_id = "arm_ee_link"
g.pre_grasp_approach.direction.vector.x = 1.0
g.pre_grasp_approach.min_distance = 0.06
g.pre_grasp_approach.desired_distance = 0.1
#g.allowed_touch_objects = ["<octomap>", "obj"]
g.allowed_touch_objects = ["obj"]
#g.max_contact_force = 0.0
g.grasp_quality = selected_grasps[0].score.data
formatted_grasps.append(g)
# Add config for cartesian pick
gp_cartesian = PoseStamped()
gp_cartesian.header.frame_id = "arm_camera_depth_optical_frame"
gp_cartesian.pose.position.x = selected_grasps[
i].surface.x + self.grasp_offset_cartesian * selected_grasps[
i].approach.x
gp_cartesian.pose.position.y = selected_grasps[
i].surface.y + self.grasp_offset_cartesian * selected_grasps[
i].approach.y
gp_cartesian.pose.position.z = selected_grasps[
i].surface.z + self.grasp_offset_cartesian * selected_grasps[
i].approach.z
gp_cartesian.pose.orientation.x = float(quat.elements[1])
gp_cartesian.pose.orientation.y = float(quat.elements[2])
gp_cartesian.pose.orientation.z = float(quat.elements[3])
gp_cartesian.pose.orientation.w = float(quat.elements[0])
translated_pose = tf_listener_.transformPose(
"summit_xl_base_footprint", gp_cartesian)
g_cartesian = Grasp()
g_cartesian.id = "cart"
g_cartesian.grasp_pose = translated_pose
g_cartesian.allowed_touch_objects = ["obj"]
formatted_grasps_cartesian.append(g_cartesian)
# Sort grasps using z (get higher grasps first)
formatted_grasps.sort(
key=lambda grasp: grasp.grasp_pose.pose.position.z, reverse=True)
formatted_grasps_cartesian.sort(
key=lambda grasp: grasp.grasp_pose.pose.position.z, reverse=True)
return formatted_grasps, formatted_grasps_cartesian
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.allowed_touch_objects = ["<octomap>", "obj"]
g.pre_grasp_posture.joint_names = ["joint_6"]
g.pre_grasp_posture.header.frame_id = "hand_link"
pos = JointTrajectoryPoint()
pos.positions.append(0)
g.pre_grasp_posture.points.append(pos)
g.max_contact_force = 0.0
g.grasp_quality = 0
#Opena gripper, execute pickup planning and close gripper
#pevent("Pick sequence started!")
n.change_tool(TOOL_GRIPPER_3_ID)
n.open_gripper(TOOL_GRIPPER_3_ID, 200)
print("[GRIPPER 3 OPENED]")
if mark_pose:
show_grasp_pose(marker_publisher, g.grasp_pose.pose)
rospy.sleep(1)
if verbose:
#pevent("Executing grasp: ")
pprint(g.grasp_pose.pose)
def create_grasp(self, pose, grasp_id):
"""
:type pose: Pose
pose of the gripper for the grasp
:type grasp_id: str
name for the grasp
:rtype: Grasp
"""
g = Grasp()
g.id = grasp_id
pre_grasp_posture = JointTrajectory()
pre_grasp_posture.header.frame_id = self._grasp_postures_frame_id
pre_grasp_posture.joint_names = [
name for name in self._gripper_joint_names.split()]
jtpoint = JointTrajectoryPoint()
jtpoint.positions = [
float(pos) for pos in self._gripper_pre_grasp_positions.split()]
jtpoint.time_from_start = rospy.Duration(self._time_pre_grasp_posture)
pre_grasp_posture.points.append(jtpoint)
grasp_posture = copy.deepcopy(pre_grasp_posture)
grasp_posture.points[0].time_from_start = rospy.Duration(
self._time_pre_grasp_posture + self._time_grasp_posture)
jtpoint2 = JointTrajectoryPoint()
jtpoint2.positions = [
float(pos) for pos in self._gripper_grasp_positions.split()]
jtpoint2.time_from_start = rospy.Duration(
self._time_pre_grasp_posture +
self._time_grasp_posture + self._time_grasp_posture_final)
grasp_posture.points.append(jtpoint2)
g.pre_grasp_posture = pre_grasp_posture
g.grasp_posture = grasp_posture
header = Header()
header.frame_id = self._grasp_pose_frame_id # base_footprint
q = [pose.orientation.x, pose.orientation.y,
pose.orientation.z, pose.orientation.w]
# Fix orientation from gripper_link to parent_link (tool_link)
fix_tool_to_gripper_rotation_q = quaternion_from_euler(
math.radians(self._fix_tool_frame_to_grasping_frame_roll),
math.radians(self._fix_tool_frame_to_grasping_frame_pitch),
math.radians(self._fix_tool_frame_to_grasping_frame_yaw)
)
q = quaternion_multiply(q, fix_tool_to_gripper_rotation_q)
fixed_pose = copy.deepcopy(pose)
fixed_pose.orientation = Quaternion(*q)
g.grasp_pose = PoseStamped(header, fixed_pose)
g.grasp_quality = self._grasp_quality
g.pre_grasp_approach = GripperTranslation()
g.pre_grasp_approach.direction.vector.x = self._pre_grasp_direction_x # NOQA
g.pre_grasp_approach.direction.vector.y = self._pre_grasp_direction_y # NOQA
g.pre_grasp_approach.direction.vector.z = self._pre_grasp_direction_z # NOQA
g.pre_grasp_approach.direction.header.frame_id = self._grasp_postures_frame_id # NOQA
g.pre_grasp_approach.desired_distance = self._grasp_desired_distance # NOQA
g.pre_grasp_approach.min_distance = self._grasp_min_distance
g.post_grasp_retreat = GripperTranslation()
g.post_grasp_retreat.direction.vector.x = self._post_grasp_direction_x # NOQA
g.post_grasp_retreat.direction.vector.y = self._post_grasp_direction_y # NOQA
g.post_grasp_retreat.direction.vector.z = self._post_grasp_direction_z # NOQA
g.post_grasp_retreat.direction.header.frame_id = self._grasp_postures_frame_id # NOQA
g.post_grasp_retreat.desired_distance = self._grasp_desired_distance # NOQA
g.post_grasp_retreat.min_distance = self._grasp_min_distance
g.max_contact_force = self._max_contact_force
g.allowed_touch_objects = self._allowed_touch_objects
return g
def generate_grasp_msgs(self, grasps):
formatted_grasps = []
for i in range(0, len(grasps)):
g = Grasp()
g.id = "dupa_" + str(i)
gp = PoseStamped()
gp.header.frame_id = "camera_color_optical_frame"
org_q = self.trans_matrix_to_quaternion(grasps[i])
# rot_q = Quaternion(0.7071, 0.7071, 0, 0) # 90* around X axis (W, X, Y, Z)
# quat = rot_q * org_q
quat = org_q
# 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
#why this like Lukasz?
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])
#pprint(gp.pose.orientation)
g.grasp_pose = gp
g.pre_grasp_approach.direction.header.frame_id = "tool_link"
g.pre_grasp_approach.direction.vector.x = 1.0
g.pre_grasp_approach.direction.vector.y = 0.0
g.pre_grasp_approach.direction.vector.z = 0.0
g.pre_grasp_approach.min_distance = 0.04
g.pre_grasp_approach.desired_distance = 0.08
# 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.allowed_touch_objects = ["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
#Create virtual link so I can get the transform from the gripper_link to grasp pose
# transform_msg = geometry_msgs.msg.TransformStamped()
# transform_msg.header.frame_id = "camera_color_optical_frame"
# transform_msg.child_frame_id = "virtual_frame"
# transform_msg.transform.translation.x = g.grasp_pose.pose.position.x
# transform_msg.transform.translation.y = g.grasp_pose.pose.position.y
# transform_msg.transform.translation.z = g.grasp_pose.pose.position.z
# transform_msg.transform.rotation.x = g.grasp_pose.pose.orientation.x
# transform_msg.transform.rotation.y = g.grasp_pose.pose.orientation.y
# transform_msg.transform.rotation.z = g.grasp_pose.pose.orientation.z
# transform_msg.transform.rotation.w = g.grasp_pose.pose.orientation.w
# self.bcaster.sendTransform(transform_msg)
# time.sleep(1)
# #t = self.tf.getLatestCommonTime("virtual_frame", "gripper_link")
# t = rospy.Time(0)
# self.tf.waitForTransform("gripper_link", "virtual_frame", t, rospy.Duration(4.0))
# (v_trans, v_rot) = self.tf.lookupTransformFull("gripper_link", t, "virtual_frame", t, "base_link")
# #t = self.tf.getLatestCommonTime("tool_link", "base_link")
# self.tf.waitForTransform("base_link", "tool_link", t, rospy.Duration(4.0))
# (tool_trans, tool_rot) = self.tf.lookupTransformFull("base_link",t, "tool_link", t, "base_link")
# pprint((v_trans, tool_trans))
# #Update the grasp message, tool_link and gripper have the same orientation
# g.grasp_pose.pose.position.x = tool_trans[0] + v_trans[0]
# g.grasp_pose.pose.position.y = tool_trans[1] + v_trans[1]
# g.grasp_pose.pose.position.z = tool_trans[2] + v_trans[2]
# gp.header.frame_id = "base_link"
#t = rospy.Time(0)
#grasp_point = geometry_msgs.msg.PointStamped()
#grasp_point.header.frame_id = "camera_color_optical_frame"
#grasp_point.point = g.grasp_pose.pose.position
#Get grasp point in base_link coordinate system
#t = self.tf.getLatestCommonTime("camera_color_optical_frame", "base_link")
#print(t)
#self.tf.waitForTransform("camera_color_optical_frame", "base_link", t, rospy.Duration(4.0))
#grasp_base = self.transformer.TransformPose("base_link", grasp_point)
#grasp_base = self.transformer.transform(grasp_point, "base_link", timeout=rospy.Duration(4.0))
grasp_base = self.tf.transformPose("base_link", g.grasp_pose)
# #Get tool and gripper translations from base_link
# #self.tf.waitForTransform("base_link", "tool_link", rospy.Duration(4.0))
# tool_trans, _ = self.tf.lookupTransform("base_link", "tool_link", rospy.Time(0))
# gripper_trans, _ = self.tf.lookupTransform("base_link", "gripper_link", rospy.Time(0))
# g.grasp_pose.header.frame_id = "base_link"
# g.grasp_pose.pose.position.x = tool_trans[0] + grasp_base.pose.position.x - gripper_trans[0]
# g.grasp_pose.pose.position.y = tool_trans[1] + grasp_base.pose.position.y - gripper_trans[1]
# g.grasp_pose.pose.position.z = tool_trans[2] + grasp_base.pose.position.z - gripper_trans[2]
# g.grasp_pose.pose.orientation.x = grasp_base.pose.orientation.x
# g.grasp_pose.pose.orientation.y = grasp_base.pose.orientation.y
# g.grasp_pose.pose.orientation.z = grasp_base.pose.orientation.z
# g.grasp_pose.pose.orientation.w = grasp_base.pose.orientation.w
#pprint(g.grasp_pose)
# q = Quaternion(g.grasp_pose.pose.orientation.w,
# g.grasp_pose.pose.orientation.x,
# g.grasp_pose.pose.orientation.y,
# g.grasp_pose.pose.orientation.z)
# (x_axis, z_axis) = (q.rotate([1.0, 0.0, 0.0]),
# q.rotate([0.0, 0.0, 1.0]))
# g.grasp_pose.header.frame_id = "base_link"
# g.grasp_pose.pose.position.x = grasp_base.pose.position.x - 0.025 * x_axis[0] + 0.015 * z_axis[0]
# g.grasp_pose.pose.position.y = grasp_base.pose.position.y - 0.025 * x_axis[1] + 0.015 * z_axis[1]
# g.grasp_pose.pose.position.z = grasp_base.pose.position.z - 0.025 * x_axis[2] + 0.015 * z_axis[2]
# g.grasp_pose.pose.orientation.x = grasp_base.pose.orientation.x
# g.grasp_pose.pose.orientation.y = grasp_base.pose.orientation.y
# g.grasp_pose.pose.orientation.z = grasp_base.pose.orientation.z
# g.grasp_pose.pose.orientation.w = grasp_base.pose.orientation.w
t = rospy.Time.now()
self.br.sendTransform(
(grasp_base.pose.position.x, grasp_base.pose.position.y,
grasp_base.pose.position.z),
(grasp_base.pose.orientation.x, grasp_base.pose.orientation.y,
grasp_base.pose.orientation.z, grasp_base.pose.orientation.w),
t, "grasp_frame", "base_link")
self.br.sendTransform((-0.025, 0.0, 0.015), (0, 0, 0, 1), t,
"virtual_tool", "grasp_frame")
tool_pose = geometry_msgs.msg.PoseStamped()
tool_pose.header.frame_id = "virtual_tool"
tool_pose.pose.orientation.w = 1.0
self.tf.waitForTransform("base_link", "virtual_tool", t,
rospy.Duration(4.0))
g.grasp_pose.header.frame_id = "base_link"
g.grasp_pose = self.tf.transformPose("base_link", tool_pose)
formatted_grasps.append(g)
return formatted_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_GRASP)
# Set the approach and retreat parameters as desired
#g.pre_grasp_approach = self.make_gripper_translation(0.01, 0.1, [1.0, 1.0, -1.0])
#g.post_grasp_retreat = self.make_gripper_translation(0.1, 0.15, [-1.0, -1.0, 1.0])
# Set the first grasp pose to the input pose
g.grasp_pose = initial_pose_stamped
ideal_roll = 3 #-1.57 #pi/2
ideal_pitch = 0
ideal_yaw = 1.57
step_size = 0.05
idx = 0.3
idx_roll = ideal_roll + idx
idx_pitch = ideal_pitch + idx
idx_yaw = ideal_yaw + idx
roll_vals = []
pitch_vals = []
yaw_vals = []
while idx >= -0.3:
roll_vals.append(idx_roll)
pitch_vals.append(idx_pitch)
yaw_vals.append(idx_yaw)
idx -= step_size
idx_roll -= step_size
idx_pitch -= step_size
idx_yaw -= step_size
# A list to hold the grasps
grasps = []
print "Generating Poses"
# Generate a grasp for each roll pitch and yaw angle
for r in roll_vals:
for y in yaw_vals:
for p in pitch_vals:
# Create a quaternion from the Euler angles
q = quaternion_from_euler(r, 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 "Generated " + g.id + " poses"
# Return the list
return grasps
def create_grasp_2(self, pose, grasp_id):
"""
:type pose: Pose
pose of the gripper for the grasp
:type grasp_id: str
name for the grasp
:rtype: Grasp
"""
g = Grasp()
g.id = grasp_id
pre_grasp_posture = JointTrajectory()
pre_grasp_posture.header.frame_id = self._grasp_postures_frame_id
pre_grasp_posture.joint_names = [
name for name in self._gripper_joint_names.split()
]
jtpoint = JointTrajectoryPoint()
jtpoint.positions = [
float(pos) for pos in self._gripper_pre_grasp_positions.split()
]
jtpoint.time_from_start = rospy.Duration(self._time_pre_grasp_posture)
pre_grasp_posture.points.append(jtpoint)
grasp_posture = copy.deepcopy(pre_grasp_posture)
grasp_posture.points[0].time_from_start = rospy.Duration(
self._time_pre_grasp_posture + self._time_grasp_posture)
jtpoint2 = JointTrajectoryPoint()
jtpoint2.positions = [
float(pos) for pos in self._gripper_grasp_positions.split()
]
jtpoint2.time_from_start = rospy.Duration(
self._time_pre_grasp_posture + self._time_grasp_posture +
self._time_grasp_posture_final)
grasp_posture.points.append(jtpoint2)
g.pre_grasp_posture = pre_grasp_posture
g.grasp_posture = grasp_posture
header = Header()
header.frame_id = self._grasp_pose_frame_id
fixed_pose = copy.deepcopy(pose)
g.grasp_pose = PoseStamped(header, fixed_pose)
g.grasp_quality = self._grasp_quality
g.pre_grasp_approach = GripperTranslation()
g.pre_grasp_approach.direction.vector.x = self._pre_grasp_direction_x # NOQA
g.pre_grasp_approach.direction.vector.y = self._pre_grasp_direction_y # NOQA
g.pre_grasp_approach.direction.vector.z = self._pre_grasp_direction_z # NOQA
g.pre_grasp_approach.direction.header.frame_id = self._grasp_postures_frame_id # NOQA
g.pre_grasp_approach.desired_distance = self._grasp_desired_distance # NOQA
g.pre_grasp_approach.min_distance = self._grasp_min_distance
g.post_grasp_retreat = GripperTranslation()
g.post_grasp_retreat.direction.vector.x = self._post_grasp_direction_x # NOQA
g.post_grasp_retreat.direction.vector.y = self._post_grasp_direction_y # NOQA
g.post_grasp_retreat.direction.vector.z = self._post_grasp_direction_z # NOQA
g.post_grasp_retreat.direction.header.frame_id = self._grasp_postures_frame_id # NOQA
g.post_grasp_retreat.desired_distance = 0.13 # NOQA
g.post_grasp_retreat.min_distance = self._grasp_min_distance
g.max_contact_force = self._max_contact_force
g.allowed_touch_objects = self._allowed_touch_objects
return g
