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.2, -0.2, 0.4, -0.4]
#pitch_vals = [0]
yaw_vals = [-0.2, -0.1, 0, 0.1, 0.2]
#yaw_vals = [0]
if mega_angle:
pitch_vals += [0.78, -0.78, 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 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 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 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 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 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 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 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.1, 0.15, [0.0, 0.0, -1.5])
g.post_grasp_retreat = self.make_gripper_translation(
0.1, 0.15, [0.0, 0.0, 1.5])
# 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, 0.5, -0.5, 0.6, -0.6, 0.7,
-0.7, 0.8, -0.8, 0.9, -0.9, 1.0, -1.0, 1.1, -1.1, 1.2, -1.2, 1.3,
-1.3, 1.4, -1.4, 1.5, -1.5
]
# 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._tf2_buff.transform(initial_pose_stamped,
ARM_BASE_FRAME)
x = target_pose_arm_ref.pose.position.x
y = target_pose_arm_ref.pose.position.y
yaw_vals = [atan2(y, x) + inc for inc in [0, 0.1, -0.1]]
# 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 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(pitch)
# Append the grasp to the list
grasps.append(deepcopy(g))
# Return the list
return grasps
g.grasp_posture.joint_names = ["finger_joint"]
pos = JointTrajectoryPoint()
pos.positions.append(0.2)
pos.effort.append(0.0)
g.grasp_posture.points.append(pos)
# set the post-grasp retreat
g.post_grasp_retreat.direction.header.frame_id = "world"
g.post_grasp_retreat.direction.vector.x = 0.0
g.post_grasp_retreat.direction.vector.y = 0.0
g.post_grasp_retreat.direction.vector.z = 1.0
g.post_grasp_retreat.desired_distance = 0.25
g.post_grasp_retreat.min_distance = 0.01
g.allowed_touch_objects = ["table"]
g.max_contact_force = 0
# append the grasp to the list of grasps
grasps.append(g)
rospy.sleep(2)
# pick the object
robot.arm.pick("part", grasps)
rospy.spin()
roscpp_shutdown()
pos.positions.append(0.2)
pos.effort.append(0.0)
g.grasp_posture.points.append(pos)
# set the post-grasp retreat
g.post_grasp_retreat.direction.header.frame_id = "base_footprint"
g.post_grasp_retreat.direction.vector.x = 1
g.post_grasp_retreat.direction.vector.y = -1
g.post_grasp_retreat.direction.vector.z = 1
g.post_grasp_retreat.desired_distance = 0.35
g.post_grasp_retreat.min_distance = 0.01
g.allowed_touch_objects = ["table"]
g.max_contact_force = 0
# append the grasp to the list of grasps
grasps.append(g)
# pick the object
#robot.right_arm.pick("part", grasps)
result = False
n_attempts = 0
# repeat until will succeed
while result == False:
result = robot.right_arm.pick("part", grasps)
n_attempts += 1
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)
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.1, 0.1, [0, 1, 0])
g.post_grasp_retreat = self.make_gripper_translation(
0.1, 0.15, [1, 0, 0])
# Set the first grasp pose to the input pose
g.grasp_pose = initial_pose_stamped
ideal_roll = 0
ideal_pitch = 0
ideal_yaw = 0
step_size = 0.1
idx = 0.1
idx_roll = ideal_roll + idx
idx_pitch = ideal_pitch + idx
idx_yaw = ideal_yaw + idx
roll_vals = []
pitch_vals = []
yaw_vals = []
while idx >= -0.1:
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
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
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 = 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