print "Original goal from planner: \n" + goal_point.__str__() # want to move in negative x direction as encoded by the quarternion offset = 0.19 #0.16645 # Offset between gripper and controled link distance_approach = 0.3 + offset # distance from which to start the approach import tf matrix = tf.transformations.quaternion_matrix([goal_quart.x,goal_quart.y,goal_quart.z,goal_quart.w]) # print matrix goal_point.x -= distance_approach*matrix[0][0] goal_point.y -= distance_approach*matrix[1][0] goal_point.z -= distance_approach*matrix[2][0] print "Start of grasp execution: \n" + goal_point.__str__() final_point.x -= offset*matrix[0][0] final_point.y -= offset*matrix[1][0] final_point.z -= offset*matrix[2][0] print "Goal for grasp: \n" + final_point.__str__() # Interpolate between the two for execution; Implicit: Hope for little pose change if only moving a bit approach_poses = [] for i in range(10): point = Point() point.x = goal_point.x * (9-i)/9 + final_point.x *i/9 point.y = goal_point.y * (9-i)/9 + final_point.y *i/9 point.z = goal_point.z * (9-i)/9 + final_point.z *i/9 approach_poses.append(Pose(point,goal_quart)) # Construct a "pose_stamped" message as required by moveToPose gripper_pose_stamped = PoseStamped() gripper_pose_stamped.header.frame_id = 'base_link' # Creat planning scene and move group to interact with robot