141 lines
5.4 KiB
Python
141 lines
5.4 KiB
Python
import math
|
|
import numpy as np
|
|
from math_ops.Math_Ops import Math_Ops as U
|
|
from behaviors.custom.Step.Step_Generator import Step_Generator
|
|
from agent.Base_Agent import Base_Agent
|
|
|
|
|
|
class Env_LL:
|
|
|
|
def __init__(self, base_agent: Base_Agent, step_width=None):
|
|
self.world = base_agent.world
|
|
self.obs = np.zeros(78, np.float32)
|
|
self.STEP_DUR = 8
|
|
self.STEP_Z_SPAN = 0.02
|
|
self.STEP_Z_MAX = 0.7
|
|
|
|
r = self.world.robot
|
|
nao_specs = base_agent.inv_kinematics.NAO_SPECS
|
|
self.leg_length = nao_specs[1] + nao_specs[3]
|
|
feet_y_dev = nao_specs[0] * step_width
|
|
sample_time = r.STEPTIME
|
|
max_ankle_z = nao_specs[5]
|
|
|
|
self.step_generator = Step_Generator(feet_y_dev, sample_time, max_ankle_z)
|
|
self.inv_kinematics = base_agent.inv_kinematics
|
|
self.DEFAULT_ARMS = np.array([
|
|
-90, -90, 8, 8, 90, 90, 70, 70], np.float32)
|
|
self.HL_abs_direction = None
|
|
self.push_speed = 1
|
|
self.step_counter = 0
|
|
self.act = np.zeros(16, np.float32)
|
|
self.values_l = None
|
|
self.values_r = None
|
|
|
|
def observe(self, init=False):
|
|
w = self.world
|
|
r = self.world.robot
|
|
|
|
if init:
|
|
self.step_counter = 0
|
|
self.act = np.zeros(16, np.float32)
|
|
|
|
# 填充观测向量
|
|
self.obs[0] = min(self.step_counter, 96) / 100
|
|
self.obs[1] = r.loc_head_z * 3
|
|
self.obs[2] = r.loc_head_z_vel / 2
|
|
self.obs[3] = r.imu_torso_roll / 15
|
|
self.obs[4] = r.imu_torso_pitch / 15
|
|
self.obs[5:8] = r.gyro / 100
|
|
self.obs[8:11] = r.acc / 10
|
|
self.obs[11:17] = r.frp.get('lf', np.zeros(6)) * (10, 10, 10, 0.01, 0.01, 0.01)
|
|
self.obs[17:23] = r.frp.get('rf', np.zeros(6)) * (10, 10, 10, 0.01, 0.01, 0.01)
|
|
self.obs[23:43] = r.joints_position[2:22] / 100
|
|
self.obs[43:63] = r.joints_speed[2:22] / 6.1395
|
|
|
|
if init:
|
|
self.obs[63] = 1
|
|
self.obs[64] = 1
|
|
self.obs[65] = 0
|
|
self.obs[66] = 0
|
|
else:
|
|
self.obs[63] = self.step_generator.external_progress
|
|
self.obs[64] = float(self.step_generator.state_is_left_active)
|
|
self.obs[65] = float(not self.step_generator.state_is_left_active)
|
|
self.obs[66] = math.sin((self.step_generator.state_current_ts / self.step_generator.ts_per_step) * math.pi)
|
|
|
|
ball_rel_hip_center = self.inv_kinematics.torso_to_hip_transform(w.ball_rel_torso_cart_pos)
|
|
ball_dist_hip_center = np.linalg.norm(ball_rel_hip_center)
|
|
|
|
if init:
|
|
self.obs[67:70] = (0, 0, 0)
|
|
elif w.ball_is_visible:
|
|
self.obs[67:70] = (ball_rel_hip_center - self.obs[70:73]) * 10
|
|
|
|
self.obs[70:73] = ball_rel_hip_center
|
|
self.obs[73] = ball_dist_hip_center * 2
|
|
|
|
rel_HL_target = U.normalize_deg(self.HL_abs_direction - r.imu_torso_orientation)
|
|
self.obs[74] = U.deg_cos(rel_HL_target)
|
|
self.obs[75] = U.deg_sin(rel_HL_target)
|
|
self.obs[76] = 2
|
|
self.obs[77] = 2
|
|
|
|
# 找到最近的对手
|
|
opps_dist = [o.state_horizontal_dist for o in w.opponents]
|
|
if opps_dist:
|
|
closest_opp_idx = np.argmin(opps_dist)
|
|
o = w.opponents[closest_opp_idx]
|
|
|
|
if opps_dist[closest_opp_idx] < 1:
|
|
body_parts_rel_torso_2d_avg = np.zeros(2)
|
|
weight_sum = 0
|
|
|
|
for pos in o.body_parts_cart_rel_pos.values():
|
|
bp_rel_torso_2d = r.head_to_body_part_transform('torso', pos)[:2]
|
|
weight = math.pow(1e+06, -np.linalg.norm(bp_rel_torso_2d))
|
|
body_parts_rel_torso_2d_avg += weight * bp_rel_torso_2d
|
|
weight_sum += weight
|
|
|
|
if weight_sum > 0:
|
|
body_parts_rel_torso_2d_avg /= weight_sum
|
|
self.obs[76] = body_parts_rel_torso_2d_avg[0]
|
|
self.obs[77] = body_parts_rel_torso_2d_avg[1]
|
|
|
|
return self.obs
|
|
|
|
def execute_ik(self, l_pos, l_rot, r_pos, r_rot):
|
|
r = self.world.robot
|
|
(indices, self.values_l, error_codes) = self.inv_kinematics.leg(
|
|
l_pos, l_rot, True, dynamic_pose=False)
|
|
r.set_joints_target_position_direct(indices, self.values_l, harmonize=False)
|
|
|
|
(indices, self.values_r, error_codes) = self.inv_kinematics.leg(
|
|
r_pos, r_rot, False, dynamic_pose=False)
|
|
r.set_joints_target_position_direct(indices, self.values_r, harmonize=False)
|
|
|
|
def execute(self, action):
|
|
r = self.world.robot
|
|
self.act = 0.8 * self.act + 0.2 * action * 0.9 * self.push_speed
|
|
|
|
(lfy, lfz, rfy, rfz) = self.step_generator.get_target_positions(
|
|
self.step_counter == 0, self.STEP_DUR, self.STEP_Z_SPAN,
|
|
self.leg_length * self.STEP_Z_MAX)
|
|
|
|
a = self.act
|
|
l_ankle_pos = (a[0] * 0.025 - 0.01, a[1] * 0.01 + lfy, a[2] * 0.01 + lfz)
|
|
r_ankle_pos = (a[3] * 0.025 - 0.01, a[4] * 0.01 + rfy, a[5] * 0.01 + rfz)
|
|
|
|
l_foot_rot = a[6:9] * (2, 2, 3)
|
|
r_foot_rot = a[9:12] * (2, 2, 3)
|
|
l_foot_rot[2] = max(0, l_foot_rot[2] + 18.3)
|
|
r_foot_rot[2] = min(0, r_foot_rot[2] - 18.3)
|
|
|
|
arms = np.copy(self.DEFAULT_ARMS)
|
|
arms[0:4] += a[12:16] * 4
|
|
|
|
self.execute_ik(l_ankle_pos, l_foot_rot, r_ankle_pos, r_foot_rot)
|
|
r.set_joints_target_position_direct(slice(14, 22), arms, harmonize=False)
|
|
|
|
self.step_counter += 1
|