MoveIt: set_pose_targets() doesn't plan a trajectory through a list of 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 the calculated waypoints and than to execute it, but the end effector does not follow them. Does anyoane know what am I doing wrong and how can I plan it? Thanks a lot!
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
move_group.set_pose_targets(waypoints)
move_group.go(wait=True)
rospy.sleep(0.1)
out of curiosity: could you tell us why you are still using Kinetic on Ubuntu Xenial?
I've updated the title of your question to better reflect what your question is about.
I'm using ROS Kinetic because I'm working with Fanuc m6ib and it has a branch on ROS.org just for Kinetic :)
please read the readme carefully. Specifically the section about building on older and newer versions of ROS.
There is no need to use ROS Kinetic.
You really want to use more up-to-date OS, ROS and MoveIt versions.