def sampling_9(serial_num):
world_name = "flatworld"
world_file = "../../data/robot_model_files/worlds/" + world_name + ".xml"
robot_file = "../../data/robot_model_files/robots/irb120_icp.rob"
world = get_world(world_file, robot_file, visualize_robot=True)
robot = world.robot(0)
est_config_list = cal_est_config_list(robot, serial_num)
path = collision_checking(world, est_config_list, 0.001)
if path is False:
sys.exit("The calibration path exist self-collision.")
else:
print("The calibration path is collision free.")
vis.clear()
vis.add("world", world)
for i in range(len(est_config_list)):
vis.add("estimated configure" + str(i), est_config_list[i])
vis.setColor("estimated configure" + str(i), 1.0, 0.0, 0.1 * i, 0.5)
vis.spin(float('inf'))
df = pd.DataFrame(np.asarray(est_config_list)[:, 7:13] * (180. / np.pi))
df.to_csv("../../data/robot_configuration_files/configuration_record_" + serial_num + ".csv",
header=False, index=False)
time.sleep(1.)
est_config_list = est_config_list[0:]
controller = SampleController(est_config_list, robot, '192.168.1.178')
controller.start()
def main():
world, robot, link_index, geom_index, obs_index = create_world()
q0 = [0, -2, 0]
qf = [3, 1, 3.14]
config = TrajOptSettings(cvxpy_args={'solver': 'GUROBI'},
limit_joint=False)
trajopt = KineTrajOpt(world,
robot,
q0=q0,
qf=qf,
config=config,
link_index=link_index,
geom_index=geom_index,
obs_index=obs_index)
# create guess and solve
n_grid = 10
guess = np.linspace(q0, qf, n_grid)
rst = trajopt.optimize(guess)
print(rst)
traj = rst['sol']
# show the results
vis.add('world', world)
robot.setConfig(traj[-1])
for i, qi in enumerate(traj[:-1]):
name = 'ghost%d' % i
vis.add(name, qi)
vis.setAttribute(name, 'type', 'Config')
vis.setColor(name, 0, 1, 0, 0.4)
vis.spin(100)
def qt_template(world):
"""Runs a custom Qt frame around a visualization window"""
if not glinit.available('PyQt5'):
print(
"PyQt5 is not available on your system, try sudo apt-get install python-qt5"
)
return
#Qt objects must be explicitly deleted for some reason in PyQt5...
g_mainwindow = None
#All Qt functions must be called in the vis thread.
#To hook into that thread, you will need to pass a window creation function into vis.customUI.
def makefunc(gl_backend):
global g_mainwindow
g_mainwindow = MyQtMainWindow(gl_backend)
return g_mainwindow
vis.customUI(makefunc)
vis.add("world", world)
vis.setWindowTitle("Klamp't Qt test")
vis.spin(float('inf'))
vis.kill()
#Safe cleanup of all Qt objects created in makefunc.
#If you don't have this, PyQt5 complains about object destructors being called from the wrong thread
del g_mainwindow
def viewport_template(world):
"""Changes the parameters of the viewport and main window"""
#add the world to the visualizer
vis.add("world",world)
vp = vis.getViewport()
vp.w,vp.h = 800,800
vis.setViewport(vp)
#this auto-sizes the camera
vis.autoFitCamera()
vis.setWindowTitle("Viewport modification test")
vis.spin(float('inf'))
vis.kill()
def main():
world, robot, link_index, geom_index, obs_index = create_world()
# get start and target for the joints
joint_start = [
0.115, 0.6648, -0.3526, 1.6763, -1.9242, 2.9209, -1.2166, 1.3425,
-0.6365, 0.0981, -1.226, -2.0264, -3.0125, -1.3958, -1.9289, 2.9295,
0.5748, -3.137
]
joint_target = [
0.115, 0.6808, -0.3535, 1.4343, -1.8516, 2.7542, -1.2005, 1.5994,
-0.6929, -0.3338, -1.292, -1.9048, -2.6915, -1.2908, -1.7152, 1.3155,
0.6877, -0.0041
]
assert (len(link_index) == len(joint_start) == len(joint_target))
# generate guess
n_grid = 10 # resolution
guess = np.linspace(joint_start, joint_target, n_grid)
# create trajopt instance
config = TrajOptSettings(cvxpy_args={'solver': 'GUROBI'},
limit_joint=False)
trajopt = KineTrajOpt(world,
robot,
joint_start,
joint_target,
config=config,
link_index=link_index,
geom_index=geom_index,
obs_index=obs_index)
# create some stuff for visualization
config = np.array(robot.getConfig())
def update_config(q):
config[link_index] = q
def get_config(q):
cq = copy.copy(config)
cq[link_index] = q
return cq
rst = trajopt.optimize(guess)
sol = rst['sol']
vis.add('world', world)
dists = []
for i in range(1, n_grid -
1): # set to -2 so the robot stops there with collision
vis.add('ghost%d' % i, list(get_config(sol[i])))
vis.setColor('ghost%d' % i, 0, 1, 0, 0.3)
robot.setConfig(get_config(sol[i]))
update_config(sol[-1])
robot.setConfig(config)
vis.spin(float('inf'))
def main():
world, robot, link_index, geom_index, obs_index = create_world()
joint_start = [-1.832, -0.332, -1.011, -1.437, -1.1, -1.926, 3.074]
joint_target = [0.062, 1.287, 0.1, -1.554, -3.011, -0.268, 2.988]
# compute the goal pose
config0 = robot.getConfig()
for lid, theta in zip(link_index, joint_target):
config0[lid] = theta
robot.setConfig(config0)
tR, tt = robot.link(link_index[-1]).getTransform()
print(f'target is {tR}, {tt}')
# create the trajopt instance
config = TrajOptSettings(cvxpy_args={'solver': 'GUROBI'},
limit_joint=False)
trajopt = KineTrajOpt(world,
robot,
q0=joint_start,
qf=None,
config=config,
link_index=link_index,
geom_index=geom_index,
obs_index=obs_index)
FINAL_POSE = True
if FINAL_POSE:
trajopt.add_pose_constraint(-1, link_index[-1], (tR, tt))
# add keep constraint
trajopt.add_orientation_constraint(None, link_index[-1], tR)
# generate guess
n_grid = 10 # resolution
guess = np.linspace(joint_start, joint_target, n_grid)
# create some stuff for visualization
config = np.array(robot.getConfig())
def update_config(q):
config[link_index] = q
def get_config(q):
cq = copy.copy(config)
cq[link_index] = q
return cq
rst = trajopt.optimize(guess)
sol = rst['sol']
vis.add('world', world)
for i in range(1, n_grid -
2): # set to -2 so the robot stops there with collision
vis.add('ghost%d' % i, list(get_config(sol[i])))
vis.setColor('ghost%d' % i, 0, 1, 0, 0.5)
update_config(sol[-1])
vis.spin(float('inf'))
def vis_interaction_test(world):
"""Run some tests of visualization module interacting with the resource module"""
print("Showing robot in modal dialog box")
vis.add("robot", world.robot(0))
vis.add("ee", world.robot(0).link(11).getTransform())
vis.dialog()
config = resource.get("resourcetest1.config",
description="Should show up without a hitch...",
doedit=True,
editor='visual',
world=world)
import time
if MULTITHREADED:
print(
"Showing threaded visualization (this will fail on GLUT or Mac OS)"
)
vis.show()
for i in range(3):
vis.lock()
q = world.robot(0).getConfig()
q[9] = 3.0
world.robot(0).setConfig(q)
vis.unlock()
time.sleep(1.0)
if not vis.shown():
break
vis.lock()
q = world.robot(0).getConfig()
q[9] = -1.0
world.robot(0).setConfig(q)
vis.unlock()
time.sleep(1.0)
if not vis.shown():
break
print("Hiding visualization window")
vis.show(False)
else:
print("Showing single-threaded visualization for 5s")
vis.spin(5.0)
config = resource.get("resourcetest1.config",
description="Should show up without a hitch...",
doedit=True,
editor='visual',
world=world)
def multiwindow_template(world):
"""Tests multiple windows and views."""
vis.add("world", world)
vp = vis.getViewport()
vp.w, vp.h = 400, 600
vis.setViewport(vp)
vis.addText("label1", "This is Window 1", (20, 20))
vis.setWindowTitle("Window 1")
vis.show()
id1 = vis.getWindow()
print("First window ID:", id1)
id2 = vis.createWindow("Window 2")
vis.add("Lone point", [0, 0, 0])
vis.setViewport(vp)
vis.addText("label1", "This is Window 2", (20, 20))
print("Second window ID:", vis.getWindow())
vis.setWindow(id2)
vis.spin(float('inf'))
#restore back to 1 window, clear the text
vis.setWindow(id1)
vis.clearText()
vp = vis.getViewport()
vp.w, vp.h = 800, 800
vis.setViewport(vp)
vis.setWindowTitle("vis.spin test: will close in 5 seconds...")
vis.spin(5.0)
#Now testing ability to re-launch windows
vis.setWindowTitle("Shown again. Close me to proceed.")
vis.spin(float('inf'))
vis.setWindowTitle("Dialog test. Close me to proceed.")
vp = vis.getViewport()
vp.w, vp.h = 400, 600
vis.setViewport(vp)
vis.dialog()
vp.w, vp.h = 640, 480
vis.setViewport(vp)
for i in range(3):
widgets = GLWidgetPlugin()
widgets.addWidget(RobotPoser(world.robot(0)))
vis.addPlugin(widgets)
vis.setWindowTitle("Split screen test")
vis.spin(float('inf'))
vis.setPlugin(None)
vis.setWindowTitle("Back to normal. Close me to quit.")
vis.dialog()
vis.kill()
def visualize(world, robot, path=None, start=None, goal=None):
"""Visualize a path"""
# visualize start/goal as spheres if exist
r = 0.04
if start != None:
sphere = create.primitives.sphere(r, start, world=world, name='start')
# vis.add('start', sphere)
if goal != None:
create.primitives.sphere(r, goal, world=world, name='goal')
vis.add("world", world)
# animate path if exist
if path != None:
traj = trajectory.RobotTrajectory(robot, range(len(path)), path)
vis.animate(("world", robot.getName()), path, speed=0.5)
vis.add("trajectory", traj)
vis.spin(float('inf'))
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 main():
world, robot, link_index, geom_index, obs_index = create_world()
joint_start = [-1.832, -0.332, -1.011, -1.437, -1.1, -1.926, 3.074]
joint_target = [0.062, 1.287, 0.1, -1.554, -3.011, -0.268, 2.988]
# generate guess
n_grid = 10 # resolution
guess = np.linspace(joint_start, joint_target, n_grid)
# create trajopt instance
config = TrajOptSettings(cvxpy_args={'solver': 'GUROBI'},
limit_joint=False)
trajopt = KineTrajOpt(world,
robot,
joint_start,
joint_target,
config=config,
link_index=link_index,
geom_index=geom_index,
obs_index=obs_index)
# create some stuff for visualization
config = np.array(robot.getConfig())
def update_config(q):
config[link_index] = q
def get_config(q):
cq = copy.copy(config)
cq[link_index] = q
return cq
rst = trajopt.optimize(guess)
sol = rst['sol']
vis.add('world', world)
for i in range(1, n_grid -
2): # set to -2 so the robot stops there with collision
vis.add('ghost%d' % i, list(get_config(sol[i])))
vis.setColor('ghost%d' % i, 0, 1, 0, 0.5)
# dists = compute_distance(world, robot, link_index, obs_index)
# print(dists)
update_config(sol[-1])
vis.spin(float('inf'))
def multiwindow_template(world):
"""Tests multiple windows and views."""
vis.add("world",world)
vp = vis.getViewport()
vp.w,vp.h = 800,800
vis.setViewport(vp)
vis.setWindowTitle("vis.spin test: will close in 5 seconds...")
vis.spin(5.0)
#Now testing ability to re-launch windows
vis.setWindowTitle("Shown again. Close me to proceed.")
vis.spin(float('inf'))
vis.setWindowTitle("Dialog test. Close me to proceed.")
vp = vis.getViewport()
vp.w,vp.h = 400,600
vis.setViewport(vp)
vis.dialog()
vp.w,vp.h = 640,480
vis.setViewport(vp)
for i in range(3):
widgets = GLWidgetPlugin()
widgets.addWidget(RobotPoser(world.robot(0)))
vis.addPlugin(widgets)
vis.setWindowTitle("Split screen test")
vis.spin(float('inf'))
vis.setPlugin(None)
vis.setWindowTitle("Back to normal. Close me to quit.")
vis.dialog()
vis.kill()
print(plan.space.cspace.feasibilityQueryOrder())
print("Plan stats:")
print(plan.getStats())
print("CSpace stats:")
print(plan.space.getStats())
#to be nice to the C++ module, do this to free up memory
plan.space.close()
plan.close()
if len(wholepath) > 1:
#print "Path:"
#for q in wholepath:
# print " ",q
#if you want to save the path to disk, uncomment the following line
#wholepath.save("test.path")
#draw the path as a RobotTrajectory (you could just animate wholepath, but for robots with non-standard joints
#the results will often look odd). Animate with 5-second duration
times = [i * 5.0 / (len(wholepath) - 1) for i in range(len(wholepath))]
traj = trajectory.RobotTrajectory(robot, times=times, milestones=wholepath)
#show the path in the visualizer, repeating for 60 seconds
vis.animate("robot", traj)
vis.spin(60)
else:
print("Failed to generate a plan")
vis.kill()
glColor3f(0, 0, 0)
glBegin(GL_POINTS)
for pt in self.pc:
if len(pt) == 6:
glColor3f(*pt[3:6])
if read_local:
glVertex3fv(se3.apply(T, pt[0:3]))
else:
glVertex3fv(pt[0:3])
glEnd()
#print "Draw PC time",time.time()-t0
return True
vis.pushPlugin(SensorTestWorld())
vis.spin(float('inf'))
vis.kill()
"""
#Note: GLEW doesn't work outside of the main thread, hence the use of the GLPluginInterface.
#The below code falls back to the non-accelerated sensor simulation
vis.show()
time.sleep(0.5)
for sample in range(10):
vis.lock()
robot.randomizeConfig()
#sensor.kinematicSimulate(world,0.01)
sim.controller(0).setPIDCommand(robot.getConfig(),[0.0]*7)
vis.unlock()
for i in range(100):
#obj.geometry().loadFile(OBJECT_DIR+"ycb-select/011_banana/nontextured.ply");
#obj.geometry().loadFile(OBJECT_DIR+"ycb-select/021_bleach_cleanser/nontextured.ply")
obj.geometry().loadFile(OBJECT_DIR + "ycb-select/048_hammer/nontextured.ply")
#obj.geometry().loadFile(OBJECT_DIR+"cube.off"); obj.geometry().scale(0.2)
#weird bug in Qhull -- cylinder can't be converted to ConvexHull
#obj.geometry().loadFile(OBJECT_DIR+"cylinder.off")
#this will perform a reasonable center of mass / inertia estimate
m = obj.getMass()
m.estimate(obj.geometry(), mass=0.454, surfaceFraction=0.2)
obj.setMass(m)
obj.appearance().setColor(0.3, 0.3, 0.3, 1)
#debugging the stable faces
sides = stable_faces(obj, stability_tol=0.0, merge_tol=0.05)
vis.add("world", world)
vis.setBackgroundColor(1, 1, 1)
vis.add("COM", m.getCom(), color=(1, 0, 0, 1), size=5, hide_label=True)
for i, f in enumerate(sides):
gf = GeometricPrimitive()
gf.setPolygon(np.stack(f).flatten())
color = (0.5 + 0.5 * random.random(), 0.5 + 0.5 * random.random(),
0.5 + 0.5 * random.random(), 0.5)
vis.add("face{}".format(i), Geometry3D(gf), color=color, hide_label=True)
vis.show()
vis.spin(float("inf"))
def main():
print("""
===============================================================================
A program to quickly browse Klamp't objects.
USAGE: %s [item1 item2 ...]
where the given items are world, robot, terrain, object, or geometry files. Run
it without arguments
%s
for an empty reference world. You may add items to the reference world using
the `Add to World` button. If you know what items to use in the reference
world, run it with
%s world.xml
or
%s item1 item2 ...
where the items are world, robot, terrain, object, or geometry files.
===============================================================================
""" % (sys.argv[0], sys.argv[0], sys.argv[0], sys.argv[0]))
#must be explicitly deleted for some reason in PyQt5...
g_browser = None
def makefunc(gl_backend):
global g_browser
browser = ResourceBrowser(gl_backend)
g_browser = browser
dw = QtWidgets.QDesktopWidget()
x = dw.width() * 0.8
y = dw.height() * 0.8
browser.setFixedSize(x, y)
for fn in sys.argv[1:]:
res = browser.world.readFile(fn)
if not res:
print("Unable to load model", fn)
print("Quitting...")
sys.exit(1)
print("Added", fn, "to world")
if len(sys.argv) > 1:
browser.emptyVisPlugin.add("world", browser.world)
return browser
vis.customUI(makefunc)
vis.show()
vis.setWindowTitle("Klamp't Resource Browser")
vis.spin(float('inf'))
vis.kill()
del g_browser
return
#this code below is incorrect...
app = QtWidgets.QApplication(sys.argv)
browser = ResourceBrowser()
for fn in sys.argv[1:]:
res = browser.world.readFile(fn)
if not res:
print("Unable to load model", fn)
print("Quitting...")
sys.exit(1)
print("Added", fn, "to world")
if len(sys.argv) > 1:
browser.emptyVisPlugin.add("world", browser.world)
dw = QtWidgets.QDesktopWidget()
x = dw.width() * 0.8
y = dw.height() * 0.8
browser.setFixedSize(x, y)
#browser.splitter.setWindowState(QtCore.Qt.WindowMaximized)
browser.setWindowTitle("Klamp't Resource Browser")
browser.show()
# Start the main loop.
res = app.exec_()
return res
#print some interpolated points
print 0.5,":",traj.eval(0.5)
print 2.5,":",traj.eval(2.5)
#print some stuff after the end of trajectory
print 7,":",traj.eval(6)
print 100.3,":",traj.eval(100.3)
print -2,":",traj.eval(-2)
from klampt import vis
vis.add("point",[0,0,0])
vis.animate("point",traj)
vis.add("traj",traj)
#vis.spin(float('inf')) #show the window until you close it
traj2 = trajectory.HermiteTrajectory()
traj2.makeSpline(traj)
vis.animate("point",traj2)
vis.hide("traj")
vis.add("traj2",traj2.configTrajectory().discretize(0.1))
#vis.spin(float('inf'))
traj_timed = trajectory.path_to_trajectory(traj,vmax=2,amax=4)
#next, try this line instead
#traj_timed = trajectory.path_to_trajectory(traj,timing='sqrt-L2',speed='limited',vmax=2,amax=4)
#or this line
#traj_timed = trajectory.path_to_trajectory(traj2.configTrajectory().discretize(0.1),timing='sqrt-L2',speed=0.3)
vis.animate("point",traj_timed)
vis.spin(float('inf'))
def mousefunc(self, button, state, x, y):
print("Mouse button", button, "state", state, "at point", x, y)
def motionfunc(self, x, y, dx, dy):
if 'shift' in self.modifiers():
self.q[2] = float(y) / 400
self.q[3] = float(x) / 400
self.world.robot(0).setConfig(self.q)
return True
return False
if __name__ == "__main__":
print("""================================================================
mouse_capture.py: A simple program where the mouse motion, when
shift-clicking, gets translated into joint values for an animated robot.
========================================================================
""")
world = klampt.WorldModel()
res = world.readFile("../../data/tx90blocks.xml")
if not res:
raise RuntimeError("Unable to load world")
plugin = MouseCapture(world)
vis.add("world", world)
vis.pushPlugin(plugin)
vis.setWindowTitle("mouse_capture.py")
vis.spin(float('inf')) #shows the window
vis.kill()
def optimizeGrasping(robot,
objs,
init_config,
gridres,
pcres,
score_oracle=ORACLE,
collision_links=None):
print(f"Oracle used: {score_oracle}")
robot.setConfig(init_config)
robot_geometry = RobotKinematicsCache(robot, gridres, pcres)
object_geometry = PenetrationDepthGeometry(objs[0].geometry(), gridres,
pcres)
q_init = robot.getConfig()
grasping_problem = SIPCCProblem()
grasping_problem.dim_z = 7
grasping_problem.z_proj = np.array([0, 0, 0, 0, 0, 0, 1])
grasping_problem.z_lb = np.array([0, 0, 0, 0, 0, 0, 0])
grasping_problem.domain = LinkAndGeometryDomain()
grasping_problem.set_objective(ConstantObjectiveFunction(0))
grasping_problem.set_complementarity(
RobotCollisionConstraint(robot, object_geometry, gridres, pcres,
robot_geometry, collision_links))
grasping_problem.add_ineq(FrictionConstraint())
grasping_problem.add_eq(Equilibrium3DConstraint(objs[0], object_geometry))
grasping_problem.colliding_links = collision_links
if score_oracle == 'LSO':
grasping_problem.oracle = local_score_oracle
elif score_oracle == 'MVO':
grasping_problem.oracle = maximum_violation_oracle
elif score_oracle == 'GSO':
grasping_problem.oracle = global_score_oracle
else:
raise ValueError("score_oracle must be LSO, GSO, or MVO")
x_init = q_init
x_lb = grasping_problem.complementarity.robot.robot.getJointLimits()[0]
x_ub = grasping_problem.complementarity.robot.robot.getJointLimits()[1]
dim_z = 7
from klampt import vis
wdisplay = WorldModel()
wdisplay.add('robot', robot)
display_robot = wdisplay.robot(0)
for i, o in enumerate(objs):
wdisplay.add('obj_{}'.format(i), o)
def update_robot_viz(x, y, z):
vis.clear()
vis.add("robot", display_robot)
for i, o in enumerate(objs):
odisplay = wdisplay.rigidObject(i)
vis.add("obj_{}".format(i), odisplay)
try:
display_robot.setConfig(x)
for i, yi in enumerate(y):
vis.add("pt_{}".format(i),
yi[1],
color=(1, 0, 0, 1),
hide_label=True)
f_normal = z[i * 7 + 6]
f_friction = z[i * 7:i * 7 + 6]
n = np.array(normal(grasping_problem.xyz_eq.env_geom, yi[1]))
f = n * f_normal
n1 = so3.canonical(n)[3:6]
n2 = so3.canonical(n)[6:9]
for j in range(6):
n_tmp = (math.cos(
(math.pi / 3) * j) * np.array(n1) + math.sin(
(math.pi / 3) * j) * np.array(n2))
f += n_tmp * f_friction[j]
# f is the force pointing inward
vis.add(
"n_{}".format(i),
Trajectory([0, 1],
[yi[1], vectorops.madd(yi[1], n, 0.05)]),
color=(1, 1, 0, 1),
hide_label=True)
vis.add(
"f_{}".format(i),
Trajectory(
[0, 1],
[yi[1], vectorops.madd(yi[1], f.tolist(), -1.0)]),
color=(1, 0.5, 0, 1),
hide_label=True)
finally:
pass
time.sleep(0.01)
vis.show()
settings = SIPCCOptimizationSettings()
settings.callback = lambda *args: vis.threadCall(lambda: update_robot_viz(
*args))
settings.min_index_point_distance = MINIMUM_INDEX_DISTANCE
t_start = time.time()
res = optimizeSIPCC(grasping_problem,
x_init,
x_lb,
x_ub,
settings=settings)
t_end = time.time()
res.time_opt = t_end - t_start
vis.spin(float('inf'))
res.number_of_points = object_geometry.pcdata.numPoints()
return res
def optimization(self):
"""
:return: None
"""
time_stamp = time.time()
min_error = 10e5
for k in range(25):
random.seed(time.time())
est_input = [random.uniform(-2 * np.pi, 2 * np.pi) for i in range(3)] + \
[random.uniform(-0.8, 0.8) for i in range(3)] + \
[random.uniform(-2 * np.pi, 2 * np.pi) for i in range(3)] + \
[random.uniform(-0.2, 0.2) for i in range(3)]
print("We will start " + str(k + 1) + "th minimize!")
res = root(opt_error_fun,
np.array(est_input),
args=(self.Tlink_set, self.trans_list),
method='lm',
options={})
print("The reason for termination: \n", res.message,
"\nAnd the number of the iterations are: ", res.nfev)
error = np.sum(np.absolute(res.fun))
print("The minimize is end, and the error is: ", error)
if error <= min_error:
min_error = error
self.min_res = res
print("The optimization uses: ", time.time() - time_stamp, "seconds")
print("The error is: ", np.sum(np.absolute(self.min_res.fun)))
print("The optimized T_oct is: ",
(self.min_res.x[0:3], self.min_res.x[3:6]))
print(
"And the matrix form is: \n",
np.array(
se3.homogeneous(
(so3.from_rpy(self.min_res.x[0:3]), self.min_res.x[3:6]))))
print("The optimized T_needle_end is: ",
(self.min_res.x[6:9], self.min_res.x[9:12]))
print(
"And the matrix form is: \n",
np.array(
se3.homogeneous((so3.from_rpy(self.min_res.x[6:9]),
self.min_res.x[9:12]))))
# os.makedirs("../../data/suture_experiment/calibration_result_files/" + self.serial_num + "/", exist_ok=True)
# np.save("../../data/suture_experiment/calibration_result_files/" + self.serial_num +
# "/calibration_result.npy", self.min_res.x, allow_pickle=True)
vis.spin(float('inf'))
vis.clear()
self.robot.setConfig(self.qstart)
vis.add("world", self.world)
World_base = model.coordinates.Frame("Base Frame",
worldCoordinates=(so3.from_rpy(
[0, 0, 0]), [0, 0, 0]))
vis.add('World_base', World_base)
est_OCT_coordinate = model.coordinates.Frame(
"est OCT Frame",
worldCoordinates=(so3.from_rpy(self.min_res.x[0:3]),
self.min_res.x[3:6]))
vis.add("est OCT Frame", est_OCT_coordinate)
Tlink_6 = model.coordinates.Frame(
"Link 6 Frame",
worldCoordinates=self.robot.link('link_6').getTransform())
vis.add('Link 6 Frame', Tlink_6)
est_Tneedle_world = se3.mul(
self.robot.link('link_6').getTransform(),
(so3.from_rpy(self.min_res.x[6:9]), self.min_res.x[9:12]))
est_needle_coordinate = model.coordinates.Frame(
"est needle Frame", worldCoordinates=est_Tneedle_world)
vis.add("est needle Frame", est_needle_coordinate)
vis.spin(float('inf'))
"""
Try to load the urdf of pr2.
"""
from klampt import WorldModel, vis
world = WorldModel()
world.readFile('pr2.urdf')
vis.add('world', world)
vis.spin(100)
