Rosanswers logo

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

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 


Originally posted by amy90 on ROS Answers with karma: 1 on 2021-07-05

Post score: 0

Original comments

Comment by gvdhoorn on 2021-07-06:\

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?

Comment by gvdhoorn on 2021-07-06:
I've updated the title of your question to better reflect what your question is about.

Comment by amy90 on 2021-07-06:
I'm using ROS Kinetic because I'm working with Fanuc m6ib and it has a branch on ROS.org just for Kinetic :)

Comment by gvdhoorn on 2021-07-06:\

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.


1 Answer 1


Rosanswers logo

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.

Originally posted by gvdhoorn with karma: 86574 on 2021-07-06

This answer was ACCEPTED on the original site

Post score: 0

Original comments

Comment by amy90 on 2021-07-06:
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.

Comment by gvdhoorn on 2021-07-06:\

I'm updating my post right now.

please revert your edit.

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.


Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.