Robotics StackExchange | Archived questions

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

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

orientation is a quaternion, not an RPY triple. It's unusual for a quaternion to have w equal to 0.

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

Answers