# 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
final_pose.orientation.y = quaternion
final_pose.orientation.z = quaternion
final_pose.orientation.w = quaternion
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
wpose.orientation.y = quat
wpose.orientation.z = quat
wpose.orientation.w = quat
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)

edit retag close merge delete

I'm working on ROS Kinetic Ubuntu 16.04, Python 2.7

out of curiosity: could you tell us why you are still using Kinetic on Ubuntu Xenial?

I'm using ROS Kinetic because I'm working with Fanuc m6ib and it has a branch on ROS.org just for Kinetic :)

I'm using ROS Kinetic because I'm working with Fanuc m6ib and it has a branch on ROS.org just for Kinetic :)

There is no need to use ROS Kinetic.

You really want to use more up-to-date OS, ROS and MoveIt versions.

Sort by » oldest newest most voted

It's been a while since I've used the MoveIt commander, but from the C++ documentation of this method (which the commander essentially wraps):

When planning, the planner will find a path to one (arbitrarily chosen) pose from the list. If this group contains multiple end effectors then all end effectors in the group should have the same number of pose targets. If planning is successful then the result of the plan will place all end effectors at a pose from the same index in the list. (In other words, if one end effector ends up at the 3rd pose in the list then all end effectors in the group will end up at the 3rd pose in their respective lists. End effectors which do not matter (i.e. can end up in any position) can have their pose targets disabled by calling clearPoseTarget() for that end_effector_link. This new orientation target replaces any pre-existing JointValueTarget or pre-existing Position, Orientation, or Pose target(s) for this end_effector_link.

Summarising: set_pose_targets(..) does not do what you assumed it does.

It is not used to make "MoveIt plan a trajectory computed by a set of waypoints", but has a wholly different purpose.

more

Thank you so much for your response! I also tried to compute a trajectory with ComputeCartesianPath() with those waypoints and then execute the plan made by it, but it doesn't follow the waypoints either. I'm updating my post right now.

I'm updating my post right now.

the text in your question body after the edit is almost completely disconnected from the version I answered.

For future readers it will be almost impossible to understand the chronology of questions and answers like this.

If you want to provide additional information, edit your original question, but append the new information. Do not overwrite the old version of the question.

the end effector does not follow them

this is not sufficient for anyone to understand what is going wrong. Please provide more information, such as any errors or warnings you observe, the behaviour fo your "end effector" if/when it does do something, what you expect, etc.

Simply stating "it does not work" is not enough.