def _take_steps(self, is_swing_leg_left, initial_position, lift_up_front_position, lift_up_front_forward_position, put_down_position): lift_up_front_trajectory_section, lift_up_front_forward_trajectory_section, put_down_trajectory_section\ = self._get_3_walking_trajectory_sections(initial_position, lift_up_front_position, lift_up_front_forward_position, put_down_position) while True: next_count = self.timer.next_count() time_fraction = next_count / self.step_timer.max_count interpolated_position = bezier_curves.get_interpolated_position(time_fraction, lift_up_front_trajectory_section, lift_up_front_forward_trajectory_section, put_down_trajectory_section) self.calculate_and_actuate_lower_limps_angle_set_at_good_timing(is_swing_leg_left=is_swing_leg_left, interpolated_position=interpolated_position, time_fraction=time_fraction) if self.timer.is_done(): sleep(self.duration_of_rest) self.timer.restart() self.set_current_walking_set_index_to_next() is_swing_leg_left, initial_position, lift_up_front_position, lift_up_front_forward_position, put_down_position\ = self._get_is_swing_leg_left_and_walk_trajectory_four_positions__with_current_working_set(self.current_walking_set_index) lift_up_front_trajectory_section, lift_up_front_forward_trajectory_section, put_down_trajectory_section\ = self._get_3_walking_trajectory_sections(initial_position, lift_up_front_position, lift_up_front_forward_position, put_down_position)
def _take_steps(self, is_swing_leg_left, initial_position, lift_up_front_position, lift_up_front_forward_position, put_down_position): lift_up_front_trajectory_section, lift_up_front_forward_trajectory_section, put_down_trajectory_section\ = self._get_3_walking_trajectory_sections(initial_position, lift_up_front_position, lift_up_front_forward_position, put_down_position) while True: next_count = self.timer.next_count() time_fraction = next_count / self.step_timer.max_count interpolated_position = bezier_curves.get_interpolated_position( time_fraction, lift_up_front_trajectory_section, lift_up_front_forward_trajectory_section, put_down_trajectory_section) self.calculate_and_actuate_lower_limps_angle_set_at_good_timing( is_swing_leg_left=is_swing_leg_left, interpolated_position=interpolated_position, time_fraction=time_fraction) if self.timer.is_done(): sleep(self.duration_of_rest) self.timer.restart() self.set_current_walking_set_index_to_next() is_swing_leg_left, initial_position, lift_up_front_position, lift_up_front_forward_position, put_down_position\ = self._get_is_swing_leg_left_and_walk_trajectory_four_positions__with_current_working_set(self.current_walking_set_index) lift_up_front_trajectory_section, lift_up_front_forward_trajectory_section, put_down_trajectory_section\ = self._get_3_walking_trajectory_sections(initial_position, lift_up_front_position, lift_up_front_forward_position, put_down_position)
import numpy as np import matplotlib.pyplot as plt from scipy.interpolate.fitpack import splrep, splev from util.point import Point from walking.helpers import bezier_curves from walking.helpers.bezier_curves import LineSegment x = (0, 2.0, 6.0, 8.0) y = (-47.7, -44.7, -44.7, -47.7) tck = splrep(x, y) x2 = np.linspace(0, 8) y2 = splev(x2, tck) points = [Point((x_, 0, y_)) for (x_, y_) in zip(x, y)] l1 = LineSegment(points[0], points[1]) l2 = LineSegment(points[1], points[2]) l3 = LineSegment(points[2], points[3]) yy2 = [bezier_curves.get_interpolated_position(x2_/8, l1, l2, l3)[2] for x2_ in x2] plt.plot(x, y, 'o', x, y, '--', x2, y2, x2, yy2) plt.show()
import numpy as np import matplotlib.pyplot as plt from scipy.interpolate.fitpack import splrep, splev from util.point import Point from walking.helpers import bezier_curves from walking.helpers.bezier_curves import LineSegment x = (0, 2.0, 6.0, 8.0) y = (-47.7, -44.7, -44.7, -47.7) tck = splrep(x, y) x2 = np.linspace(0, 8) y2 = splev(x2, tck) points = [Point((x_, 0, y_)) for (x_, y_) in zip(x, y)] l1 = LineSegment(points[0], points[1]) l2 = LineSegment(points[1], points[2]) l3 = LineSegment(points[2], points[3]) yy2 = [ bezier_curves.get_interpolated_position(x2_ / 8, l1, l2, l3)[2] for x2_ in x2 ] plt.plot(x, y, 'o', x, y, '--', x2, y2, x2, yy2) plt.show()