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
