Your browser was unable to load all of the resources. They may have been blocked by your firewall, proxy or browser configuration.
Press Ctrl+F5 or Ctrl+Shift+R to have your browser try again.

Start configuration is infeasible #4

#1

Klamp't version: 0.8.1
Operating System: Ubuntu 16.04 LTS
IDE: PyCharm 2018.3.5

Problem Description:
Hello Dr.Hauser. We want to let the UR5e with Reflex Takktile Robotic Hand grasp a cylindrical cup. Firstly, we use the inverse kinematics solver to find the goal configuration. Secondly, we input the start configuration and goal configuration to the motion planner to get the path or trajectory.
We want to allow the robot goal configuration solution can rotate through the cup's Z axis, so the robot can always find a solution to catch a cup when the cup's position changed in world coordination. However, the motion planner always reports:

RuntimeError: Start configuration is infeasible

Bur when both the orientation and position are fixed, we can get a valid goal configuration in the inverse kinematics solver, and we also can get the valid path through the start and goal configuration through the motion planner.
Here is our whole code:

from klampt import *
from klampt.plan import cspace, robotplanning
from klampt.model.trajectory import RobotTrajectory
from klampt.model import ik
from klampt import vis
from klampt.model import coordinates
import numpy as np

import math
import time


def motion_planner(robot, qstart, qgoal, gen_space):
    settings = {'type':'sbl',
        'perturbationRadius':0.25,
        'bidirectional':True,
        'shortcut':True,
        'restart':False,
        'restartTermCond':"{foundSolution:2,maxIters:4000}"
    }
    t0 = time.time()
    print "Creating planner..."
    #Manual construction of planner
    planner = cspace.MotionPlan(gen_space, **settings)
    planner.setEndpoints(qstart, qgoal)
    print "Planner creation time", time.time()-t0
    t0 = time.time()
    print "Planning..."
    numIters = 0
    for round in range(10):
        planner.planMore(5000)
        numIters += 1
        if planner.getPath() is not None:
          break
    print "Planning time,", numIters,"iterations", time.time()-t0

    path = planner.getPath()
    if path is not None:
        path = gen_space.discretizePath(path)
        print "Got a path with", len(path), "milestones"
    else:
        print "No feasible path was found"

    #provide some debugging information
    V, E = planner.getRoadmap()
    print len(V), "feasible milestones sampled,", len(E), "edges connected"
    return path


def gen_hand_dictionary(robot):
    dict = {}
    for i in range(6, 40):
        dict[robot.driver(i).getName()] = i
    return dict


def gen_had_traj(robot, dict, space):
    qold = robot.getConfig()
    milestone = 55
    #set goal of hand configuration
    robot.driver(dict.get("gripper:swivel_1")).setValue(0)
    robot.driver(dict.get("gripper:proximal_1")).setValue(1.4)
    robot.driver(dict.get("gripper:proximal_2")).setValue(1.4)
    robot.driver(dict.get("gripper:proximal_3")).setValue(1.4)
    qnew = robot.getConfig()
    qmin,qmax = robot.getJointLimits()
    # for i in range(len(qold)):
    #     print i,qold[i],qnew[i],qmin[i],qmax[i]
    # raw_input()
    sub = []
    from klampt.math import so2
    for i in range(len(qnew)):
        # sub.append(qnew[i] - qold[i])
        sub.append(so2.diff(qnew[i],qold[i]))
    robot.setConfig(qold)
    path = []
    for i in range(milestone):
        point = []
        for j in range(len(qnew)):
            point.append(qold[j] + sub[j]/(milestone-i))
        path.append(point)
    for i in range(6*milestone):
        path.append(path[-1])
    return path


def solve_ik(robotlink, localpos, worldpos):
    linkindex = robotlink.index
    robot = robotlink.robot()
    obj = ik.objective(robotlink, local=localpos, world=worldpos)
    maxIters = 100
    tol = 1e-3
    s = ik.solver(obj, maxIters, tol)
    # Set up some parameters for the numerical solver
    # Optionally you can set an initial configuration like so:
    # robot.setConfig([0]*robot.numLinks())
    # or set a random initial configuration like so:
    # s.sampleInitial()
    res = s.solve();
    if not res:
        print "Couldn't solve IK problem"
        s.sampleInitial()
    return robot.getConfig()


def visualize_world(world):
    vis.add("world", world)
    vis.add("coordinates", coordinates.manager())
    vis.setWindowTitle("Height Map GeneratorSuperclass World")
    vp = vis.getViewport()
    vp.w, vp.h = 600, 600
    vis.setViewport(vp)
    vis.autoFitCamera()
    vis.show()


def get_world(world_file, robot_file, visualize_robot=True):
    world = WorldModel()
    res = world.readFile(world_file)
    if not res:
        raise RuntimeError("Unable to load terrain model")
    if visualize_robot:
        res = world.readFile(robot_file)
        if not res:
            raise RuntimeError("Unable to load robot model")
        # world.robot(0).setConfig(ProjectConstants.STARTING_CONFIG)
    return world


if __name__ == "__main__":
    world_name = "flatworld"

    world_file = "data/worlds/" + world_name + ".xml"
    robot_file = "data/robots/ur5_with_reflex.rob"

    world = get_world(world_file, robot_file, visualize_robot=True)
    time.sleep(1)

    robot = world.robot(0)
    qstart = robot.getConfig()
    qstart[2] = -0.5 * math.pi
    qstart[6] = -0.5 * math.pi
    robot.setConfig(qstart)
    link = robot.link(7)
    # coordinates of the constrained IK point on the robot, in the link's local frame
    ### Orientation fixed ###
    # localpos = [[0, 0, 0], [0.1, 0, 0], [0, 0, 0.1]]
    ### Allow Z axis rotating ###
    localpos = [[0, 0, 0], [0, 0, 0.1]]
    # coordinates of the IK goal in world coordinates, modify it whatever you want.
    ### Orientation fixed ###
    # position = [[0.1, 0.5, 0.5], [0.1, 0.6, 0.5], [0.1, 0.5, 0.6]]
    ### Allow Z axis rotating ###
    position = [[0.1, 0.5, 0.5], [0.1, 0.5, 0.6]]

    # fire up a visual editor to get some start and goal configurations
    qgoal = robot.getConfig()
    # use ik to get the qgoal
    qgoal = solve_ik(link, localpos, position)

    # this is the CSpace that will be used.  Standard collision and joint limit constraints
    # will be checked

    ### Use fixed orientation in the path ###
    # obj = ik.fixed_rotation_objective(robot.link(6))
    # space = robotplanning.makeSpace(world, robot, edgeCheckResolution=0.05,
    #                                 equalityConstraints=[obj],
    #                                 equalityTolerance=0.01)

    ### Allow the Z axis rotating in the path ###
    obj1 = ik.fixed_rotation_objective(robot.link(6), world_axis=[1, 0, 0])
    obj2 = ik.fixed_rotation_objective(robot.link(6), world_axis=[0, 1, 0])
    space = robotplanning.makeSpace(world, robot, edgeCheckResolution=0.05,
                                    equalityConstraints=[obj1, obj2],
                                    equalityTolerance=0.01)

    # start motion planning and visualize it
    print "The start configuration is: ", qstart
    print "The goal configuration is: ", qgoal
    path1 = motion_planner(robot, qstart, qgoal, space)
    # get the hands dictionary
    hand_dict = gen_hand_dictionary(robot)
    path2 = gen_had_traj(robot, hand_dict, space)

    path_comb = path1 + path2
    traj = RobotTrajectory(robot,
                           np.linspace(0, 5, len(path_comb)).tolist(),
                           path_comb)

    # Here's another way to do it: visualize path in the vis module
    vis.add("world", world)
    vis.animate(("world", robot.getName()), traj)
    vis.add("trajectory", traj)  # this draws the end effector trajectory
    vis.spin(float('inf'))

How to solve the problem that the motion planner always reports "Start configuartion is infeasible"?. Thank you.

  • solved #2
  • replies 6
  • views 6.5K
  • likes 0
#2

Hi Yuan, we figured this out in person. Specifically the ik.fixed_rotation function uses the robots current configuration, which changes after you’ve solved for the goal configuration. Can you post the solution?

blackjack1815 accepted post #2 as the answer
#3
#4

Sure. Firstly, the constraint:

obj2 = ik.fixed_rotation_objective(robot.link(6), world_axis=[0, 1, 0])

is not necessary, we only add constraints:

obj1 = ik.fixed_rotation_objective(robot.link(6), world_axis=[1, 0, 0])

Constraint obj1 already only allowed the robot.link(6) rotating with its Z axis.

Secondly, we can use:

print "Start fails", space.cspace.feasibilityFailures(qstart)
    print "Goal fails", space.cspace.feasibilityFailures(qgoal)

to find the reason why the start configuration and goal configuration are infeasible. According to our code, it prints out:

Start fails ['closed loop constraint']
Goal fails ['closed loop constraint']

This means that the start configuration and goal configuration do not satisfy our constraints on motion planning. To solve this problem, we need to set the robot configuration to the start configuration after we call IK solver and before we make motion planning space, which likes that:

qgoal = solve_ik(link, localpos, position)
    robot.setConfig(qstart)
    obj1 = ik.fixed_rotation_objective(robot.link(6), world_axis=[1, 0, 0])
    space = robotplanning.makeSpace(world, robot, edgeCheckResolution=0.05,
                                    equalityConstraints=[obj1],
                                    equalityTolerance=0.01)

Then, this problem is solved.

blackjack1815 unaccepted post #2 and accepted post #4 as the answer
#5
blackjack1815 unaccepted post #4
#6
blackjack1815 accepted post #2 as the answer
#7