def __init__(self, hip_height, torso_angle, lift_off_slope, step_height, step_length, put_down_slope): self.hip_height = hip_height self.torso_angle = torso_angle self.lift_off_slope = lift_off_slope self.step_height = step_height self.step_length = step_length self.put_down_slope = put_down_slope self.upper_limp_part = UpperLimbWhileWalking(torso_angle, hip_height) self.hip_offset = self.upper_limp_part.hip_offset self.painter = Painter() self.robot_model = RobotModel(self.painter)
class RobotWalkingLowerLimpKinematicsEngine: """ Responsible for computing the control points on the walking trajectory """ def __init__(self, hip_height, torso_angle, lift_off_slope, step_height, step_length, put_down_slope): self.hip_height = hip_height self.torso_angle = torso_angle self.lift_off_slope = lift_off_slope self.step_height = step_height self.step_length = step_length self.put_down_slope = put_down_slope self.upper_limp_part = UpperLimbWhileWalking(torso_angle, hip_height) self.hip_offset = self.upper_limp_part.hip_offset self.painter = Painter() self.robot_model = RobotModel(self.painter) def get_squat_lower_limps_angles_set__with_tilted_torso_effect(self): squat_left_lower_limp_angles = self._get_one_lower_limp_angles__with__both_tilted_torso_and_hip_offset_effect(get_initial_foot_position(is_left=True), is_left=True) squat_right_lower_limp_angles = self._get_one_lower_limp_angles__with__both_tilted_torso_and_hip_offset_effect(get_initial_foot_position(is_left=False), is_left=False) return squat_left_lower_limp_angles, squat_right_lower_limp_angles def get_walk_trajectory_three_positions(self, is_swing_leg_left, step_length, current_position_relative_to_initial_foot_position): """ when ``step_length`` can be set to half step length, the robot only take half step. """ ratio = step_length / self.step_length walk_trajectory = WalkTrajectory(self.hip_offset, self.lift_off_slope / ratio, self.step_height, step_length, self.put_down_slope / ratio) lift_up_front_position = walk_trajectory.get_lift_front_position(is_left=is_swing_leg_left, foot_position_relative_to=current_position_relative_to_initial_foot_position) lift_up_front_forward_position = walk_trajectory.get_lift_up_front_forward_position(lift_up_front_position) put_down_position = walk_trajectory.get_put_down_position(lift_up_front_forward_position) return lift_up_front_position, lift_up_front_forward_position, put_down_position def get_feet_positions_set__with__both_tilted_torso_and_hip_offset_effect(self, is_swing_leg_left, step_length, current_position_relative_to_initial_foot_position): """ when ``step_length`` is set to half step length, the robot only take half step. """ lift_up_front_position, lift_up_front_forward_position, put_down_position\ = self.get_walk_trajectory_three_positions(is_swing_leg_left, step_length, current_position_relative_to_initial_foot_position) lower_limps_angle_set_for_lift_up_front_position = self.get_lower_limps_angle_set__with__both_tilted_torso_and_hip_offset_effect(swing_leg_foot_position=lift_up_front_position, is_swing_leg_left=is_swing_leg_left) lower_limps_angle_set_for_lift_up_front_forward_position = self.get_lower_limps_angle_set__with__both_tilted_torso_and_hip_offset_effect(swing_leg_foot_position=lift_up_front_forward_position, is_swing_leg_left=is_swing_leg_left) lower_limps_angle_set_for_put_down_position = self.get_lower_limps_angle_set__with__both_tilted_torso_and_hip_offset_effect(swing_leg_foot_position=put_down_position, is_swing_leg_left=is_swing_leg_left) return (lower_limps_angle_set_for_lift_up_front_position, lower_limps_angle_set_for_lift_up_front_forward_position, lower_limps_angle_set_for_put_down_position) def get_lower_limps_angle_set__with__both_tilted_torso_and_hip_offset_effect(self, swing_leg_foot_position, is_swing_leg_left): swing_lower_limp_angles = self._get_one_lower_limp_angles__with__both_tilted_torso_and_hip_offset_effect(swing_leg_foot_position, is_left=is_swing_leg_left) support_lower_limp_angles = self._get_support_lower_limp_angles(is_support_leg_left=not is_swing_leg_left, swing_leg_foot_position=swing_leg_foot_position) return self._convert_swing_and_support_lower_limps_to_left_and_right(is_swing_leg_left, swing_lower_limp_angles, support_lower_limp_angles) def _convert_swing_and_support_lower_limps_to_left_and_right(self, is_swing_leg_left, swing_lower_limp_angles, support_lower_limp_angles): if is_swing_leg_left: return [swing_lower_limp_angles, support_lower_limp_angles] else: return [support_lower_limp_angles, swing_lower_limp_angles] def _get_support_lower_limp_angles(self, is_support_leg_left, swing_leg_foot_position): opposite_ground_position = WalkTrajectory.project_to_opposite_ground(Point(swing_leg_foot_position), is_left=is_support_leg_left) support_lower_limp_angles = self._get_one_lower_limp_angles__with__both_tilted_torso_and_hip_offset_effect(opposite_ground_position, is_left=is_support_leg_left) return support_lower_limp_angles def _get_one_lower_limp_angles__with__both_tilted_torso_and_hip_offset_effect(self, position, is_left): """ Returns a new position and lower limp angles """ position_with_hip_offset = self. upper_limp_part.add_hip_offset_effect(position) lower_limp_angles_without_tilted_torso_effect = self._get_lower_limp_angles(position_with_hip_offset, is_left=is_left) lower_limp_angles = self.upper_limp_part.add_tilted_torso_effect(lower_limp_angles_without_tilted_torso_effect, is_left=is_left) return lower_limp_angles def _get_lower_limp_angles(self, target_position, is_left): lower_limp_angles_except_hip_transversal = InverseKinematicsLeg(is_left=is_left, hip_transversal_radians=0, target_position=target_position, robot_model=self.robot_model, painter=self.painter).get_angles() lower_limp_angles = tuple([0] + list(lower_limp_angles_except_hip_transversal)) return lower_limp_angles
class RobotWalkingLowerLimpKinematicsEngine: """ Responsible for computing the control points on the walking trajectory """ def __init__(self, hip_height, torso_angle, lift_off_slope, step_height, step_length, put_down_slope): self.hip_height = hip_height self.torso_angle = torso_angle self.lift_off_slope = lift_off_slope self.step_height = step_height self.step_length = step_length self.put_down_slope = put_down_slope self.upper_limp_part = UpperLimbWhileWalking(torso_angle, hip_height) self.hip_offset = self.upper_limp_part.hip_offset self.painter = Painter() self.robot_model = RobotModel(self.painter) def get_squat_lower_limps_angles_set__with_tilted_torso_effect(self): squat_left_lower_limp_angles = self._get_one_lower_limp_angles__with__both_tilted_torso_and_hip_offset_effect( get_initial_foot_position(is_left=True), is_left=True) squat_right_lower_limp_angles = self._get_one_lower_limp_angles__with__both_tilted_torso_and_hip_offset_effect( get_initial_foot_position(is_left=False), is_left=False) return squat_left_lower_limp_angles, squat_right_lower_limp_angles def get_walk_trajectory_three_positions( self, is_swing_leg_left, step_length, current_position_relative_to_initial_foot_position): """ when ``step_length`` can be set to half step length, the robot only take half step. """ ratio = step_length / self.step_length walk_trajectory = WalkTrajectory(self.hip_offset, self.lift_off_slope / ratio, self.step_height, step_length, self.put_down_slope / ratio) lift_up_front_position = walk_trajectory.get_lift_front_position( is_left=is_swing_leg_left, foot_position_relative_to= current_position_relative_to_initial_foot_position) lift_up_front_forward_position = walk_trajectory.get_lift_up_front_forward_position( lift_up_front_position) put_down_position = walk_trajectory.get_put_down_position( lift_up_front_forward_position) return lift_up_front_position, lift_up_front_forward_position, put_down_position def get_feet_positions_set__with__both_tilted_torso_and_hip_offset_effect( self, is_swing_leg_left, step_length, current_position_relative_to_initial_foot_position): """ when ``step_length`` is set to half step length, the robot only take half step. """ lift_up_front_position, lift_up_front_forward_position, put_down_position\ = self.get_walk_trajectory_three_positions(is_swing_leg_left, step_length, current_position_relative_to_initial_foot_position) lower_limps_angle_set_for_lift_up_front_position = self.get_lower_limps_angle_set__with__both_tilted_torso_and_hip_offset_effect( swing_leg_foot_position=lift_up_front_position, is_swing_leg_left=is_swing_leg_left) lower_limps_angle_set_for_lift_up_front_forward_position = self.get_lower_limps_angle_set__with__both_tilted_torso_and_hip_offset_effect( swing_leg_foot_position=lift_up_front_forward_position, is_swing_leg_left=is_swing_leg_left) lower_limps_angle_set_for_put_down_position = self.get_lower_limps_angle_set__with__both_tilted_torso_and_hip_offset_effect( swing_leg_foot_position=put_down_position, is_swing_leg_left=is_swing_leg_left) return (lower_limps_angle_set_for_lift_up_front_position, lower_limps_angle_set_for_lift_up_front_forward_position, lower_limps_angle_set_for_put_down_position) def get_lower_limps_angle_set__with__both_tilted_torso_and_hip_offset_effect( self, swing_leg_foot_position, is_swing_leg_left): swing_lower_limp_angles = self._get_one_lower_limp_angles__with__both_tilted_torso_and_hip_offset_effect( swing_leg_foot_position, is_left=is_swing_leg_left) support_lower_limp_angles = self._get_support_lower_limp_angles( is_support_leg_left=not is_swing_leg_left, swing_leg_foot_position=swing_leg_foot_position) return self._convert_swing_and_support_lower_limps_to_left_and_right( is_swing_leg_left, swing_lower_limp_angles, support_lower_limp_angles) def _convert_swing_and_support_lower_limps_to_left_and_right( self, is_swing_leg_left, swing_lower_limp_angles, support_lower_limp_angles): if is_swing_leg_left: return [swing_lower_limp_angles, support_lower_limp_angles] else: return [support_lower_limp_angles, swing_lower_limp_angles] def _get_support_lower_limp_angles(self, is_support_leg_left, swing_leg_foot_position): opposite_ground_position = WalkTrajectory.project_to_opposite_ground( Point(swing_leg_foot_position), is_left=is_support_leg_left) support_lower_limp_angles = self._get_one_lower_limp_angles__with__both_tilted_torso_and_hip_offset_effect( opposite_ground_position, is_left=is_support_leg_left) return support_lower_limp_angles def _get_one_lower_limp_angles__with__both_tilted_torso_and_hip_offset_effect( self, position, is_left): """ Returns a new position and lower limp angles """ position_with_hip_offset = self.upper_limp_part.add_hip_offset_effect( position) lower_limp_angles_without_tilted_torso_effect = self._get_lower_limp_angles( position_with_hip_offset, is_left=is_left) lower_limp_angles = self.upper_limp_part.add_tilted_torso_effect( lower_limp_angles_without_tilted_torso_effect, is_left=is_left) return lower_limp_angles def _get_lower_limp_angles(self, target_position, is_left): lower_limp_angles_except_hip_transversal = InverseKinematicsLeg( is_left=is_left, hip_transversal_radians=0, target_position=target_position, robot_model=self.robot_model, painter=self.painter).get_angles() lower_limp_angles = tuple( [0] + list(lower_limp_angles_except_hip_transversal)) return lower_limp_angles