def build_simple_pickup_goal(grasp_joint_state, pre_grasp_joint_state, fingers_config):
# Adjust finger config un param server
rospy.set_param('/estirabot/skills/grasp/fingers_configuration', fingers_config)
g = Grasp()
g.grasp_posture = grasp_joint_state
g.pre_grasp_posture = pre_grasp_joint_state
r = PickupGoal()
r.desired_grasps.append(g)
return r
def generate_grasps(box,
num_grasps=4,
desired_approach_distance = 0.10,
min_approach_distance = 0.05,
effort = 50):
"""
Generates grasps
Parameters:
box: bounding box to genereate grasps for
num_grasps: number of grasps to generate, spaced equally around the box
desired_approach_distance: how far the pre-grasp should ideally be away from the grasp
min_approach_distance: how much distance between pre-grasp and grasp must actually be feasible for the grasp not to be rejected
Return: a list of grasps
"""
rospy.loginfo("Generating grasps")
grasps = []
for i in range(num_grasps):
g = Grasp()
# align z-axis rotation to box
euler = transformations.euler_from_quaternion([
box.pose.pose.orientation.x,
box.pose.pose.orientation.y,
box.pose.pose.orientation.z,
box.pose.pose.orientation.w])
rot_ang = euler[2]
# apply transformations based on rotations
rot_ang += i * (2 * math.pi) / num_grasps
t1 = transformations.translation_matrix([
box.pose.pose.position.x,
box.pose.pose.position.y,
box.pose.pose.position.z + \
box.box_dims.z * 0.05])
r1 = transformations.rotation_matrix(rot_ang, [0, 0, 1])
box_width = max(box.box_dims.x, box.box_dims.y)
t2 = transformations.translation_matrix([-(box_width/2 + 0.12), 0, 0])
mat = transformations.concatenate_matrices(t1, r1, t2)
translation = transformations.translation_from_matrix(mat)
g.grasp_pose.position.x = translation[0]
g.grasp_pose.position.y = translation[1]
g.grasp_pose.position.z = translation[2]
q = transformations.quaternion_from_matrix(mat)
g.grasp_pose.orientation.x = q[0]
g.grasp_pose.orientation.y = q[1]
g.grasp_pose.orientation.z = q[2]
g.grasp_pose.orientation.w = q[3]
g.desired_approach_distance = desired_approach_distance
g.min_approach_distance = min_approach_distance
pre_grasp_posture = JointState()
pre_grasp_posture.header.stamp = rospy.Time.now()
pre_grasp_posture.header.frame_id = 'base_link'
pre_grasp_posture.name = ['r_wrist_flex_link']
pre_grasp_posture.position = [0.8]
pre_grasp_posture.effort = [effort]
g.pre_grasp_posture = pre_grasp_posture
grasp_posture = JointState()
grasp_posture.header.stamp = rospy.Time.now()
grasp_posture.header.frame_id = 'base_link'
grasp_posture.name = ['r_wrist_flex_link']
grasp_posture.position = [0.45]
grasp_posture.effort = [effort]
g.grasp_posture = grasp_posture
grasps.append(g)
np.random.shuffle(grasps)
return grasps
