def solve_ik(robotlink, localpos, worldpos, index):
"""
:param robotlink: {Klampt.RobotModelLink} -- the Klampt robot link model
:param localpos: {list} -- the list of points in the robot link frame
:param worldpos: {list} -- the list of points in the world frame
:param index: {int} -- the index number
:return: robot.getConfig(): {list} -- the list of the robot configuration
"""
robot = robotlink.robot()
space = robotcspace.RobotCSpace(robot)
obj = ik.objective(robotlink, local=localpos, world=worldpos)
maxIters = 100
tol = 1e-8
for i in range(1000):
s = ik.solver(obj, maxIters, tol)
res = s.solve()
if res and not space.selfCollision() and not space.envCollision():
return robot.getConfig()
else:
print(
"Couldn't solve IK problem in " + str(index) +
"th. Or the robot exists self-collision or environment collision."
)
s.sampleInitial()
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()
space = robotcspace.ClosedLoopRobotCSpace(robot, obj, collider)
space.eps = 1e-3
space.setup()
#Generate some waypoint configurations using the resource editor