UR5 arms doesn't execute path planned by compute_cartesian_path due to empty velocity vector

[CMobley7]

To Whom It May Concern,

I use the following code, based on R. Patrick Goebel moveit_cartesian_demo.py, https://github.com/pirobot/rbx2/blob/indigo-devel/rbx2_arm_nav/scripts/moveit_cartesian_demo.py, to compute a cartesian path between a start_pose and a target_pose. While a path is generated, it fails to execute due to empty velocity and acceleration vectors being sent to ThomasTimm's ur5 driver, https://github.com/ThomasTimm/ur_modern_driver (@ThomasTimm). I found a solution to this issue in C++ here, https://groups.google.com/forum/#!topic/moveit-users/x5FwalM5ruk and https://groups.google.com/forum/#!topic/moveit-users/MOoFxy2exT4, but have been unable to find a similar workaround for moveit_commander. Any help you can provide would be greatly appreciated.

# Set the target pose
        for corner_pose in corner_locations.poses[1:]:

            #Move the end effecor to the x - .45, y, z positon
            #Set the target pose to the hole location in the base_link frame
            target_pose = Pose()
            target_pose.position.x = corner_pose.position.x - 0.075
            target_pose.position.y = corner_pose.position.y
            target_pose.position.z = corner_pose.position.z
            target_pose.orientation.x = corner_pose.orientation.x
            target_pose.orientation.y = corner_pose.orientation.y
            target_pose.orientation.z = corner_pose.orientation.z
            target_pose.orientation.w = corner_pose.orientation.w


            fraction = 0.0
            attempts = 0            
            start_pose = self.arm.get_current_pose(end_effector_link).pose

            # Set the start state to the current state
            self.arm.set_start_state_to_current_state()

            # Plan the Cartesian path connecting the start point and goal
            while fraction < 1.0 and attempts < 100:
                (plan, fraction) = self.arm.compute_cartesian_path([start_pose, target_pose],   # waypoint poses
                                                                    0.01,        # eef_step
                                                                    0.0,         # jump_threshold
                                                                    True)        # avoid_collisions

                # Increment the number of attempts 
                attempts += 1

            # If we have a complete plan, return that plan
            if fraction == 1.0:
                self.arm.execute(plan)
                rospy.loginfo("Successfully sealed section " + str(location))
                location += 1

            else:
                rospy.loginfo("Unable to seal section " + str(location))
                location += 1
                continue

Thank you for your time and attention to this matter. Have a great week. God bless.

Very Respectfully,
CMobley7

[v4hn]

Heya, could you give a minimal example for a python script I can execute with a ur5 to reproduce the issue?
This is related to moveit/moveit_ros#622 which was never merged into indigo, but I feel like that's not the whole story here.

[CMobley7]

v4hn,

My apologies for the delay; however, the file you requested is included below.

#!/usr/bin/env python

"""
    moveit_cartesian_path.py - Version 0.1 2016-07-28

    Based on the R. Patrick Goebel's moveit_cartesian_demo.py demo code.

    Plan and execute a Cartesian path for the end-effector.

    Created for the Pi Robot Project: http://www.pirobot.org
    Copyright (c) 2014 Patrick Goebel.  All rights reserved.
    This program is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation; either version 2 of the License, or
    (at your option) any later version.5

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details at:

    http://www.gnu.org/licenses/gpl.html
"""

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

class MoveItCartesianPath:
    def __init__(self):
        rospy.init_node("moveit_cartesian_path", anonymous=False)

        rospy.loginfo("Starting node moveit_cartesian_path")

        rospy.on_shutdown(self.cleanup)

        # Initialize the move_group API
        moveit_commander.roscpp_initialize(sys.argv)

        # Initialize the move group for the ur5_arm
        self.arm = moveit_commander.MoveGroupCommander('manipulator')

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

        # Set the reference frame for pose targets
        reference_frame = "/base_link"

        # Set the ur5_arm reference frame accordingly
        self.arm.set_pose_reference_frame(reference_frame)

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

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

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

        # Initialize the waypoints list
        waypoints = []

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

        wpose = deepcopy(start_pose)

        # Set the next waypoint to the right 0.5 meters
        wpose.position.y -= 0.5

        waypoints.append(deepcopy(wpose))

        fraction = 0.0
        maxtries = 100
        attempts = 0

        # Set the internal state to the current state
        self.arm.set_start_state_to_current_state()

        # Plan the Cartesian path connecting the waypoints
        while fraction < 1.0 and attempts < maxtries:
            (plan, fraction) = self.arm.compute_cartesian_path (waypoints, 0.01, 0.0, True)

            # 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.")
            self.arm.execute(plan)
            rospy.loginfo("Path execution complete.")
        else:
            rospy.loginfo("Path planning failed with only " + str(fraction) + " success after " + str(maxtries) + " attempts.")

    def cleanup(self):
        rospy.loginfo("Stopping the robot")

        # Stop any current arm movement
        self.arm.stop()

        #Shut down MoveIt! cleanly
        rospy.loginfo("Shutting down Moveit!")
        moveit_commander.roscpp_shutdown()
        moveit_commander.os._exit(0)

if __name__ == "__main__":
    try:
        MoveItCartesianPath()
    except KeyboardInterrupt:
        print "Shutting down MoveItCartesianPath node."

Thank you for your help, as well as, your time and attention to this matter. Have a great rest of the week. God bless.

Very Respectfully,
CMobley7

[v4hn]

This is github, not an office. No need for that much etiquette.

I just merged #29 ,
you can use this new method to add velocity/effort data to the plan you
you get back from cartesian planning like that:

self.robot= moveit_commander.RobotCommander()
self.arm= self.robot.right_arm
...
self.arm.compute_cartesian_path(waypoints, 0.01, 0.0, True)
plan= self.arm.retime_trajectory(self.robot.get_current_state(), plan, 1.0)

This solves your problem on the user side.

The question remains whether we want have velocities/accelerations added in the cartesian
path planning automatically. I added a pull-request for this here.