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.4K
- likes 0