compute_cartesian_path() does not follow the calculated waypoints
I'm working on ROS Kinetic Ubuntu 16.04, Python 2.7. Here I'm attaching the function in which I'm calculating the waypoints and planning the trajectory in MoveIT! . As parameters I have the goal pose read from keyboard in main and the step(10 for example to calculate 10 waypoints). I calculate each of them by interpolating the initial position of the end effector and the final position with linear interpolation, and interpolating the initial orientation and the final one with slerp in a loop, for a given step. I'm trying compute a trajectory with compute_cartesian_path() with the calculated waypoints and than to execute it, but the end effector does not follow them. Does anyoane know what am I doing wrong? Or is there another way to execute the trajectory made by these waypoints?
def go_to_pose_goal(self, goal_position_x, goal_position_y, goal_position_z, goal_roll, goal_pitch, goal_yaw, steps):
move_group = self.move_group
current_pose = self.move_group.get_current_pose().pose
final_pose = geometry_msgs.msg.Pose()
wpose = geometry_msgs.msg.Pose()
waypoints = []
#First we need the initial position of the eef, which is given by moveit_commander and final position(read from keyboard)
#initial position
xstart = current_pose.position.x
ystart = current_pose.position.y
zstart = current_pose.position.z
#final position
xend = goal_position_x #from keyboard
yend = goal_position_y
zend = goal_position_z
#orientation quaternion
quaternion = quaternion_from_euler(goal_roll, goal_pitch, goal_yaw) #transf euler from keyboard into quaternions
final_pose.orientation.x = quaternion[0]
final_pose.orientation.y = quaternion[1]
final_pose.orientation.z = quaternion[2]
final_pose.orientation.w = quaternion[3]
quat0 = [current_pose.orientation.x, current_pose.orientation.y, current_pose.orientation.z, current_pose.orientation.w]
quat1 = [final_pose.orientation.x, final_pose.orientation.y, final_pose.orientation.z, final_pose.orientation.w]
wpose = current_pose
move_group.set_pose_target(wpose)
move_group.go(wait=True)
for i in range (steps): #i for numbering the moves
s = float(i+1)/(steps) #i+1 because i=0 is the current_pose
x = (1-s)*xstart+s*xend
y = (1-s)*ystart+s*yend
z = (1-s)*zstart+s*zend
wpose.position.x = x
wpose.position.y = y
wpose.position.z = z
quat = quaternion_slerp(quat0, quat1, s, 1, shortestpath=True)
wpose.orientation.x = quat[0]
wpose.orientation.y = quat[1]
wpose.orientation.z = quat[2]
wpose.orientation.w = quat[3]
waypoints.append(copy.deepcopy(wpose)) #add the wpose at the end of the 'waypoints' list
print("waypoints", waypoints)
(plan, fraction) = move_group.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0, # jump_threshold
1) # avoid collision
move_group.execute(plan, wait=True)
rospy.sleep(0.1)
move_group.stop()
The initial pose( from current_pose) is this one: position: x: 0.900004864638 y: 0.0399930761474 z: 0.645023924923 orientation: x: 5.10658489884e-05 y: -0.890997693402 z: -0.000341506382529 w: -0.453969478116 The final one is this one: position: x: 0.9 y: 0.4 z: 0.6 orientation: x: 0.0 y: 0.0 z: 0.0 w: 1.0
And the calculated waypoints for a step of 10 by the interpolations are this one: waypoints [position: x: 0.900004864638 y: 0.0399930761474 z: 0.645023924923 orientation: x: 5.10658489884e-05 y: -0.890997693402 z: -0.000341506382529 w: -0.453969478116 ...