MoveIt! Let robot move to pose with IKFast

asked 2021-03-15 09:57:07 -0500

vonunwerth gravatar image

updated 2021-03-16 17:51:04 -0500

Hihi, i try to let a UR10 robot move to a pose using the following python code. I simply get the current pose and change it a bit. I also tried modifying the other axis and some random poses, but the the same here.

Before i created a IKFast Plugin like descriped here: https://roboy-ik-doc.readthedocs.io/e..., changed the kinematics.yaml to use the plugin and tested it with the openraves iktests. The most (96%) of the iktests finished successfully. Then I started the robots demo.launch, moved the goal using the arrows clicked Plan&Execute. As OMPL RRTConnect is selected. The robot now moves to the desired pose. Is RVIZ using my IKFast plugin at this point? By the way do i understand it correctly, that the IKFast (or KDL or the basic UR10Kinematic one) generates a list of possible joint configurations which generates the desired pose and after that the OMPL choose the "best" one?

When I now try to start my python code, i get the following error:

Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline.
[ INFO] [1615818612.199151226]: Planning attempt 1 of at most 1
[ INFO] [1615818612.199753194]: Planner configuration 'manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1615818612.199960497]: RRTConnect: Starting planning with 1 states already in datastructure
[ERROR] [1615818613.564380162]: RRTConnect: Unable to sample any valid states for goal tree
[ INFO] [1615818613.564471329]: RRTConnect: Created 1 states (1 start + 0 goal)
[ INFO] [1615818613.564490864]: No solution found after 1.364602 seconds
[ INFO] [1615818613.564517949]: Unable to solve the planning problem
[ INFO] [1615818613.565357171]: Received event 'stop'

If I use the URKinematics basic plugin in kinematics.yaml it does work. So I think there could be something with the IKFast Plugin. But why does it then work, when using the RVIZ Gui?

def move_robot():
    robot = moveit_commander.RobotCommander()
    scene = moveit_commander.PlanningSceneInterface()
    group_name = "manipulator"
    group = moveit_commander.MoveGroupCommander(group_name)
    display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',
                                                   moveit_msgs.msg.DisplayTrajectory,
                                                   queue_size=20)
    planning_frame = group.get_planning_frame()
    eef_link = group.get_end_effector_link()
    group_names = robot.get_group_names()
    pose_goal = group.get_current_pose()
    pose_goal.pose.position.y += -0.002
    group.set_pose_target(pose_goal)

    plan = group.go(wait=True)
    group.stop()
    group.clear_pose_targets()


if __name__ == '__main__':
    args = rospy.myargv()
    rospy.init_node('move_it_tutorial', anonymous=True)
    move_robot()

Thanks for your help

edit retag flag offensive close merge delete

Comments

1

An other thing to ask. I see there is an ikfast.h file in the ur_kinematics package. Are the ur_kinematics simply an ikfast generated solution (or based on that)?

vonunwerth gravatar image vonunwerth  ( 2021-03-15 20:00:20 -0500 )edit