Python ComputeCartesianPath

[Karol Hausman]

Hi everyone,

I have been playing with Moveit on the PR2 for quite a bit now and I am facing a problem that I wasn't able to solve myself. I am trying to use compute_cartesian_path python function in Moveit with valid waypoints (I checked them with go to pose target) but the resulted fraction is always very small (less than 0.1)

Here is the snippet that I am using:

moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_test',
                  anonymous=True)
robot = moveit_commander.RobotCommander()
group = moveit_commander.MoveGroupCommander("left_arm")
group.allow_replanning(True)
group.set_pose_reference_frame('base_footprint')

pose_target = group.get_current_pose().pose
pose_target.position.x += 0.01

waypoints = []
waypoints.append(pose_target)
(plan3, fraction) = group.compute_cartesian_path(
                               waypoints,   # waypoints to follow
                               0.01,        # eef_step
                               0.0)         # jump_threshold
print "fraction: ", fraction


I tried it with different values of eef step and jump threshold but I can never get the fraction to >0.5

Did I forget about something?

Cheers,
Karol

[Sachin Chitta]

HI Karol,

Could you please test and let us know if you have the same problem with the C++ version of this call? Also, have you tried this from start states other than the default start state?

Also, how are you launching things? Are you using one of the default launch files from source/debians?

Thanks,

Sachin

[Patrick Goebel]

Hi Karol,

Below is the script I have been using successfully to move the gripper
along a Cartesian path using the Python API. Not sure what I am doing
differently but perhaps you can see if this script works on your machine
after changing the group name and target poses. The ~cartesian
parameter is in there just so I can compare Cartesian paths to
non-Cartesian paths by setting it to True (default) or False.

Note that even though I put the compute_cartesian_path() function in a
loop to give it multiple chances to succeed, it seems to always succeed
with a fraction of 1.0 on the first try. Also, the script works the
same whether I use the default KDL kinematics plugin or a custom IK Fast
plugin.

I am running this with the latest Debian install of MoveIt! under ROS
Hydro and Ubuntu 12.04.

--patrick

#!/usr/bin/env python

import rospy, sys
import moveit_commander
from moveit_commander import MoveGroupCommander
from geometry_msgs.msg import Pose
from copy import deepcopy

class MoveItDemo:
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)

# Initialize the ROS node
rospy.init_node('moveit_demo', anonymous=True)

cartesian = rospy.get_param('~cartesian', True)

# Connect to the right_arm move group
right_arm = MoveGroupCommander('right_arm')

# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)

# Set the right arm reference frame
right_arm.set_pose_reference_frame('base_footprint')

# Allow some leeway in position(meters) and orientation (radians)
right_arm.set_goal_position_tolerance(0.01)
right_arm.set_goal_orientation_tolerance(0.1)

# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()

# Start in the "straight_forward" configuration stored in the
SRDF file
right_arm.set_named_target('straight_forward')

# Plan and execute a trajectory to the goal configuration
right_arm.go()

# Get the current pose so we can add it as a waypoint
start_pose = right_arm.get_current_pose(end_effector_link).pose

# Initialize the waypoints list
waypoints = []

# Set the first waypoint to be the starting pose
if cartesian:
# Append the pose to the waypoints list
waypoints.append(start_pose)

wpose = deepcopy(start_pose)

# Set the next waypoint back 0.2 meters and right 0.2 meters
wpose.position.x -= 0.2
wpose.position.y -= 0.2

if cartesian:
# Append the pose to the waypoints list
waypoints.append(deepcopy(wpose))
else:
right_arm.set_pose_target(wpose)
right_arm.go()
rospy.sleep(1)

# Set the next waypoint to the right 0.15 meters
wpose.position.x += 0.05
wpose.position.y += 0.15
wpose.position.z -= 0.15

if cartesian:
# Append the pose to the waypoints list
waypoints.append(deepcopy(wpose))
else:
right_arm.set_pose_target(wpose)
right_arm.go()
rospy.sleep(1)

if cartesian:
# Append the pose to the waypoints list
waypoints.append(deepcopy(start_pose))
else:
right_arm.set_pose_target(start_pose)
right_arm.go()
rospy.sleep(1)

if cartesian:
fraction = 0.0
maxtries = 100
attempts = 0

# Set the internal state to the current state
right_arm.set_start_state_to_current_state()

# Plan the Cartesian path connecting the waypoints
while fraction < 1.0 and attempts < maxtries:
(plan, fraction) = right_arm.compute_cartesian_path (
waypoints, # waypoint poses
0.01, # eef_step
0.0, # jump_threshold
True) # avoid_collisions

# Increment the number of attempts
attempts += 1

# Print out a progress message
if attempts % 10 == 0:
rospy.loginfo("Still trying after " + str(attempts)
+ " attempts...")

# If we have a complete plan, execute the trajectory
if fraction == 1.0:
rospy.loginfo("Path computed successfully. Moving the
arm.")

right_arm.execute(plan)

rospy.loginfo("Path execution complete.")
else:
rospy.loginfo("Path planning failed with only " +
str(fraction) + " success after " + str(maxtries) + " attempts.")

# Move normally back to the 'resting' position
right_arm.set_named_target('resting')
right_arm.go()
rospy.sleep(1)

# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()

# Exit MoveIt
moveit_commander.os._exit(0)

if __name__ == "__main__":
try:
MoveItDemo()
except rospy.ROSInterruptException:
pass

[Karol Hausman]

Hi Patrick and Sachin,

thanks a lot for your responses. I tried Patrick's code and the robot does all the movements perfectly if cartesian = False. When I set cartesian = true then I get fraction equal to 0.14 and it doesnt change with multiple attempts.

This is the message I am getting in the moveit launch file:

[ INFO] [1402498030.924018953]: Attempting to follow 4 waypoints for link 'r_wrist_roll_link' using a step of 0.010000 m and jump threshold 0.000000 (in global reference frame)
[ INFO] [1402498031.047348292]: Computed Cartesian path with 78 points (followed 14.473684% of requested trajectory)

Sachin, my launch file looks as follows:

<launch>
  <arg name="db" default="false" />
  <arg name="debug" default="false" />

  <node pkg="tf" type="static_transform_publisher" name="virtual_joint_broadcaster_0" args="0 0 0 0 0 0 odom_combined base_footprint 100" /> 
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />

  <include file="$(find pr2_moveit_config)/launch/move_group.launch">
    <arg name="allow_trajectory_execution" value="true"/>
    <arg name="fake_execution" value="$(arg debug)"/>
    <arg name="info" value="$(arg debug)"/>
    <arg name="debug" value="$(arg debug)"/>
  </include>

  <include file="$(find pr2_moveit_config)/launch/moveit_rviz.launch">
    <arg name="config" value="$(arg debug)"/>
    <arg name="debug" value="$(arg debug)"/>
  </include>
</launch>

I haven't tried the cpp version of that call yet, that's my next step.

Cheers,
Karol

[Karol Hausman]

I just tested the same commands with C++ API and it worked. - fraction = 100%. I made sure that everything is set the same way in python and C++.

I get 100% fraction in C++

[ INFO] [1402503090.803927615]: Computed Cartesian path with 36 points (followed 100.000000% of requested trajectory)

and 12% in python

[ INFO] [1402503146.771255173]: Computed Cartesian path with 50 points (followed 12.373737% of requested trajectory)

I am also not sure why python script computes many more points...