Unable to move simulated ur5 to target goal
I'm trying to get the simulated UR5 robot in gazebo to move it's end effector to a specific point. This is the tutorial code
#!/usr/bin/env python3
import rospy
import moveit_commander
import geometry_msgs.msg
import sys
def move_to_pose():
# Initialize the moveit_commander and rospy
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('ur_pose_controller', anonymous=True)
# Instantiate a RobotCommander object to interface with the robot
robot = moveit_commander.RobotCommander()
# Instantiate a PlanningSceneInterface object to interact with the world
scene = moveit_commander.PlanningSceneInterface()
# Instantiate a MoveGroupCommander object for the UR5 arm
group_name = "manipulator"
move_group = moveit_commander.MoveGroupCommander(group_name)
# Set the target pose
target_pose = geometry_msgs.msg.Pose()
target_pose.position.x = 0.01 # Specify the desired x position
target_pose.position.y = 0.01 # Specify the desired y position
target_pose.position.z = 0.01 # Specify the desired z position
target_pose.orientation.x = 0.0 # Specify the desired x orientation
target_pose.orientation.y = 0.01 # Specify the desired y orientation
target_pose.orientation.z = 0.0 # Specify the desired z orientation
target_pose.orientation.w = 0.0 # Specify the desired w orientation
# Set the target pose as the goal for the arm
move_group.set_pose_target(target_pose)
# Plan and execute the motion
plan = move_group.go(wait=True)
# Print the execution result
if plan:
rospy.loginfo("Motion planning and execution succeeded!")
else:
rospy.logerr("Motion planning and execution failed!")
# Clean up MoveIt! resources
move_group.clear_pose_targets()
moveit_commander.roscpp_shutdown()
if __name__ == '__main__':
try:
move_to_pose()
except rospy.ROSInterruptException:
pass
The goal position I set isnt crazy so I dont see when the joints wouldn't be able to figre it out. However I get :
rosrun arm_test test.py
[ INFO] [1687302369.831549949]: Loading robot model 'ur5_robot'...
[ WARN] [1687302369.909696535]: IK plugin for group 'manipulator' relies on deprecated API. Please implement initialize(RobotModel, ...).
[ INFO] [1687302369.912024761]: IK Using joint shoulder_link -6.28319 6.28319
[ INFO] [1687302369.912046131]: IK Using joint upper_arm_link -6.28319 6.28319
[ INFO] [1687302369.912055311]: IK Using joint forearm_link -3.14159 3.14159
[ INFO] [1687302369.912063102]: IK Using joint wrist_1_link -6.28319 6.28319
[ INFO] [1687302369.912070931]: IK Using joint wrist_2_link -6.28319 6.28319
[ INFO] [1687302369.912079314]: IK Using joint wrist_3_link -6.28319 6.28319
[ INFO] [1687302369.912091161]: Looking in common namespaces for param name: manipulator/position_only_ik
[ INFO] [1687302369.913252206]: Looking in common namespaces for param name: manipulator/solve_type
[ INFO] [1687302369.915273582, 1005.398000000]: Using solve type Distance
[ INFO] [1687302370.942939093, 1006.422000000]: Ready to take commands for planning group manipulator.
[ INFO] [1687302375.955777049, 1011.413000000]: ABORTED: TIMED_OUT
[ERROR] [1687302375.955978, 1011.413000]: Motion planning and execution failed!
and
[ WARN] [1687301381.874166425, 21.087000000]: Orientation constraint for link 'tool0' is probably incorrect: 0.000000, 0.100000, 0.000000, 0.000000. Assuming identity instead.
[ INFO] [1687301381.875202234, 21.088000000]: Planner configuration 'manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1687301381.875654957, 21.089000000]: manipulator/manipulator: Starting planning with 1 states already in datastructure
[ERROR] [1687301386.883659881, 26.087000000]: manipulator/manipulator: Unable to sample any valid states for goal tree
[ INFO] [1687301386.883701468, 26.087000000]: manipulator/manipulator: Created 1 states (1 start + 0 goal)
[ INFO] [1687301386.883728277, 26.087000000]: No solution found after 5.008187 seconds
[ WARN] [1687301386.883786515, 26.087000000]: Timed out
[ INFO] [1687301386.896475529, 26.099000000]: Unable to solve the planning problem
[ INFO] [1687302370.943444422, 1006.422000000]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline.
[ INFO] [1687302370.943521480, 1006.422000000]: Planning attempt 1 of at most 1
[ INFO] [1687302370.943624740, 1006.422000000]: Starting state is just outside bounds (joint 'wrist_3_joint'). Assuming within bounds.
[ WARN] [1687302370.944054229, 1006.423000000]: Orientation constraint for link 'tool0' is probably incorrect: 0.000000, 0.010000, 0.000000, 0.000000. Assuming identity instead.
[ WARN] [1687302370.944193260, 1006.423000000]: Orientation constraint for link 'tool0' is probably incorrect: 0.000000, 0.010000, 0.000000, 0.000000. Assuming identity instead.
[ INFO] [1687302370.945570209, 1006.424000000]: Planner configuration 'manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1687302370.945965668, 1006.425000000]: manipulator/manipulator: Starting planning with 1 states already in datastructure
[ERROR] [1687302375.954145111, 1011.411000000]: manipulator/manipulator: Unable to sample any valid states for goal tree
[ INFO] [1687302375.954228610, 1011.411000000]: manipulator/manipulator: Created 1 states (1 start + 0 goal)
[ INFO] [1687302375.954274855, 1011.411000000]: No solution found after 5.008384 seconds
[ WARN] [1687302375.954312887, 1011.411000000]: Timed out
[ INFO] [1687302375.955287448, 1011.412000000]: Unable to solve the planning problem
I dont know what tool0 means specifically. The joints shouldnt be at their limit trying to peform the task. I'm very new to UR and I'm pretty sure what I'm trying to do is simple. Any feedback would be greatly appreciated
Asked by distro on 2023-06-22 00:51:47 UTC
Comments
orientation
is a quaternion, not an RPY triple. It's unusual for a quaternion to havew
equal to0
.Are you sure this is correct?
Asked by gvdhoorn on 2023-06-22 02:53:36 UTC
@gvdhoom what should I set
w
as to test?Asked by distro on 2023-06-22 02:58:01 UTC
That depends on the pose you want the link you are planning for to reach. To learn more about quaternions, you could take a look at tf2/Tutorials/Quaternions. And perhaps also #q9981.
Asked by gvdhoorn on 2023-06-22 04:53:33 UTC
@gvdhoorn thanks, I have a better understanding now.
Asked by distro on 2023-07-08 20:02:28 UTC