def __init__(self, xml: str, stats: Stats, args: MagicDict):
"""
:param xml: the xml filename for Klampt to read the world settings
:param stats: the Stats object to keep track of stats
:param args: an instance of the input arguments
"""
super().__init__(stats)
import klampt
from klampt.plan import robotplanning
world = klampt.WorldModel()
world.readFile(xml) # very cluttered
robot = world.robot(0)
# this is the CSpace that will be used.
# Standard collision and joint limit constraints will be checked
space = robotplanning.makeSpace(world, robot, edgeCheckResolution=0.1)
self.space = space
self.robot = robot
self.world = world
import copy
self.template_pos = copy.copy(robot.getConfig())
self.template_pos[1:7] = [0] * 6
def compute_initial_final(world, robot):
"""
Initial point is fixed and given, target is set by
1. rotate q0 a random angle between -pi/4, 3*pi/4
2. random perturb ee position by 0.3 m
If IK fails, simply abandon it.
Exit when 10 plans are generated.
"""
end_effector = robot.link(7)
space = makeSpace(world, robot)
# we want to the end effector to move from start to goal
start = [-0.2, -0.50, 1.4]
# solve IK for the start point
unit_vec1 = [0.0, -0.1, 0.0]
null = [0.0, 0.0, 0.0]
unit_vec2 = [0.0, 0., -0.1]
objective = ik.objective(end_effector,
local=[[0, 0, 0], unit_vec1],
world=[start, vo.madd(start, unit_vec2, 1)])
collision_check = lambda: (space.selfCollision(robot.getConfig(
))) == False and (space.envCollision(robot.getConfig()) == False)
ikflag = ik.solve_global(objective,
feasibilityCheck=collision_check,
startRandom=True,
numRestarts=500)
if ikflag == False:
raise Exception("IK for start point failed!")
qstart = robot.getConfig()
print('Feasible ik start ', robot.getConfig())
# then IK for goal
qtmp = qstart[:]
qtmp[1] += np.random.random(
) * np.pi + np.pi / 2 # rotation angle, pi/2 to 3pi/2
robot.setConfig(qtmp)
eepos = np.array(end_effector.getWorldPosition(null))
rand_pos = eepos.copy()
rand_pos[:2] += np.random.random(2) * 0.3 - 0.15
rand_pos[2] += (np.random.random() * 0.8 - 0.4
) # this might be too large, we will see
goal = rand_pos.tolist()
objective = ik.objective(end_effector,
local=[[0, 0, 0], unit_vec1],
world=[goal, vo.madd(goal, unit_vec2, 1)])
ikflag = ik.solve_global(objective,
feasibilityCheck=collision_check,
startRandom=True,
numRestarts=500)
if ikflag == False:
raise Exception("IK for goal point failed!")
print("Feasible ik goal ", robot.getConfig())
qgoal = robot.getConfig()
return np.array(qstart), np.array(qgoal)
def main():
world, robot = load_world()
space = makeSpace(world, robot)
collision_check = lambda: (space.selfCollision(robot.getConfig(
))) == False and (space.envCollision(robot.getConfig()) == False)
# q0, qf = compute_initial_final(world, robot)
q0 = np.array([
0.0, 0.04143683792522413, -1.183867130629673, 2.0680756463305556,
3.745524327531242, 3.6939852943821956e-06, 0.08270468308944955, 0.0
])
qf = np.array([
0.0, 3.590769443639893, -0.8366423979290207, 1.5259988654642873,
5.59380779741848, 0.8157228966102285, 4.712401453217654, 0.0
])
robot.setConfig(q0)
q0 = q0[1:-1]
qf = qf[1:-1]
link_index = [1, 2, 3, 4, 5, 6]
geom_index = link_index
obs_index = [0, 1]
n_grid = 10 # resolution
guess = np.linspace(q0, qf, n_grid)
# guess += np.random.uniform(-0.1, 0.1, size=guess.shape)
vis.add("world", world)
# this one has no additional constraint so it should be easy to solve...
config = TrajOptSettings(cvxpy_args={'solver': 'GUROBI'})
trajopt = KineTrajOpt(world,
robot,
q0,
qf,
config=config,
link_index=link_index,
obs_index=obs_index,
geom_index=geom_index)
rst = trajopt.optimize(guess)
sol = rst['sol']
for i in range(1, n_grid):
robot.setConfig([0] + sol[i].tolist() + [0])
print(collision_check())
vis.add('ghost%d' % i, [0] + list(sol[i]) + [0])
vis.setColor('ghost%d' % i, 0, 1, 0, 0.5)
vis.spin(float('inf'))
def feasible_plan(world, robot, qtarget):
"""Plans for some number of iterations from the robot's current configuration to
configuration qtarget. Returns the first path found.
Returns None if no path was found, otherwise returns the plan.
"""
t0 = time.time()
moving_joints = [1, 2, 3, 4, 5, 6, 7]
space = robotplanning.makeSpace(world=world,
robot=robot,
edgeCheckResolution=1e-2,
movingSubset=moving_joints)
plan = MotionPlan(space, type='prm')
#TODO: maybe you should use planToConfig?
numIters = 0
t1 = time.time()
print("Planning time,", numIters, "iterations", t1 - t0)
#to be nice to the C++ module, do this to free up memory
plan.space.close()
plan.close()
return path
for i in range(world.numTerrains()):
vis.add("terrain" + str(i), world.terrain(i))
#if you want to just see the robot in a pop up window...
if DO_SIMPLIFY and DEBUG_SIMPLIFY:
print("#########################################")
print("Showing the simplified robot")
print("#########################################")
vis.setWindowTitle("Simplified robot")
vis.dialog()
#Automatic construction of space
if not CLOSED_LOOP_TEST:
if not MANUAL_SPACE_CREATION:
space = robotplanning.makeSpace(world=world,
robot=robot,
edgeCheckResolution=1e-3,
movingSubset='all')
else:
#Manual construction of space
collider = WorldCollider(world)
space = robotcspace.RobotCSpace(robot, collider)
space.eps = 1e-3
space.setup()
else:
#TESTING: closed loop robot cspace
collider = WorldCollider(world)
obj = ik.objective(robot.link(robot.numLinks() - 1),
local=[0, 0, 0],
world=[0.5, 0, 0.5])
vis.add("IK goal", obj)
vis.dialog()
from klampt.math import vectorops as vo
from klampt import Simulator, SimRobotController
def print_red(*args):
print(*args)
world = WorldModel()
create.primitives.sphere(0.1, [-1, -1, 0], world=world, name='origin')
create.primitives.sphere(0.1, [0, -1, 0], world=world, name='X')
create.primitives.sphere(0.1, [-1, 1, 0], world=world, name='Y')
fn = "./ur5_plate.xml"
res = world.readFile(fn)
robot = world.robot(0)
space = makeSpace(world, robot)
path_data = np.load('../data/up_rrt_plans.npz')
plan = path_data['plan0']
q0 = plan[0]
class MyGLViewer(GLRealtimeProgram):
def __init__(self, world, sim):
GLRealtimeProgram.__init__(self, "GL test")
self.world = world
self.sim = sim
self.controller = self.sim.controller(0)
print('gains ', self.controller.getPIDGains())
self.goal = None
self.store_goal = None
from klampt import *
from klampt.plan import robotplanning,cspace
from klampt.io import resource
import time
world = WorldModel()
world.readFile("/home/motion/Klampt-examples/data/tx90cuptable.xml")
robot = world.robot(0)
#this is the CSpace that will be used. Standard collision and joint limit constraints
#will be checked
space = robotplanning.makeSpace(world,robot,edgeCheckResolution=0.05)
#fire up a visual editor to get some start and goal configurations
qstart = robot.getConfig()
qgoal = robot.getConfig()
save,qstart = resource.edit("Start config",qstart,"Config",world=world)
#it's worthwile to make sure that it's feasible
while save and not space.feasible(qstart):
print "Start configuration isn't feasible, please pick one that is collision-free"
save,qstart = resource.edit("Start config",qstart,"Config",world=world)
save,qgoal = resource.edit("Goal config",qgoal,"Config",world=world)
while save and not space.feasible(qgoal):
print "Goal configuration isn't feasible, please pick one that is collision-free"
save,qgoal = resource.edit("Goal config",qgoal,"Config",world=world)
settings = {'type':'rrt',
'perturbationRadius':0.25,
'bidirectional':True,
'shortcut':True,