def end_arranging(self):
n_subjoints = len(self.init_spaces)
key_times = set()
for i in range(self.N_FINGERS):
self.events[i].sort()
for t, pos in self.events[i]:
key_times.add(t)
self.intervals[i].sort()
# Replace events with its interpolation
for i in range(len(self.KEY_SUBJOINTS)):
ts = np.array([t for t, _ in self.events[i]], dtype=np.double)
ps = np.array([p for _, p in self.events[i]], dtype=np.double)
self.events[i] = interp1d(ts, ps.T)
key_times = list(key_times)
key_times.sort()
# key_angles[i, j] is the angle of subjoint i at key_times[j]
key_angles = np.zeros((n_subjoints, len(key_times)), dtype=np.double)
solver = self.solver
# progress.info = 'Generating for key frames'
# progress.total = len(key_times)
for j, time in enumerate(key_times):
# Set all targets at this time.
print(j, len(key_times))
solver.clear()
for i in range(len(self.KEY_SUBJOINTS)):
if self.has_event_at(i, time):
solver.set_target_pos(
self.key_subjoint_ids[i], self.events[i](float(time)))
# print(time, self.KEY_SUBJOINTS[i], self.key_subjoint_ids[i],
# self.events[i](time))
# solver.add_constraint_pos_ref(
# solver.get_joint_id('arm_lower_z'),
# 2.,
# self.scene.get_empty_node_pos('elbow_left')
# )
# Solver for all subjoint angles.
with timeit_context('solve'):
try:
solver.solve(max_iteration=30)
except TargetUnreachable:
pass
# solver._dump_state()
# raise
# Store the angles for later interpolation.
for i in range(n_subjoints):
key_angles[i, j] = solver.get_angle(i)
# progress.current = j + 1
# del self.events
# del self.intervals
self.angle_interp = interp1d(
np.array(key_times, dtype=np.double), key_angles,
bounds_error=False, fill_value=key_angles[:, -1])
def test_solve_3_joints(self):
configs = [
# parent_name, name, position, orient, axis, min_angle, max_angle, range_weight
JointConfig(None, "joint-1", (0, 0, 0), (1, 0, 0), (0, 0, -1), -np.pi / 2.0, np.pi / 2, 1.0),
JointConfig("joint-1", "joint-2", (0, 2, 0), (0, 1, 0), (0, 0, 1), -np.pi / 2.0, np.pi / 2, 1.0),
JointConfig("joint-2", "joint-3", (0, 2, 0), (0, 1, 0), (0, 0, 1), 0.0, np.pi / 2, 1.0),
]
solver = self.IKSolverClass(configs)
joint2_id = solver.get_joint_id("joint-2")
joint3_id = solver.get_joint_id("joint-3")
solver.set_target_pos(joint3_id, (2, 2, 0))
with timeit_context("solve"):
solver.solve()
assert np.allclose(solver.get_angle(joint2_id), -np.pi / 2)
