def test_controller_Y(self):
window = IKViewer()
window.load_scene(get_resource_path("models", "segments-y.dae"))
pi = np.pi
angle_ranges = {
"joint-0": (0, pi / 2),
"joint-1": (0, pi / 2),
"joint-l-0": (0, pi / 2),
"joint-l-1": (0, pi / 2),
"joint-r-0": (0, pi / 2),
"joint-r-1": (0, pi / 2),
}
controller1 = Controller(window.scene, "Cube", angle_ranges)
target = window.scene.get_model("Icosphere")
targetPos = target.matrix[0:3, 3] / target.matrix[3, 3]
controller1.solver.set_target_pos_by_name("joint-l-2", targetPos)
target = window.scene.get_model("Icosphere_001")
targetPos = target.matrix[0:3, 3] / target.matrix[3, 3]
controller1.solver.set_target_pos_by_name("joint-r-2", targetPos)
gen = controller1.solver.solve()
def step(dt):
try:
next(gen)
except (StopIteration, TargetUnreachable) as e:
pyglet.clock.unschedule(step)
print(e)
controller1.update_model_joints()
pyglet.clock.schedule_interval(step, 0.1)
# window.add_controller(controller1)
window.start()
def test_controller_hand(self):
window = IKViewer()
window.load_scene(get_resource_path("models", "hand3.dae"))
angle_ranges = {}
for finger in "01234":
section = "000"
name = "finger_{}_{}".format(finger, section)
angle_ranges[name] = (-np.pi / 6, np.pi / 6)
for section in ("001", "002", "003"):
name = "finger_{}_{}".format(finger, section)
angle_ranges[name] = (0, np.pi / 1.5)
# angle_ranges = {'joint_{:03d}'.format(i): (-np.pi / 4, np.pi / 4)
# for i in range(1, 20)}
controller1 = Controller(window.scene, "hand", angle_ranges)
#
target = window.scene.get_model("target")
targetPos = target.matrix[0:3, 3] / target.matrix[3, 3]
controller1.solver.set_target_pos_by_name("finger_1_tip", targetPos)
#
target = window.scene.get_model("target_001")
targetPos = target.matrix[0:3, 3] / target.matrix[3, 3]
controller1.solver.set_target_pos_by_name("finger_2_tip", targetPos)
#
target = window.scene.get_model("target_002")
targetPos = target.matrix[0:3, 3] / target.matrix[3, 3]
controller1.solver.set_target_pos_by_name("finger_3_tip", targetPos)
#
target = window.scene.get_model("target_003")
targetPos = target.matrix[0:3, 3] / target.matrix[3, 3]
controller1.solver.set_target_pos_by_name("finger_0_tip", targetPos)
gen = controller1.solver.solve()
def step(dt):
try:
next(gen)
except (StopIteration, TargetUnreachable) as e:
pyglet.clock.unschedule(step)
print(e)
controller1.update_model_joints()
print(dt)
pyglet.clock.schedule_interval(step, 0.02)
# window.add_controller(controller1)
window.start()
def test_controller(self):
window = IKViewer()
window.load_scene(get_resource_path("models", "segments.dae"))
controller1 = Controller(window.scene, "pipe-1")
target = window.scene.get_model("Icosphere")
targetPos = target.matrix[0:3, 3] / target.matrix[3, 3]
controller1.solver.set_target_pos_by_name("joint-3", targetPos)
gen = controller1.solver.solve()
def step(dt):
try:
next(gen)
except (StopIteration, TargetUnreachable) as e:
pyglet.clock.unschedule(step)
print(e)
controller1.update_model_joints()
pyglet.clock.schedule_interval(step, 0.1)
# window.add_controller(controller1)
window.start()
def test_controller_long(self):
window = IKViewer()
window.load_scene(get_resource_path("models", "segments-long.dae"))
angle_ranges = {"joint_{:03d}".format(i): (-np.pi / 4, np.pi / 4) for i in range(1, 20)}
controller1 = Controller(window.scene, "pipe-1", angle_ranges)
target = window.scene.get_model("target")
targetPos = target.matrix[0:3, 3] / target.matrix[3, 3]
controller1.solver.set_target_pos_by_name("tip", targetPos)
# with timeit_context('solve'):
gen = controller1.solver.solve()
def step(dt):
try:
next(gen)
except (StopIteration, TargetUnreachable) as e:
pyglet.clock.unschedule(step)
print(e)
controller1.update_model_joints()
print(dt)
pyglet.clock.schedule_interval(step, 0.2)
# window.add_controller(controller1)
window.start()
