To Whom It May Concern,
I'm using the code below to demonstrate sealing a window using Moveit's python interface. When I test it on both Clearpath Robotics' Husky with UR5 and Fetch Robotics' Fetch, I get either an error or erratic behavior that appears to be due to current pose and the start_pose not matching. However, they are set one after each other.
When the code is run on the UR5, the UR5 driver, Thomas Tim's ur_modern_driver ( https://github.com/ThomasTimm/ur_modern_driver), produces the following error.
[ERROR] [1470606513.263462897]: Goal start doesn't match current pose
However, this appears to occur every other trajectory. The Moveit! terminal output is included below.
[ INFO] [1470606478.642762190]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline.
[ INFO] [1470606478.642964878]: Planning attempt 1 of at most 5
[ INFO] [1470606478.651951898]: No planner specified. Using default.
[ INFO] [1470606478.655162522]: RRTConnect: Starting planning with 1 states already in datastructure
[ INFO] [1470606478.672401766]: RRTConnect: Created 5 states (2 start + 3 goal)
[ INFO] [1470606478.672509679]: Solution found in 0.017943 seconds
[ INFO] [1470606478.684172456]: SimpleSetup: Path simplification took 0.011350 seconds and changed from 4 to 2 states
[ INFO] [1470606502.709724240]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
[ INFO] [1470606502.710689320]: RRTConnect: Starting planning with 1 states already in datastructure
[ INFO] [1470606502.780919741]: RRTConnect: Created 4 states (2 start + 2 goal)
[ INFO] [1470606502.780984621]: Solution found in 0.070456 seconds
[ INFO] [1470606502.824677454]: SimpleSetup: Path simplification took 0.000493 seconds and changed from 3 to 2 states
[ INFO] [1470606502.829340760]: Received new trajectory execution service request...
[ INFO] [1470606504.651201818]: Execution completed: SUCCEEDED
[ INFO] [1470606505.912313522]: Received request to compute Cartesian path
[ INFO] [1470606505.912874758]: Attempting to follow 2 waypoints for link 'ur5_arm_marker_link' using a step of 0.010000 m and jump threshold 0.000000 (in global reference frame)
[ INFO] [1470606508.899455833]: Computed Cartesian path with 58 points (followed 100.000000% of requested trajectory)
[ INFO] [1470606508.906403401]: Received new trajectory execution service request...
[ INFO] [1470606511.885918977]: Execution completed: SUCCEEDED
**[ INFO] [1470606511.890627476]: Received request to compute Cartesian path
[ INFO] [1470606511.890953046]: Attempting to follow 2 waypoints for link 'ur5_arm_marker_link' using a step of 0.010000 m and jump threshold 0.000000 (in global reference frame)
[ INFO] [1470606513.256998251]: Computed Cartesian path with 27 points (followed 100.000000% of requested trajectory)
[ INFO] [1470606513.262543511]: Received new trajectory execution service request...
[ WARN] [1470606513.263840859]: Controller arm_controller failed with error code INVALID_GOAL
[ WARN] [1470606513.263936079]: Controller handle arm_controller reports status FAILED
[ INFO] [1470606513.264018256]: Execution completed: FAILED**
[ INFO] [1470606513.265924877]: Received request to compute Cartesian path
[ INFO] [1470606513.266154401]: Attempting to follow 2 waypoints for link 'ur5_arm_marker_link' using a step of 0.010000 m and jump threshold 0.000000 (in global reference frame)
[ INFO] [1470606516.585900623]: Computed Cartesian path with 64 points (followed 100.000000% of requested trajectory)
[ INFO] [1470606516.591274222]: Received new trajectory execution service request...
[ INFO] [1470606520.105039153]: Execution completed: SUCCEEDED
**[ INFO] [1470606520.107687355]: Received request to compute Cartesian path
[ INFO] [1470606520.107977450]: Attempting to follow 2 waypoints for link 'ur5_arm_marker_link' using a step of 0.010000 m and jump threshold 0.000000 (in global reference frame)
[ INFO] [1470606521.445493228]: Computed Cartesian path with 27 points (followed 100.000000% of requested trajectory)
[ INFO] [1470606521.453115526]: Received new trajectory execution service request...
[ WARN] [1470606521.454731340]: Controller arm_controller failed with error code INVALID_GOAL
[ WARN] [1470606521.454904598]: Controller handle arm_controller reports status FAILED
[ INFO] [1470606521.455072531]: Execution completed: FAILED**
[ INFO] [1470606521.457603549]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline.
[ INFO] [1470606521.457719245]: Planning attempt 1 of at most 5
[ INFO] [1470606521.459060714]: RRTConnect: Starting planning with 1 states already in datastructure
[ INFO] [1470606521.472863524]: RRTConnect: Created 4 states (2 start + 2 goal)
[ INFO] [1470606521.480973790]: Solution found in 0.022110 seconds
[ INFO] [1470606521.483070776]: SimpleSetup: Path simplification took 0.001920 seconds and changed from 3 to 2 states
[ INFO] [1470606527.690005836]: Received event 'stop'
[ INFO] [1470606527.690275769]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline.
[ INFO] [1470606527.690327849]: Planning attempt 1 of at most 5
[ INFO] [1470606527.690454600]: Starting state is just outside bounds (joint 'ur5_arm_wrist_1_joint'). Assuming within bounds.
[ INFO] [1470606527.691189388]: RRTConnect: Starting planning with 1 states already in datastructure
[ INFO] [1470606527.715571919]: RRTConnect: Created 4 states (2 start + 2 goal)
[ INFO] [1470606527.716842772]: Solution found in 0.025781 seconds
[ INFO] [1470606527.716900227]: SimpleSetup: Path simplification took 0.000013 seconds and changed from 3 to 2 states
[ INFO] [1470606527.717317337]: Planning adapters have added states at index positions: [ 0 ]
While the Fetch executes every trajectory, at the end of some of the trajectories (sometimes all of them, except the last), the arm will move back to a position along the Cartesian path
and repeat much of the last trajectory before beginning the next. The Moveit! terminal output is included below. In addition, I have linked to a video due to GitHub not letting me add it as a .zip. (https://drive.google.com/file/d/0B8QbGCFcn-dwYU55cXQ0SU5oamM/view?usp=sharing)
[ INFO] [1470607840.318662060]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline.
[ INFO] [1470607840.318808855]: Planning attempt 1 of at most 5
[ INFO] [1470607840.325326234]: No planner specified. Using default.
[ INFO] [1470607840.325449348]: LBKPIECE1: Attempting to use default projection.
[ INFO] [1470607840.326953858]: LBKPIECE1: Starting planning with 1 states already in datastructure
[ INFO] [1470607840.478435172]: LBKPIECE1: Created 57 (8 start + 49 goal) states in 42 cells (7 start (7 on boundary) + 35 goal (35 on boundary))
[ INFO] [1470607840.478483011]: Solution found in 0.152855 seconds
[ INFO] [1470607840.479726155]: SimpleSetup: Path simplification took 0.001129 seconds and changed from 24 to 2 states
[ INFO] [1470607845.159145777]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
[ INFO] [1470607845.162410200]: No planner specified. Using default.
[ INFO] [1470607845.162452518]: LBKPIECE1: Attempting to use default projection.
[ INFO] [1470607845.163552137]: LBKPIECE1: Starting planning with 1 states already in datastructure
[ INFO] [1470607845.343225032]: LBKPIECE1: Created 27 (12 start + 15 goal) states in 24 cells (11 start (11 on boundary) + 13 goal (13 on boundary))
[ INFO] [1470607845.343365727]: Solution found in 0.180874 seconds
[ INFO] [1470607845.386835195]: SimpleSetup: Path simplification took 0.004402 seconds and changed from 14 to 2 states
[ INFO] [1470607845.405667133]: Received new trajectory execution service request...
[ INFO] [1470607849.518333136]: Execution completed: SUCCEEDED
[ INFO] [1470607850.774847453]: Received request to compute Cartesian path
[ INFO] [1470607850.775077716]: Attempting to follow 2 waypoints for link 'wrist_roll_link' using a step of 0.010000 m and jump threshold 0.000000 (in global reference frame)
[ INFO] [1470607853.644398866]: Computed Cartesian path with 58 points (followed 100.000000% of requested trajectory)
[ INFO] [1470607853.646321709]: Received new trajectory execution service request...
[ INFO] [1470607865.433584192]: Execution completed: SUCCEEDED
[ INFO] [1470607865.435228287]: Received request to compute Cartesian path
[ INFO] [1470607865.435497986]: Attempting to follow 2 waypoints for link 'wrist_roll_link' using a step of 0.010000 m and jump threshold 0.000000 (in global reference frame)
[ INFO] [1470607869.526370248]: Computed Cartesian path with 82 points (followed 100.000000% of requested trajectory)
[ INFO] [1470607869.528837090]: Received new trajectory execution service request...
[ INFO] [1470607886.033478353]: Execution completed: SUCCEEDED
[ INFO] [1470607886.034956138]: Received request to compute Cartesian path
[ INFO] [1470607886.035082853]: Attempting to follow 2 waypoints for link 'wrist_roll_link' using a step of 0.010000 m and jump threshold 0.000000 (in global reference frame)
[ INFO] [1470607890.075843317]: Computed Cartesian path with 81 points (followed 100.000000% of requested trajectory)
[ INFO] [1470607890.077995486]: Received new trajectory execution service request...
[ INFO] [1470607906.283832539]: Execution completed: SUCCEEDED
[ INFO] [1470607906.285818011]: Received request to compute Cartesian path
[ INFO] [1470607906.285920761]: Attempting to follow 2 waypoints for link 'wrist_roll_link' using a step of 0.010000 m and jump threshold 0.000000 (in global reference frame)
[ INFO] [1470607910.364466796]: Computed Cartesian path with 82 points (followed 100.000000% of requested trajectory)
[ INFO] [1470607910.366617598]: Received new trajectory execution service request...
[ INFO] [1470607926.878415544]: Execution completed: SUCCEEDED
[ INFO] [1470607926.879282723]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline.
[ INFO] [1470607926.879340369]: Planning attempt 1 of at most 5
[ INFO] [1470607926.882826435]: LBKPIECE1: Starting planning with 1 states already in datastructure
[ INFO] [1470607926.983473584]: LBKPIECE1: Created 30 (19 start + 11 goal) states in 23 cells (18 start (18 on boundary) + 5 goal (5 on boundary))
[ INFO] [1470607926.983519283]: Solution found in 0.101442 seconds
[ INFO] [1470607927.067627948]: SimpleSetup: Path simplification took 0.084048 seconds and changed from 16 to 17 states
[ INFO] [1470607935.318948160]: Received event 'stop'
[ INFO] [1470607935.319185843]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline.
[ INFO] [1470607935.319235286]: Planning attempt 1 of at most 5
[ INFO] [1470607935.326490876]: LBKPIECE1: Starting planning with 1 states already in datastructure
[ INFO] [1470607935.427244565]: LBKPIECE1: Created 38 (27 start + 11 goal) states in 31 cells (26 start (26 on boundary) + 5 goal (5 on boundary))
[ INFO] [1470607935.427405089]: Solution found in 0.102788 seconds
[ INFO] [1470607935.427739310]: SimpleSetup: Path simplification took 0.000018 seconds and changed from 18 to 2 states
#!/usr/bin/env python
"""
corner_prediction.py - Version 0.1 2016-08-07
Use Moveit to seal a window frame
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
import sys
import moveit_commander
from geometry_msgs.msg import PoseArray, PoseStamped, Pose
from copy import deepcopy
class CornerPrediction:
def __init__(self):
#Give the node a name
rospy.init_node("corner_prediction", anonymous=False)
rospy.loginfo("Starting node seal_prediction")
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.robot= moveit_commander.RobotCommander()
#Uncomment for Husky
#self.arm = self.robot.ur5_arm
self.arm = self.robot.arm_with_torso
# 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.001)
self.arm.set_goal_orientation_tolerance(0.001)
# Start the arm in the work pose stored in the SRDF file
self.arm.set_named_target("work")
self.arm.go()
rospy.sleep(2)
corner_locations = rospy.wait_for_message("/initial_locations_seal", PoseArray)
# Initialize needed variables
traj = None
location = 1
corner_pose = corner_locations.poses[0]
#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 = PoseStamped()
target_pose.header.frame_id = reference_frame
target_pose.header.stamp = rospy.Time.now()
target_pose.pose.position.x = corner_pose.position.x - 0.1
target_pose.pose.position.y = corner_pose.position.y
target_pose.pose.position.z = corner_pose.position.z
target_pose.pose.orientation.x = corner_pose.orientation.x
target_pose.pose.orientation.y = corner_pose.orientation.y
target_pose.pose.orientation.z = corner_pose.orientation.z
target_pose.pose.orientation.w = corner_pose.orientation.w
# Set the start state to the current state
self.arm.set_start_state_to_current_state()
# Set the goal pose of the end effector to the stored pose
self.arm.set_pose_target(target_pose, end_effector_link)
# Plan the trajectory to the goal
traj = self.arm.plan()
if traj is not None:
# Execute the planned trajectory
self.arm.execute(traj)
# Pause for a second
rospy.sleep(1)
rospy.loginfo("Successfully moved to corner " + str(location))
else:
rospy.loginfo("Unable to reach corner " + str(location))
seal_locations = deepcopy(corner_locations)
seal_locations.poses.append(corner_locations.poses[0])
# Set the target pose
for corner_pose in seal_locations.poses[1:]:
#Move the end effecor to the x - 0.1, 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.1
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
plan = None
**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
False) # avoid_collisions
# Increment the number of attempts
attempts += 1
# If we have a complete plan, return that plan
if fraction == 1.0:
#Uncomment for Husky
#plan = self.arm.retime_trajectory(self.robot.get_current_state(), plan, 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
# Finish the ur5_arm in the stow pose stored in the SRDF file
self.arm.set_named_target("stow")
self.arm.go()
rospy.sleep(2)
def cleanup(self):
rospy.loginfo("Stopping the robot")
# Stop any current arm movement
self.arm.stop()
# Move the arm to the stow position
self.arm.set_named_target("stow")
self.arm.go()
#Shut down MoveIt! cleanly
rospy.loginfo("Shutting down Moveit!")
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
if __name__ == "__main__":
try:
CornerPrediction()
except KeyboardInterrupt:
print "Shutting down CornerPrediction node."
Thank you for any help you are able to provide, as well as your time and attention to this matter. Have a great week. God bless.
Very Respectfully,
CMobley7
React
Typescript
Django
Laravel
Facebook
Microsoft
Google
Alibaba
D3
Tencent