Ask Your Question
0

MoveIt: set_pose_targets() doesn't plan a trajectory through a list of waypoints

asked 2021-07-05 16:26:46 -0500

amy90 gravatar image

updated 2021-07-06 12:18:27 -0500

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)
edit retag flag offensive close merge delete

Comments

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?

gvdhoorn gravatar image gvdhoorn  ( 2021-07-06 05:09:18 -0500 )edit

I've updated the title of your question to better reflect what your question is about.

gvdhoorn gravatar image gvdhoorn  ( 2021-07-06 05:17:38 -0500 )edit

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

amy90 gravatar image amy90  ( 2021-07-06 06:12:08 -0500 )edit

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.

gvdhoorn gravatar image gvdhoorn  ( 2021-07-06 06:53:00 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2021-07-06 05:08:40 -0500

gvdhoorn gravatar image

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.

edit flag offensive delete link more

Comments

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.

amy90 gravatar image amy90  ( 2021-07-06 06:00:46 -0500 )edit

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.

Additionally:

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.

gvdhoorn gravatar image gvdhoorn  ( 2021-07-06 06:54:32 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2021-07-05 16:26:46 -0500

Seen: 83 times

Last updated: Jul 06