compute_cartesian_path() does not follow the calculated waypoints

asked 2021-07-06 12:35:08 -0500

amy90 gravatar image

updated 2021-07-06 14:46:56 -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 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

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)

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 ... (more)

edit retag flag offensive close merge delete


amy90 gravatar image amy90  ( 2021-07-06 14:48:01 -0500 )edit