Example #1
0
    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])
Example #2
0
 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)