Cartesian move for end effector (Python version)

[clint p]

Hello all,

I'm currently new to ROS, using Hydro. I'm doing some Python stuff with our robot, a 7-DOF Motoman arm.

I'm just trying to do a simple cartesian move with moveitcommander. The code moves the end effector back 0.16 meters in the x direction by the default path planner, and moving it back to its original pose via cartesian move.

It's failing to compute a cartesian path, but the fraction is between 0 and 1, the fraction varies. It gave me back about 15% computed the last time I tried before the computation failed. I've put the code below. Please excuse the horrible mess, I was experimenting a lot. Any help is appreciated, I'm just confused why it will compute for part of the path, but not all of it.

here's a video of it (attempting) to work: http://www.youtube.com/watch?v=UB1EPkSMkEw&feature=youtu.be


#!/usr/bin/env python

import rospy
from moveit_commander import RobotCommander, PlanningSceneInterface, MoveGroupCommander, conversions
from geometry_msgs.msg import Pose, PoseStamped
from moveit_msgs.msg import RobotTrajectory, Grasp

if __name__=='__main__':

    rospy.init_node('moveit_py_demo', anonymous=True)

    scene = PlanningSceneInterface()

    robot = MoveGroupCommander("sia5d");
    rospy.sleep(1)
   
    currentrf = robot.get_pose_reference_frame()
    robot.set_pose_reference_frame(currentrf)
    newpose = robot.get_current_pose().pose
    a = robot.get_current_pose().pose
    newposerpy = robot.get_current_rpy()
    print newpose
    print newposerpy

    print ('Going to..')
    newpose.position.x = newpose.position.x-0.15
    print newpose   
    print('Setting a new target pose..')
    robot.set_pose_target(newpose)
    planned = robot.plan()
    print ('Joint interpolated plan:')
    print planned
    robot.execute(planned)
    rospy.sleep(2)

    newpose = robot.get_current_pose().pose
    print newpose
    robot.set_start_state_to_current_state()
    robot.set_pose_target(a)
    b = robot.compute_cartesian_path([newpose, a],0.01, 10000)
    pathplan = robot.plan(b[0])
    fraction = b[1]
    print('cartesian path computed:')
    print pathplan

    robot.execute(pathplan)

    finalpose = robot.get_current_pose().pose
    print newpose.position.y
    print finalpose.position.y
    print fraction

    rospy.spin()