Init gym
This commit is contained in:
535
world/Robot.py
Normal file
535
world/Robot.py
Normal file
@@ -0,0 +1,535 @@
|
||||
from collections import deque
|
||||
from math import atan, pi, sqrt, tan
|
||||
from math_ops.Math_Ops import Math_Ops as M
|
||||
from math_ops.Matrix_3x3 import Matrix_3x3
|
||||
from math_ops.Matrix_4x4 import Matrix_4x4
|
||||
from world.commons.Body_Part import Body_Part
|
||||
from world.commons.Joint_Info import Joint_Info
|
||||
import numpy as np
|
||||
import xml.etree.ElementTree as xmlp
|
||||
|
||||
class Robot():
|
||||
STEPTIME = 0.02 # Fixed step time
|
||||
VISUALSTEP = 0.04 # Fixed visual step time
|
||||
SQ_STEPTIME = STEPTIME * STEPTIME
|
||||
GRAVITY = np.array([0,0,-9.81])
|
||||
IMU_DECAY = 0.996 #IMU's velocity decay
|
||||
|
||||
#------------------ constants to force symmetry in joints/effectors
|
||||
|
||||
MAP_PERCEPTOR_TO_INDEX = {"hj1":0, "hj2":1, "llj1":2, "rlj1":3,
|
||||
"llj2":4, "rlj2":5, "llj3":6, "rlj3":7,
|
||||
"llj4":8, "rlj4":9, "llj5":10,"rlj5":11,
|
||||
"llj6":12,"rlj6":13,"laj1":14,"raj1":15,
|
||||
"laj2":16,"raj2":17,"laj3":18,"raj3":19,
|
||||
"laj4":20,"raj4":21,"llj7":22,"rlj7":23 }
|
||||
|
||||
# Fix symmetry issues 1a/4 (identification)
|
||||
FIX_PERCEPTOR_SET = {'rlj2','rlj6','raj2','laj3','laj4'}
|
||||
FIX_INDICES_LIST = [5,13,17,18,20]
|
||||
|
||||
# Recommended height for unofficial beam (near ground)
|
||||
BEAM_HEIGHTS = [0.4, 0.43, 0.4, 0.46, 0.4]
|
||||
|
||||
|
||||
def __init__(self, unum:int, robot_type:int) -> None:
|
||||
robot_xml = "nao"+str(robot_type)+".xml" # Typical NAO file name
|
||||
self.type = robot_type
|
||||
self.beam_height = Robot.BEAM_HEIGHTS[robot_type]
|
||||
self.no_of_joints = 24 if robot_type == 4 else 22
|
||||
|
||||
#Fix symmetry issues 1b/4 (identification)
|
||||
self.FIX_EFFECTOR_MASK = np.ones(self.no_of_joints)
|
||||
self.FIX_EFFECTOR_MASK[Robot.FIX_INDICES_LIST] = -1
|
||||
|
||||
self.body_parts = dict() # keys='body part names' (given by the robot's XML), values='Body_Part objects'
|
||||
self.unum = unum # Robot's uniform number
|
||||
self.gyro = np.zeros(3) # Angular velocity along the three axes of freedom of the robot's torso (deg/s)
|
||||
self.acc = np.zeros(3) # Proper acceleration along the three axes of freedom of the robot's torso (m/s2)
|
||||
self.frp = dict() # foot "lf"/"rf", toe "lf1"/"rf1" resistance perceptor (relative [p]oint of origin + [f]orce vector) e.g. {"lf":(px,py,pz,fx,fy,fz)}
|
||||
self.feet_toes_last_touch = {"lf":0,"rf":0,"lf1":0,"rf1":0} # foot "lf"/"rf", toe "lf1"/"rf1" World.time_local_ms when foot/toe last touched any surface
|
||||
self.feet_toes_are_touching = {"lf":False,"rf":False,"lf1":False,"rf1":False} # foot "lf"/"rf", toe "lf1"/"rf1" True if touching in last received server message
|
||||
self.fwd_kinematics_list = None # List of body parts, ordered according to dependencies
|
||||
self.rel_cart_CoM_position = np.zeros(3) # Center of Mass position, relative to head, in cartesian coordinates (m)
|
||||
|
||||
# Joint variables are optimized for performance / array operations
|
||||
self.joints_position = np.zeros(self.no_of_joints) # Joints' angular position (deg)
|
||||
self.joints_speed = np.zeros(self.no_of_joints) # Joints' angular speed (rad/s)
|
||||
self.joints_target_speed = np.zeros(self.no_of_joints) # Joints' target speed (rad/s) (max: 6.1395 rad/s, see rcssserver3d/data/rsg/agent/nao/hingejoint.rsg)
|
||||
self.joints_target_last_speed = np.zeros(self.no_of_joints) # Joints' last target speed (rad/s) (max: 6.1395 rad/s, see rcssserver3d/data/rsg/agent/nao/hingejoint.rsg)
|
||||
self.joints_info = [None] * self.no_of_joints # Joints' constant information (see class Joint_Info)
|
||||
self.joints_transform = [Matrix_4x4() for _ in range(self.no_of_joints)] # Joints' transformation matrix
|
||||
|
||||
# Localization variables relative to head
|
||||
self.loc_head_to_field_transform = Matrix_4x4() # Transformation matrix from head to field
|
||||
self.loc_field_to_head_transform = Matrix_4x4() # Transformation matrix from field to head
|
||||
self.loc_rotation_head_to_field = Matrix_3x3() # Rotation matrix from head to field
|
||||
self.loc_rotation_field_to_head = Matrix_3x3() # Rotation matrix from field to head
|
||||
self.loc_head_position = np.zeros(3) # Absolute head position (m)
|
||||
self.loc_head_position_history = deque(maxlen=40)# Absolute head position history (queue with up to 40 old positions at intervals of 0.04s, where index 0 is the previous position)
|
||||
self.loc_head_velocity = np.zeros(3) # Absolute head velocity (m/s) (Warning: possibly noisy)
|
||||
self.loc_head_orientation = 0 # Head orientation (deg)
|
||||
self.loc_is_up_to_date = False # False if this is not a visual step, or not enough elements are visible
|
||||
self.loc_last_update = 0 # World.time_local_ms when the localization was last updated
|
||||
self.loc_head_position_last_update = 0 # World.time_local_ms when loc_head_position was last updated by vision or radio
|
||||
self.radio_fallen_state = False # True if (radio says we fell) and (radio is significantly more recent than loc)
|
||||
self.radio_last_update = 0 # World.time_local_ms when radio_fallen_state was last updated (and possibly loc_head_position)
|
||||
|
||||
# Localization variables relative to torso
|
||||
self.loc_torso_to_field_rotation = Matrix_3x3() # Rotation matrix from torso to field
|
||||
self.loc_torso_to_field_transform = Matrix_4x4() # Transformation matrix from torso to field
|
||||
self.loc_torso_roll = 0 # Torso roll (deg)
|
||||
self.loc_torso_pitch = 0 # Torso pitch (deg)
|
||||
self.loc_torso_orientation = 0 # Torso orientation (deg)
|
||||
self.loc_torso_inclination = 0 # Torso inclination (deg) (inclination of z-axis in relation to field z-axis)
|
||||
self.loc_torso_position = np.zeros(3) # Absolute torso position (m)
|
||||
self.loc_torso_velocity = np.zeros(3) # Absolute torso velocity (m/s)
|
||||
self.loc_torso_acceleration = np.zeros(3) # Absolute Coordinate acceleration (m/s2)
|
||||
|
||||
# Other localization variables
|
||||
self.cheat_abs_pos = np.zeros(3) # Absolute head position provided by the server as cheat (m)
|
||||
self.cheat_ori = 0.0 # Absolute head orientation provided by the server as cheat (deg)
|
||||
self.loc_CoM_position = np.zeros(3) # Absolute CoM position (m)
|
||||
self.loc_CoM_velocity = np.zeros(3) # Absolute CoM velocity (m/s)
|
||||
|
||||
# Localization special variables
|
||||
'''
|
||||
self.loc_head_z is often equivalent to self.loc_head_position[2], but sometimes it differs.
|
||||
There are situations in which the rotation and translation cannot be computed,
|
||||
but the z-coordinate can still be found through vision, in which case:
|
||||
self.loc_is_up_to_date is False
|
||||
self.loc_head_z_is_up_to_date is True
|
||||
It should be used in applications which rely on z as an independent coordinate, such
|
||||
as detecting if the robot has fallen, or as an observation for machine learning.
|
||||
It should NEVER be used for 3D transformations.
|
||||
'''
|
||||
self.loc_head_z = 0 # Absolute head position (z) - see above for explanation (m)
|
||||
self.loc_head_z_is_up_to_date = False # False if this is not a visual step, or not enough elements are visible
|
||||
self.loc_head_z_last_update = 0 # World.time_local_ms when loc_head_z was last computed
|
||||
self.loc_head_z_vel = 0 # Absolute head velocity (z) (m/s)
|
||||
|
||||
# Localization + Gyroscope
|
||||
# These variables are reliable. The gyroscope is used to update the rotation when waiting for the next visual cycle
|
||||
self.imu_torso_roll = 0 # Torso roll (deg) (src: Localization + Gyro)
|
||||
self.imu_torso_pitch = 0 # Torso pitch (deg) (src: Localization + Gyro)
|
||||
self.imu_torso_orientation = 0 # Torso orientation (deg) (src: Localization + Gyro)
|
||||
self.imu_torso_inclination = 0 # Torso inclination (deg) (src: Localization + Gyro)
|
||||
self.imu_torso_to_field_rotation = Matrix_3x3() # Rotation matrix from torso to field (src: Localization + Gyro)
|
||||
self.imu_last_visual_update = 0 # World.time_local_ms when the IMU data was last updated with visual information
|
||||
|
||||
# Localization + Gyroscope + Accelerometer
|
||||
# Warning: these variables are unreliable, since small errors in the Localization Orientation lead to
|
||||
# wrong acceleration -> wrong velocity -> wrong position
|
||||
self.imu_weak_torso_to_field_transform = Matrix_4x4() # Transformation matrix from torso to field (src: Localization + Gyro + Acc)
|
||||
self.imu_weak_head_to_field_transform = Matrix_4x4() # Transformation matrix from head to field (src: Localization + Gyro + Acc)
|
||||
self.imu_weak_field_to_head_transform = Matrix_4x4() # Transformation matrix from field to head (src: Localization + Gyro + Acc)
|
||||
self.imu_weak_torso_position = np.zeros(3) # Absolute torso position (m) (src: Localization + Gyro + Acc)
|
||||
self.imu_weak_torso_velocity = np.zeros(3) # Absolute torso velocity (m/s) (src: Localization + Gyro + Acc)
|
||||
self.imu_weak_torso_acceleration = np.zeros(3) # Absolute torso acceleration (m/s2) (src: Localization + Gyro + Acc)
|
||||
self.imu_weak_torso_next_position = np.zeros(3) # Absolute position in next step estimate (m) (src: Localization + Gyro + Acc)
|
||||
self.imu_weak_torso_next_velocity = np.zeros(3) # Absolute velocity in next step estimate (m/s) (src: Localization + Gyro + Acc)
|
||||
self.imu_weak_CoM_position = np.zeros(3) # Absolute CoM position (m) (src: Localization + Gyro + Acc)
|
||||
self.imu_weak_CoM_velocity = np.zeros(3) # Absolute CoM velocity (m/s) (src: Localization + Gyro + Acc)
|
||||
|
||||
|
||||
#Using explicit variables to enable IDE suggestions
|
||||
self.J_HEAD_YAW = 0
|
||||
self.J_HEAD_PITCH = 1
|
||||
self.J_LLEG_YAW_PITCH = 2
|
||||
self.J_RLEG_YAW_PITCH = 3
|
||||
self.J_LLEG_ROLL = 4
|
||||
self.J_RLEG_ROLL = 5
|
||||
self.J_LLEG_PITCH = 6
|
||||
self.J_RLEG_PITCH = 7
|
||||
self.J_LKNEE = 8
|
||||
self.J_RKNEE = 9
|
||||
self.J_LFOOT_PITCH = 10
|
||||
self.J_RFOOT_PITCH = 11
|
||||
self.J_LFOOT_ROLL = 12
|
||||
self.J_RFOOT_ROLL = 13
|
||||
self.J_LARM_PITCH = 14
|
||||
self.J_RARM_PITCH = 15
|
||||
self.J_LARM_ROLL = 16
|
||||
self.J_RARM_ROLL = 17
|
||||
self.J_LELBOW_YAW = 18
|
||||
self.J_RELBOW_YAW = 19
|
||||
self.J_LELBOW_ROLL = 20
|
||||
self.J_RELBOW_ROLL = 21
|
||||
self.J_LTOE_PITCH = 22
|
||||
self.J_RTOE_PITCH = 23
|
||||
|
||||
|
||||
#------------------ parse robot xml
|
||||
|
||||
dir = M.get_active_directory("/world/commons/robots/")
|
||||
robot_xml_root = xmlp.parse(dir + robot_xml).getroot()
|
||||
|
||||
joint_no = 0
|
||||
for child in robot_xml_root:
|
||||
if child.tag == "bodypart":
|
||||
self.body_parts[child.attrib['name']] = Body_Part(child.attrib['mass'])
|
||||
elif child.tag == "joint":
|
||||
self.joints_info[joint_no] = Joint_Info(child)
|
||||
self.joints_position[joint_no] = 0.0
|
||||
ji = self.joints_info[joint_no]
|
||||
|
||||
#save joint if body part is 1st anchor (to simplify model traversal in a single direction)
|
||||
self.body_parts[ji.anchor0_part].joints.append(Robot.MAP_PERCEPTOR_TO_INDEX[ji.perceptor])
|
||||
|
||||
joint_no += 1
|
||||
if joint_no == self.no_of_joints: break #ignore extra joints
|
||||
|
||||
else:
|
||||
raise NotImplementedError
|
||||
|
||||
assert joint_no == self.no_of_joints, "The Robot XML and the robot type don't match!"
|
||||
|
||||
|
||||
def get_head_abs_vel(self, history_steps:int):
|
||||
'''
|
||||
Get robot's head absolute velocity (m/s)
|
||||
|
||||
Parameters
|
||||
----------
|
||||
history_steps : int
|
||||
number of history steps to consider [1,40]
|
||||
|
||||
Examples
|
||||
--------
|
||||
get_head_abs_vel(1) is equivalent to (current abs pos - last abs pos) / 0.04
|
||||
get_head_abs_vel(2) is equivalent to (current abs pos - abs pos 0.08s ago) / 0.08
|
||||
get_head_abs_vel(3) is equivalent to (current abs pos - abs pos 0.12s ago) / 0.12
|
||||
'''
|
||||
assert 1 <= history_steps <= 40, "Argument 'history_steps' must be in range [1,40]"
|
||||
|
||||
if len(self.loc_head_position_history) == 0:
|
||||
return np.zeros(3)
|
||||
|
||||
h_step = min(history_steps, len(self.loc_head_position_history))
|
||||
t = h_step * Robot.VISUALSTEP
|
||||
|
||||
return (self.loc_head_position - self.loc_head_position_history[h_step-1]) / t
|
||||
|
||||
|
||||
def _initialize_kinematics(self):
|
||||
|
||||
#starting with head
|
||||
parts={"head"}
|
||||
sequential_body_parts = ["head"]
|
||||
|
||||
while len(parts) > 0:
|
||||
part = parts.pop()
|
||||
|
||||
for j in self.body_parts[part].joints:
|
||||
|
||||
p = self.joints_info[j].anchor1_part
|
||||
|
||||
if len(self.body_parts[p].joints) > 0: #add body part if it is the 1st anchor of some joint
|
||||
parts.add(p)
|
||||
sequential_body_parts.append(p)
|
||||
|
||||
self.fwd_kinematics_list = [(self.body_parts[part],j, self.body_parts[self.joints_info[j].anchor1_part] )
|
||||
for part in sequential_body_parts for j in self.body_parts[part].joints]
|
||||
|
||||
#Fix symmetry issues 4/4 (kinematics)
|
||||
for i in Robot.FIX_INDICES_LIST:
|
||||
self.joints_info[i].axes *= -1
|
||||
aux = self.joints_info[i].min
|
||||
self.joints_info[i].min = -self.joints_info[i].max
|
||||
self.joints_info[i].max = -aux
|
||||
|
||||
|
||||
def update_localization(self, localization_raw, time_local_ms):
|
||||
|
||||
# parse raw data
|
||||
loc = localization_raw.astype(float) #32bits to 64bits for consistency
|
||||
self.loc_is_up_to_date = bool(loc[32])
|
||||
self.loc_head_z_is_up_to_date = bool(loc[34])
|
||||
|
||||
if self.loc_head_z_is_up_to_date:
|
||||
time_diff = (time_local_ms - self.loc_head_z_last_update) / 1000
|
||||
self.loc_head_z_vel = (loc[33] - self.loc_head_z) / time_diff
|
||||
self.loc_head_z = loc[33]
|
||||
self.loc_head_z_last_update = time_local_ms
|
||||
|
||||
# Save last position to history at every vision cycle (even if not up to date) (update_localization is only called at vision cycles)
|
||||
self.loc_head_position_history.appendleft(np.copy(self.loc_head_position))
|
||||
|
||||
if self.loc_is_up_to_date:
|
||||
time_diff = (time_local_ms - self.loc_last_update) / 1000
|
||||
self.loc_last_update = time_local_ms
|
||||
self.loc_head_to_field_transform.m[:] = loc[0:16].reshape((4,4))
|
||||
self.loc_field_to_head_transform.m[:] = loc[16:32].reshape((4,4))
|
||||
|
||||
# extract data (related to the robot's head)
|
||||
self.loc_rotation_head_to_field = self.loc_head_to_field_transform.get_rotation()
|
||||
self.loc_rotation_field_to_head = self.loc_field_to_head_transform.get_rotation()
|
||||
p = self.loc_head_to_field_transform.get_translation()
|
||||
self.loc_head_velocity = (p - self.loc_head_position) / time_diff
|
||||
self.loc_head_position = p
|
||||
self.loc_head_position_last_update = time_local_ms
|
||||
self.loc_head_orientation = self.loc_head_to_field_transform.get_yaw_deg()
|
||||
self.radio_fallen_state = False
|
||||
|
||||
# extract data (related to the center of mass)
|
||||
p = self.loc_head_to_field_transform(self.rel_cart_CoM_position)
|
||||
self.loc_CoM_velocity = (p - self.loc_CoM_position) / time_diff
|
||||
self.loc_CoM_position = p
|
||||
|
||||
# extract data (related to the robot's torso)
|
||||
t = self.get_body_part_to_field_transform('torso')
|
||||
self.loc_torso_to_field_transform = t
|
||||
self.loc_torso_to_field_rotation = t.get_rotation()
|
||||
self.loc_torso_orientation = t.get_yaw_deg()
|
||||
self.loc_torso_pitch = t.get_pitch_deg()
|
||||
self.loc_torso_roll = t.get_roll_deg()
|
||||
self.loc_torso_inclination = t.get_inclination_deg()
|
||||
p = t.get_translation()
|
||||
self.loc_torso_velocity = (p - self.loc_torso_position) / time_diff
|
||||
self.loc_torso_position = p
|
||||
self.loc_torso_acceleration = self.loc_torso_to_field_rotation.multiply(self.acc) + Robot.GRAVITY
|
||||
|
||||
|
||||
def head_to_body_part_transform(self, body_part_name, coords, is_batch=False):
|
||||
'''
|
||||
If coord is a vector or list of vectors:
|
||||
Convert cartesian coordinates that are relative to head to coordinates that are relative to a body part
|
||||
|
||||
If coord is a Matrix_4x4 or a list of Matrix_4x4:
|
||||
Convert pose that is relative to head to a pose that is relative to a body part
|
||||
|
||||
Parameters
|
||||
----------
|
||||
body_part_name : `str`
|
||||
name of body part (given by the robot's XML)
|
||||
coords : array_like
|
||||
One 3D position or list of 3D positions
|
||||
is_batch : `bool`
|
||||
Indicates if coords is a batch of 3D positions
|
||||
|
||||
Returns
|
||||
-------
|
||||
coord : `list` or ndarray
|
||||
A numpy array is returned if is_batch is False, otherwise, a list of arrays is returned
|
||||
'''
|
||||
head_to_bp_transform : Matrix_4x4 = self.body_parts[body_part_name].transform.invert()
|
||||
|
||||
if is_batch:
|
||||
return [head_to_bp_transform(c) for c in coords]
|
||||
else:
|
||||
return head_to_bp_transform(coords)
|
||||
|
||||
|
||||
|
||||
def get_body_part_to_field_transform(self, body_part_name) -> Matrix_4x4:
|
||||
'''
|
||||
Computes the transformation matrix from body part to field, from which we can extract its absolute position and rotation.
|
||||
For best results, use this method when self.loc_is_up_to_date is True. Otherwise, the forward kinematics
|
||||
will not be synced with the localization data and strange results may occur.
|
||||
'''
|
||||
return self.loc_head_to_field_transform.multiply(self.body_parts[body_part_name].transform)
|
||||
|
||||
def get_body_part_abs_position(self, body_part_name) -> np.ndarray:
|
||||
'''
|
||||
Computes the absolute position of a body part considering the localization data and forward kinematics.
|
||||
For best results, use this method when self.loc_is_up_to_date is True. Otherwise, the forward kinematics
|
||||
will not be synced with the localization data and strange results may occur.
|
||||
'''
|
||||
return self.get_body_part_to_field_transform(body_part_name).get_translation()
|
||||
|
||||
def get_joint_to_field_transform(self, joint_index) -> Matrix_4x4:
|
||||
'''
|
||||
Computes the transformation matrix from joint to field, from which we can extract its absolute position and rotation.
|
||||
For best results, use this method when self.loc_is_up_to_date is True. Otherwise, the forward kinematics
|
||||
will not be synced with the localization data and strange results may occur.
|
||||
'''
|
||||
return self.loc_head_to_field_transform.multiply(self.joints_transform[joint_index])
|
||||
|
||||
def get_joint_abs_position(self, joint_index) -> np.ndarray:
|
||||
'''
|
||||
Computes the absolute position of a joint considering the localization data and forward kinematics.
|
||||
For best results, use this method when self.loc_is_up_to_date is True. Otherwise, the forward kinematics
|
||||
will not be synced with the localization data and strange results may occur.
|
||||
'''
|
||||
return self.get_joint_to_field_transform(joint_index).get_translation()
|
||||
|
||||
def update_pose(self):
|
||||
|
||||
if self.fwd_kinematics_list is None:
|
||||
self._initialize_kinematics()
|
||||
|
||||
for body_part, j, child_body_part in self.fwd_kinematics_list:
|
||||
ji = self.joints_info[j]
|
||||
self.joints_transform[j].m[:] = body_part.transform.m
|
||||
self.joints_transform[j].translate(ji.anchor0_axes, True)
|
||||
child_body_part.transform.m[:] = self.joints_transform[j].m
|
||||
child_body_part.transform.rotate_deg(ji.axes, self.joints_position[j], True)
|
||||
child_body_part.transform.translate(ji.anchor1_axes_neg, True)
|
||||
|
||||
self.rel_cart_CoM_position = np.average([b.transform.get_translation() for b in self.body_parts.values()], 0,
|
||||
[b.mass for b in self.body_parts.values()])
|
||||
|
||||
|
||||
def update_imu(self, time_local_ms):
|
||||
|
||||
# update IMU
|
||||
if self.loc_is_up_to_date:
|
||||
self.imu_torso_roll = self.loc_torso_roll
|
||||
self.imu_torso_pitch = self.loc_torso_pitch
|
||||
self.imu_torso_orientation = self.loc_torso_orientation
|
||||
self.imu_torso_inclination = self.loc_torso_inclination
|
||||
self.imu_torso_to_field_rotation.m[:] = self.loc_torso_to_field_rotation.m
|
||||
self.imu_weak_torso_to_field_transform.m[:] = self.loc_torso_to_field_transform.m
|
||||
self.imu_weak_head_to_field_transform.m[:] = self.loc_head_to_field_transform.m
|
||||
self.imu_weak_field_to_head_transform.m[:] = self.loc_field_to_head_transform.m
|
||||
self.imu_weak_torso_position[:] = self.loc_torso_position
|
||||
self.imu_weak_torso_velocity[:] = self.loc_torso_velocity
|
||||
self.imu_weak_torso_acceleration[:] = self.loc_torso_acceleration
|
||||
self.imu_weak_torso_next_position = self.loc_torso_position + self.loc_torso_velocity * Robot.STEPTIME + self.loc_torso_acceleration * (0.5 * Robot.SQ_STEPTIME)
|
||||
self.imu_weak_torso_next_velocity = self.loc_torso_velocity + self.loc_torso_acceleration * Robot.STEPTIME
|
||||
self.imu_weak_CoM_position[:] = self.loc_CoM_position
|
||||
self.imu_weak_CoM_velocity[:] = self.loc_CoM_velocity
|
||||
self.imu_last_visual_update = time_local_ms
|
||||
else:
|
||||
g = self.gyro / 50 # convert degrees per second to degrees per step
|
||||
|
||||
self.imu_torso_to_field_rotation.multiply( Matrix_3x3.from_rotation_deg(g), in_place=True, reverse_order=True)
|
||||
|
||||
self.imu_torso_orientation = self.imu_torso_to_field_rotation.get_yaw_deg()
|
||||
self.imu_torso_pitch = self.imu_torso_to_field_rotation.get_pitch_deg()
|
||||
self.imu_torso_roll = self.imu_torso_to_field_rotation.get_roll_deg()
|
||||
|
||||
self.imu_torso_inclination = atan(sqrt(tan(self.imu_torso_roll/180*pi)**2+tan(self.imu_torso_pitch/180*pi)**2))*180/pi
|
||||
|
||||
# Update position and velocity until 0.2 seconds has passed since last visual update
|
||||
if time_local_ms < self.imu_last_visual_update + 200:
|
||||
self.imu_weak_torso_position[:] = self.imu_weak_torso_next_position
|
||||
if self.imu_weak_torso_position[2] < 0: self.imu_weak_torso_position[2] = 0 # limit z coordinate to positive values
|
||||
self.imu_weak_torso_velocity[:] = self.imu_weak_torso_next_velocity * Robot.IMU_DECAY # stability tradeoff
|
||||
else:
|
||||
self.imu_weak_torso_velocity *= 0.97 # without visual updates for 0.2s, the position is locked, and the velocity decays to zero
|
||||
|
||||
# convert proper acceleration to coordinate acceleration and fix rounding bias
|
||||
self.imu_weak_torso_acceleration = self.imu_torso_to_field_rotation.multiply(self.acc) + Robot.GRAVITY
|
||||
self.imu_weak_torso_to_field_transform = Matrix_4x4.from_3x3_and_translation(self.imu_torso_to_field_rotation,self.imu_weak_torso_position)
|
||||
self.imu_weak_head_to_field_transform = self.imu_weak_torso_to_field_transform.multiply(self.body_parts["torso"].transform.invert())
|
||||
self.imu_weak_field_to_head_transform = self.imu_weak_head_to_field_transform.invert()
|
||||
p = self.imu_weak_head_to_field_transform(self.rel_cart_CoM_position)
|
||||
self.imu_weak_CoM_velocity = (p-self.imu_weak_CoM_position)/Robot.STEPTIME
|
||||
self.imu_weak_CoM_position = p
|
||||
|
||||
# Next Position = x0 + v0*t + 0.5*a*t^2, Next velocity = v0 + a*t
|
||||
self.imu_weak_torso_next_position = self.imu_weak_torso_position + self.imu_weak_torso_velocity * Robot.STEPTIME + self.imu_weak_torso_acceleration * (0.5 * Robot.SQ_STEPTIME)
|
||||
self.imu_weak_torso_next_velocity = self.imu_weak_torso_velocity + self.imu_weak_torso_acceleration * Robot.STEPTIME
|
||||
|
||||
|
||||
|
||||
def set_joints_target_position_direct(self,indices,values:np.ndarray,harmonize=True,max_speed=7.03,tolerance=0.012,limit_joints=True) -> int:
|
||||
'''
|
||||
Computes the speed of a list of joints, taking as argument the target position
|
||||
|
||||
Parameters
|
||||
----------
|
||||
indices : `int`/`list`/`slice`/numpy array
|
||||
joint indices
|
||||
values : numpy array
|
||||
target position for each listed joint index
|
||||
harmonize : `bool`
|
||||
if True, all joints reach target at same time
|
||||
max_speed : `float`
|
||||
max. speed for all joints in deg/step
|
||||
Most joints have a maximum speed of 351.77 deg/s according to rcssserver3d/data/rsg/agent/nao/hingejoint.rsg
|
||||
That translates as 7.0354 deg/step or 6.1395 rad/s
|
||||
tolerance : `float`
|
||||
angle error tolerance (in degrees) to return that target was reached (returns -1)
|
||||
limit_joints : `bool`
|
||||
limit values to the joints' range of motion
|
||||
|
||||
Returns
|
||||
-------
|
||||
remaining_steps : `int`
|
||||
predicted number of remaining steps or -1 if target was already reached
|
||||
|
||||
Examples
|
||||
-------
|
||||
(let p[tx] be the joint position at t=x)
|
||||
|
||||
Example for return value: moving joint[0] from 0deg to 10deg
|
||||
pos[t0]: 0, speed[t0]: 7deg/step, ret=2 # target will predictedly be reached in 2 steps
|
||||
pos[t1]: 7, speed[t1]: 3deg/step, ret=1 # target will predictedly be reached in 1 step (send final action)
|
||||
pos[t2]: 10, speed[t2]: 0deg/step, ret=0 # target was predictedly already reached
|
||||
pos[t3]: 10, speed[t3]: 0deg/step, ret=-1 # (best case scenario) server reported with delay, that target was reached (see tolerance)
|
||||
pos[t?]: 10, speed[t?]: 0deg/step, ret=-1 # if there is friction, it may take some additional steps
|
||||
|
||||
If everything worked as predicted we could stop calling this function when ret==1
|
||||
If we need precision, it is recommended to wait for ret==-1
|
||||
|
||||
Example 1:
|
||||
set_joints_target_position_direct(range(2,4),np.array([10.0,5.0]),harmonize=True)
|
||||
Joint[2] p[t0]: 0 target pos: 10 -> p[t1]=5, p[t2]=10
|
||||
Joint[3] p[t0]: 0 target pos: 5 -> p[t1]=2.5, p[t2]=5
|
||||
|
||||
Example 2:
|
||||
set_joints_target_position_direct([2,3],np.array([10.0,5.0]),harmonize=False)
|
||||
Joint[2] p[t0]: 0 target pos: 10 -> p[t1]=7, p[t2]=10
|
||||
Joint[3] p[t0]: 0 target pos: 5 -> p[t1]=5, p[t2]=5
|
||||
'''
|
||||
|
||||
assert type(values) == np.ndarray, "'values' argument must be a numpy array"
|
||||
np.nan_to_num(values, copy=False) # Replace NaN with zero and infinity with large finite numbers
|
||||
|
||||
# limit range of joints
|
||||
if limit_joints:
|
||||
if type(indices) == list or type(indices) == np.ndarray:
|
||||
for i in range(len(indices)):
|
||||
values[i] = np.clip(values[i], self.joints_info[indices[i]].min, self.joints_info[indices[i]].max)
|
||||
elif type(indices) == slice:
|
||||
info = self.joints_info[indices]
|
||||
for i in range(len(info)):
|
||||
values[i] = np.clip(values[i], info[i].min, info[i].max)
|
||||
else: # int
|
||||
values[0] = np.clip(values[0], self.joints_info[indices].min, self.joints_info[indices].max)
|
||||
|
||||
#predicted_diff: predicted difference between reported position and actual position
|
||||
|
||||
predicted_diff = self.joints_target_last_speed[indices] * 1.1459156 #rad/s to deg/step
|
||||
predicted_diff = np.asarray(predicted_diff)
|
||||
np.clip(predicted_diff,-7.03,7.03,out=predicted_diff) #saturate predicted movement in-place
|
||||
|
||||
#reported_dist: difference between reported position and target position
|
||||
|
||||
reported_dist = values - self.joints_position[indices]
|
||||
if np.all((np.abs(reported_dist) < tolerance)) and np.all((np.abs(predicted_diff) < tolerance)):
|
||||
self.joints_target_speed[indices] = 0
|
||||
return -1
|
||||
|
||||
deg_per_step = reported_dist - predicted_diff
|
||||
|
||||
relative_max = np.max( np.abs(deg_per_step) ) / max_speed
|
||||
remaining_steps = np.ceil( relative_max )
|
||||
|
||||
if remaining_steps == 0:
|
||||
self.joints_target_speed[indices] = 0
|
||||
return 0
|
||||
|
||||
if harmonize:
|
||||
deg_per_step /= remaining_steps
|
||||
else:
|
||||
np.clip(deg_per_step,-max_speed,max_speed,out=deg_per_step) #limit maximum speed
|
||||
|
||||
self.joints_target_speed[indices] = deg_per_step * 0.87266463 #convert to rad/s
|
||||
|
||||
return remaining_steps
|
||||
|
||||
|
||||
|
||||
def get_command(self) -> bytes:
|
||||
'''
|
||||
Builds commands string from self.joints_target_speed
|
||||
'''
|
||||
j_speed = self.joints_target_speed * self.FIX_EFFECTOR_MASK #Fix symmetry issues 3/4 (effectors)
|
||||
cmd = "".join(f"({self.joints_info[i].effector} {j_speed[i]:.5f})" for i in range(self.no_of_joints)).encode('utf-8')
|
||||
|
||||
self.joints_target_last_speed = self.joints_target_speed #1. both point to the same array
|
||||
self.joints_target_speed = np.zeros_like(self.joints_target_speed) #2. create new array for joints_target_speed
|
||||
return cmd
|
||||
431
world/World.py
Normal file
431
world/World.py
Normal file
@@ -0,0 +1,431 @@
|
||||
from collections import deque
|
||||
from cpp.ball_predictor import ball_predictor
|
||||
from cpp.localization import localization
|
||||
from logs.Logger import Logger
|
||||
from math import atan2, pi
|
||||
from math_ops.Matrix_4x4 import Matrix_4x4
|
||||
from world.commons.Draw import Draw
|
||||
from world.commons.Other_Robot import Other_Robot
|
||||
from world.Robot import Robot
|
||||
import numpy as np
|
||||
|
||||
|
||||
class World():
|
||||
STEPTIME = 0.02 # Fixed step time
|
||||
STEPTIME_MS = 20 # Fixed step time in milliseconds
|
||||
VISUALSTEP = 0.04 # Fixed visual step time
|
||||
VISUALSTEP_MS = 40 # Fixed visual step time in milliseconds
|
||||
|
||||
# play modes in our favor
|
||||
M_OUR_KICKOFF = 0
|
||||
M_OUR_KICK_IN = 1
|
||||
M_OUR_CORNER_KICK = 2
|
||||
M_OUR_GOAL_KICK = 3
|
||||
M_OUR_FREE_KICK = 4
|
||||
M_OUR_PASS = 5
|
||||
M_OUR_DIR_FREE_KICK = 6
|
||||
M_OUR_GOAL = 7
|
||||
M_OUR_OFFSIDE = 8
|
||||
|
||||
# play modes in their favor
|
||||
M_THEIR_KICKOFF = 9
|
||||
M_THEIR_KICK_IN = 10
|
||||
M_THEIR_CORNER_KICK = 11
|
||||
M_THEIR_GOAL_KICK = 12
|
||||
M_THEIR_FREE_KICK = 13
|
||||
M_THEIR_PASS = 14
|
||||
M_THEIR_DIR_FREE_KICK = 15
|
||||
M_THEIR_GOAL = 16
|
||||
M_THEIR_OFFSIDE = 17
|
||||
|
||||
# neutral play modes
|
||||
M_BEFORE_KICKOFF = 18
|
||||
M_GAME_OVER = 19
|
||||
M_PLAY_ON = 20
|
||||
|
||||
# play mode groups
|
||||
MG_OUR_KICK = 0
|
||||
MG_THEIR_KICK = 1
|
||||
MG_ACTIVE_BEAM = 2
|
||||
MG_PASSIVE_BEAM = 3
|
||||
MG_OTHER = 4 # play on, game over
|
||||
|
||||
FLAGS_CORNERS_POS = ((-15,-10,0), (-15,+10,0), (+15,-10,0), (+15,+10,0))
|
||||
FLAGS_POSTS_POS = ((-15,-1.05,0.8),(-15,+1.05,0.8),(+15,-1.05,0.8),(+15,+1.05,0.8))
|
||||
|
||||
def __init__(self,robot_type:int, team_name:str, unum:int, apply_play_mode_correction:bool,
|
||||
enable_draw:bool, logger:Logger, host:str) -> None:
|
||||
|
||||
self.team_name = team_name # Name of our team
|
||||
self.team_name_opponent : str = None # Name of opponent team
|
||||
self.apply_play_mode_correction = apply_play_mode_correction # True to adjust ball position according to play mode
|
||||
self.step = 0 # Total number of received simulation steps (always in sync with self.time_local_ms)
|
||||
self.time_server = 0.0 # Time, in seconds, as indicated by the server (this time is NOT reliable, use only for synchronization between agents)
|
||||
self.time_local_ms = 0 # Reliable simulation time in milliseconds, use this when possible (it is incremented 20ms for every TCP message)
|
||||
self.time_game = 0.0 # Game time, in seconds, as indicated by the server
|
||||
self.goals_scored = 0 # Goals score by our team
|
||||
self.goals_conceded = 0 # Goals conceded by our team
|
||||
self.team_side_is_left : bool = None # True if our team plays on the left side (this value is later changed by the world parser)
|
||||
self.play_mode = None # Play mode of the soccer game, provided by the server
|
||||
self.play_mode_group = None # Certain play modes share characteristics, so it makes sense to group them
|
||||
self.flags_corners : dict = None # corner flags, key=(x,y,z), always assume we play on the left side
|
||||
self.flags_posts : dict = None # goal posts, key=(x,y,z), always assume we play on the left side
|
||||
self.ball_rel_head_sph_pos = np.zeros(3) # Ball position relative to head (spherical coordinates) (m, deg, deg)
|
||||
self.ball_rel_head_cart_pos = np.zeros(3) # Ball position relative to head (cartesian coordinates) (m)
|
||||
self.ball_rel_torso_cart_pos = np.zeros(3) # Ball position relative to torso (cartesian coordinates) (m)
|
||||
self.ball_rel_torso_cart_pos_history = deque(maxlen=20) # Ball position relative to torso history (queue with up to 20 old positions at intervals of 0.04s, where index 0 is the previous position)
|
||||
self.ball_abs_pos = np.zeros(3) # Ball absolute position (up to date if self.ball_is_visible and self.robot.loc_is_up_to_date) (m)
|
||||
self.ball_abs_pos_history = deque(maxlen=20) # Ball absolute position history (queue with up to 20 old positions at intervals of 0.04s, where index 0 is the previous position)
|
||||
self.ball_abs_pos_last_update = 0 # World.time_local_ms when self.ball_abs_pos was last updated by vision or radio
|
||||
self.ball_abs_vel = np.zeros(3) # Ball velocity vector based on the last 2 known values of self.ball_abs_pos (m/s) (Warning: noisy if ball is distant, use instead get_ball_abs_vel)
|
||||
self.ball_abs_speed = 0 # Ball scalar speed based on the last 2 known values of self.ball_abs_pos (m/s) (Warning: noisy if ball is distant, use instead ||get_ball_abs_vel||)
|
||||
self.ball_is_visible = False # True if the last server message contained vision information related to the ball
|
||||
self.is_ball_abs_pos_from_vision = False # True if ball_abs_pos originated from vision, False if it originated from radio
|
||||
self.ball_last_seen = 0 # World.time_local_ms when ball was last seen (note: may be different from self.ball_abs_pos_last_update)
|
||||
self.ball_cheat_abs_pos = np.zeros(3) # Absolute ball position provided by the server as cheat (m)
|
||||
self.ball_cheat_abs_vel = np.zeros(3) # Absolute velocity vector based on the last 2 values of self.ball_cheat_abs_pos (m/s)
|
||||
self.ball_2d_pred_pos = np.zeros((1,2)) # prediction of current and future 2D ball positions*
|
||||
self.ball_2d_pred_vel = np.zeros((1,2)) # prediction of current and future 2D ball velocities*
|
||||
self.ball_2d_pred_spd = np.zeros(1) # prediction of current and future 2D ball linear speeds*
|
||||
# *at intervals of 0.02 s until ball comes to a stop or gets out of bounds (according to prediction)
|
||||
self.lines = np.zeros((30,6)) # Position of visible lines, relative to head, start_pos+end_pos (spherical coordinates) (m, deg, deg, m, deg, deg)
|
||||
self.line_count = 0 # Number of visible lines
|
||||
self.vision_last_update = 0 # World.time_local_ms when last vision update was received
|
||||
self.vision_is_up_to_date = False # True if the last server message contained vision information
|
||||
self.teammates = [Other_Robot(i, True ) for i in range(1,12)] # List of teammates, ordered by unum
|
||||
self.opponents = [Other_Robot(i, False) for i in range(1,12)] # List of opponents, ordered by unum
|
||||
self.teammates[unum-1].is_self = True # This teammate is self
|
||||
self.draw = Draw(enable_draw, unum, host, 32769) # Draw object for current player
|
||||
self.team_draw = Draw(enable_draw, 0, host, 32769) # Draw object shared with teammates
|
||||
self.logger = logger
|
||||
self.robot = Robot(unum, robot_type)
|
||||
|
||||
|
||||
def log(self, msg:str):
|
||||
'''
|
||||
Shortcut for:
|
||||
|
||||
self.logger.write(msg, True, self.step)
|
||||
|
||||
Parameters
|
||||
----------
|
||||
msg : str
|
||||
message to be written after the simulation step
|
||||
'''
|
||||
self.logger.write(msg, True, self.step)
|
||||
|
||||
def get_ball_rel_vel(self, history_steps:int):
|
||||
'''
|
||||
Get ball velocity, relative to torso (m/s)
|
||||
|
||||
Parameters
|
||||
----------
|
||||
history_steps : int
|
||||
number of history steps to consider [1,20]
|
||||
|
||||
Examples
|
||||
--------
|
||||
get_ball_rel_vel(1) is equivalent to (current rel pos - last rel pos) / 0.04
|
||||
get_ball_rel_vel(2) is equivalent to (current rel pos - rel pos 0.08s ago) / 0.08
|
||||
get_ball_rel_vel(3) is equivalent to (current rel pos - rel pos 0.12s ago) / 0.12
|
||||
'''
|
||||
assert 1 <= history_steps <= 20, "Argument 'history_steps' must be in range [1,20]"
|
||||
|
||||
if len(self.ball_rel_torso_cart_pos_history) == 0:
|
||||
return np.zeros(3)
|
||||
|
||||
h_step = min(history_steps, len(self.ball_rel_torso_cart_pos_history))
|
||||
t = h_step * World.VISUALSTEP
|
||||
|
||||
return (self.ball_rel_torso_cart_pos - self.ball_rel_torso_cart_pos_history[h_step-1]) / t
|
||||
|
||||
def get_ball_abs_vel(self, history_steps:int):
|
||||
'''
|
||||
Get ball absolute velocity (m/s)
|
||||
|
||||
Parameters
|
||||
----------
|
||||
history_steps : int
|
||||
number of history steps to consider [1,20]
|
||||
|
||||
Examples
|
||||
--------
|
||||
get_ball_abs_vel(1) is equivalent to (current abs pos - last abs pos) / 0.04
|
||||
get_ball_abs_vel(2) is equivalent to (current abs pos - abs pos 0.08s ago) / 0.08
|
||||
get_ball_abs_vel(3) is equivalent to (current abs pos - abs pos 0.12s ago) / 0.12
|
||||
'''
|
||||
assert 1 <= history_steps <= 20, "Argument 'history_steps' must be in range [1,20]"
|
||||
|
||||
if len(self.ball_abs_pos_history) == 0:
|
||||
return np.zeros(3)
|
||||
|
||||
h_step = min(history_steps, len(self.ball_abs_pos_history))
|
||||
t = h_step * World.VISUALSTEP
|
||||
|
||||
return (self.ball_abs_pos - self.ball_abs_pos_history[h_step-1]) / t
|
||||
|
||||
def get_predicted_ball_pos(self, max_speed):
|
||||
'''
|
||||
Get predicted 2D ball position when its predicted speed is equal to or less than `max_speed`
|
||||
In case that position exceeds the prediction horizon, the last available prediction is returned
|
||||
|
||||
Parameters
|
||||
----------
|
||||
max_speed : float
|
||||
maximum speed at which the ball will be moving at returned future position
|
||||
'''
|
||||
b_sp = self.ball_2d_pred_spd
|
||||
index = len(b_sp) - max( 1, np.searchsorted(b_sp[::-1], max_speed, side='right') )
|
||||
return self.ball_2d_pred_pos[index]
|
||||
|
||||
def get_intersection_point_with_ball(self, player_speed):
|
||||
'''
|
||||
Get 2D intersection point with moving ball, based on `self.ball_2d_pred_pos`
|
||||
|
||||
Parameters
|
||||
----------
|
||||
player_speed : float
|
||||
average speed at which the robot will chase the ball
|
||||
|
||||
Returns
|
||||
-------
|
||||
2D intersection point : ndarray
|
||||
2D intersection point with moving ball, assuming the robot moves at an avg. speed of `player_speed`
|
||||
intersection distance : float
|
||||
distance between current robot position and intersection point
|
||||
'''
|
||||
|
||||
params = np.array([*self.robot.loc_head_position[:2], player_speed*0.02, *self.ball_2d_pred_pos.flat], np.float32)
|
||||
pred_ret = ball_predictor.get_intersection(params)
|
||||
return pred_ret[:2], pred_ret[2]
|
||||
|
||||
def update(self):
|
||||
r = self.robot
|
||||
PM = self.play_mode
|
||||
W = World
|
||||
|
||||
# reset variables
|
||||
r.loc_is_up_to_date = False
|
||||
r.loc_head_z_is_up_to_date = False
|
||||
|
||||
# update play mode groups
|
||||
if PM in (W.M_PLAY_ON, W.M_GAME_OVER): # most common group
|
||||
self.play_mode_group = W.MG_OTHER
|
||||
elif PM in (W.M_OUR_KICKOFF, W.M_OUR_KICK_IN, W.M_OUR_CORNER_KICK, W.M_OUR_GOAL_KICK,
|
||||
W.M_OUR_OFFSIDE, W.M_OUR_PASS, W.M_OUR_DIR_FREE_KICK, W.M_OUR_FREE_KICK):
|
||||
self.play_mode_group = W.MG_OUR_KICK
|
||||
elif PM in (W.M_THEIR_KICK_IN, W.M_THEIR_CORNER_KICK, W.M_THEIR_GOAL_KICK, W.M_THEIR_OFFSIDE,
|
||||
W.M_THEIR_PASS, W.M_THEIR_DIR_FREE_KICK, W.M_THEIR_FREE_KICK, W.M_THEIR_KICKOFF):
|
||||
self.play_mode_group = W.MG_THEIR_KICK
|
||||
elif PM in (W.M_BEFORE_KICKOFF, W.M_THEIR_GOAL):
|
||||
self.play_mode_group = W.MG_ACTIVE_BEAM
|
||||
elif PM in (W.M_OUR_GOAL,):
|
||||
self.play_mode_group = W.MG_PASSIVE_BEAM
|
||||
elif PM is not None:
|
||||
raise ValueError(f'Unexpected play mode ID: {PM}')
|
||||
|
||||
r.update_pose() # update forward kinematics
|
||||
|
||||
if self.ball_is_visible:
|
||||
# Compute ball position, relative to torso
|
||||
self.ball_rel_torso_cart_pos = r.head_to_body_part_transform("torso",self.ball_rel_head_cart_pos)
|
||||
|
||||
if self.vision_is_up_to_date: # update vision based localization
|
||||
|
||||
# Prepare all variables for localization
|
||||
|
||||
feet_contact = np.zeros(6)
|
||||
|
||||
lf_contact = r.frp.get('lf', None)
|
||||
rf_contact = r.frp.get('rf', None)
|
||||
if lf_contact is not None:
|
||||
feet_contact[0:3] = Matrix_4x4( r.body_parts["lfoot"].transform ).translate( lf_contact[0:3] , True).get_translation()
|
||||
if rf_contact is not None:
|
||||
feet_contact[3:6] = Matrix_4x4( r.body_parts["rfoot"].transform ).translate( rf_contact[0:3] , True).get_translation()
|
||||
|
||||
ball_pos = np.concatenate(( self.ball_rel_head_cart_pos, self.ball_cheat_abs_pos))
|
||||
|
||||
corners_list = [[key in self.flags_corners, 1.0, *key, *self.flags_corners.get(key,(0,0,0))] for key in World.FLAGS_CORNERS_POS]
|
||||
posts_list = [[key in self.flags_posts , 0.0, *key, *self.flags_posts.get( key,(0,0,0))] for key in World.FLAGS_POSTS_POS]
|
||||
all_landmarks = np.array(corners_list + posts_list, float)
|
||||
|
||||
# Compute localization
|
||||
|
||||
loc = localization.compute(
|
||||
r.feet_toes_are_touching['lf'],
|
||||
r.feet_toes_are_touching['rf'],
|
||||
feet_contact,
|
||||
self.ball_is_visible,
|
||||
ball_pos,
|
||||
r.cheat_abs_pos,
|
||||
all_landmarks,
|
||||
self.lines[0:self.line_count])
|
||||
|
||||
r.update_localization(loc, self.time_local_ms)
|
||||
|
||||
# Update self in teammates list (only the most useful parameters, add as needed)
|
||||
me = self.teammates[r.unum-1]
|
||||
me.state_last_update = r.loc_last_update
|
||||
me.state_abs_pos = r.loc_head_position
|
||||
me.state_fallen = r.loc_head_z < 0.3 # uses same criterion as for other teammates - not as reliable as player.behavior.is_ready("Get_Up")
|
||||
me.state_orientation = r.loc_torso_orientation
|
||||
me.state_ground_area = (r.loc_head_position[:2],0.2) # relevant for localization demo
|
||||
|
||||
# Save last ball position to history at every vision cycle (even if not up to date)
|
||||
self.ball_abs_pos_history.appendleft(self.ball_abs_pos) # from vision or radio
|
||||
self.ball_rel_torso_cart_pos_history.appendleft(self.ball_rel_torso_cart_pos)
|
||||
|
||||
'''
|
||||
Get ball position based on vision or play mode
|
||||
Sources:
|
||||
Corner kick position - rcssserver3d/plugin/soccer/soccerruleaspect/soccerruleaspect.cpp:1927 (May 2022)
|
||||
Goal kick position - rcssserver3d/plugin/soccer/soccerruleaspect/soccerruleaspect.cpp:1900 (May 2022)
|
||||
'''
|
||||
ball = None
|
||||
if self.apply_play_mode_correction:
|
||||
if PM == W.M_OUR_CORNER_KICK:
|
||||
ball = np.array([15, 5.483 if self.ball_abs_pos[1] > 0 else -5.483, 0.042], float)
|
||||
elif PM == W.M_THEIR_CORNER_KICK:
|
||||
ball = np.array([-15, 5.483 if self.ball_abs_pos[1] > 0 else -5.483, 0.042], float)
|
||||
elif PM in [W.M_OUR_KICKOFF, W.M_THEIR_KICKOFF, W.M_OUR_GOAL, W.M_THEIR_GOAL]:
|
||||
ball = np.array([0, 0, 0.042], float)
|
||||
elif PM == W.M_OUR_GOAL_KICK:
|
||||
ball = np.array([-14, 0, 0.042], float)
|
||||
elif PM == W.M_THEIR_GOAL_KICK:
|
||||
ball = np.array([14, 0, 0.042], float)
|
||||
|
||||
# Discard hard-coded ball position if robot is near that position (in favor of its own vision)
|
||||
if ball is not None and np.linalg.norm(r.loc_head_position[:2] - ball[:2]) < 1:
|
||||
ball = None
|
||||
|
||||
if ball is None and self.ball_is_visible and r.loc_is_up_to_date:
|
||||
ball = r.loc_head_to_field_transform( self.ball_rel_head_cart_pos )
|
||||
ball[2] = max(ball[2], 0.042) # lowest z = ball radius
|
||||
if PM != W.M_BEFORE_KICKOFF: # for compatibility with tests without active soccer rules
|
||||
ball[:2] = np.clip(ball[:2], [-15,-10], [15,10]) # force ball position to be inside field
|
||||
|
||||
# Update internal ball position (also updated by Radio)
|
||||
if ball is not None:
|
||||
time_diff = (self.time_local_ms - self.ball_abs_pos_last_update) / 1000
|
||||
self.ball_abs_vel = (ball - self.ball_abs_pos) / time_diff
|
||||
self.ball_abs_speed = np.linalg.norm(self.ball_abs_vel)
|
||||
self.ball_abs_pos_last_update = self.time_local_ms
|
||||
self.ball_abs_pos = ball
|
||||
self.is_ball_abs_pos_from_vision = True
|
||||
|
||||
# Velocity decay for teammates and opponents (it is later neutralized if the velocity is updated)
|
||||
for p in self.teammates:
|
||||
p.state_filtered_velocity *= p.vel_decay
|
||||
for p in self.opponents:
|
||||
p.state_filtered_velocity *= p.vel_decay
|
||||
|
||||
# Update teammates and opponents
|
||||
if r.loc_is_up_to_date:
|
||||
for p in self.teammates:
|
||||
if not p.is_self: # if teammate is not self
|
||||
if p.is_visible: # if teammate is visible, execute full update
|
||||
self.update_other_robot(p)
|
||||
elif p.state_abs_pos is not None: # otherwise update its horizontal distance (assuming last known position)
|
||||
p.state_horizontal_dist = np.linalg.norm(r.loc_head_position[:2] - p.state_abs_pos[:2])
|
||||
|
||||
for p in self.opponents:
|
||||
if p.is_visible: # if opponent is visible, execute full update
|
||||
self.update_other_robot(p)
|
||||
elif p.state_abs_pos is not None: # otherwise update its horizontal distance (assuming last known position)
|
||||
p.state_horizontal_dist = np.linalg.norm(r.loc_head_position[:2] - p.state_abs_pos[:2])
|
||||
|
||||
# Update prediction of ball position/velocity
|
||||
if self.play_mode_group != W.MG_OTHER: # not 'play on' nor 'game over', so ball must be stationary
|
||||
self.ball_2d_pred_pos = self.ball_abs_pos[:2].copy().reshape(1, 2)
|
||||
self.ball_2d_pred_vel = np.zeros((1,2))
|
||||
self.ball_2d_pred_spd = np.zeros(1)
|
||||
|
||||
elif self.ball_abs_pos_last_update == self.time_local_ms: # make new prediction for new ball position (from vision or radio)
|
||||
|
||||
params = np.array([*self.ball_abs_pos[:2], *np.copy(self.get_ball_abs_vel(6)[:2])], np.float32)
|
||||
pred_ret = ball_predictor.predict_rolling_ball(params)
|
||||
sample_no = len(pred_ret) // 5 * 2
|
||||
self.ball_2d_pred_pos = pred_ret[:sample_no].reshape(-1, 2)
|
||||
self.ball_2d_pred_vel = pred_ret[sample_no:sample_no*2].reshape(-1, 2)
|
||||
self.ball_2d_pred_spd = pred_ret[sample_no*2:]
|
||||
|
||||
elif len(self.ball_2d_pred_pos) > 1: # otherwise, advance to next predicted step, if available
|
||||
self.ball_2d_pred_pos = self.ball_2d_pred_pos[1:]
|
||||
self.ball_2d_pred_vel = self.ball_2d_pred_vel[1:]
|
||||
self.ball_2d_pred_spd = self.ball_2d_pred_spd[1:]
|
||||
|
||||
r.update_imu(self.time_local_ms) # update imu (must be executed after localization)
|
||||
|
||||
|
||||
def update_other_robot(self,other_robot : Other_Robot):
|
||||
'''
|
||||
Update other robot state based on the relative position of visible body parts
|
||||
(also updated by Radio, with the exception of state_orientation)
|
||||
'''
|
||||
o = other_robot
|
||||
r = self.robot
|
||||
|
||||
# update body parts absolute positions
|
||||
o.state_body_parts_abs_pos = o.body_parts_cart_rel_pos.copy()
|
||||
for bp, pos in o.body_parts_cart_rel_pos.items():
|
||||
# Using the IMU could be beneficial if we see other robots but can't self-locate
|
||||
o.state_body_parts_abs_pos[bp] = r.loc_head_to_field_transform( pos, False )
|
||||
|
||||
# auxiliary variables
|
||||
bps_apos = o.state_body_parts_abs_pos # read-only shortcut
|
||||
bps_2d_apos_list = [v[:2] for v in bps_apos.values()] # list of body parts' 2D absolute positions
|
||||
avg_2d_pt = np.average(bps_2d_apos_list, axis=0) # 2D avg pos of visible body parts
|
||||
head_is_visible = 'head' in bps_apos
|
||||
|
||||
# evaluate robot's state (unchanged if head is not visible)
|
||||
if head_is_visible:
|
||||
o.state_fallen = bps_apos['head'][2] < 0.3
|
||||
|
||||
# compute velocity if head is visible
|
||||
if o.state_abs_pos is not None:
|
||||
time_diff = (self.time_local_ms - o.state_last_update) / 1000
|
||||
if head_is_visible:
|
||||
# if last position is 2D, we assume that the z coordinate did not change, so that v.z=0
|
||||
old_p = o.state_abs_pos if len(o.state_abs_pos)==3 else np.append(o.state_abs_pos, bps_apos['head'][2])
|
||||
velocity = (bps_apos['head'] - old_p) / time_diff
|
||||
decay = o.vel_decay # neutralize decay in all axes
|
||||
else: # if head is not visible, we only update the x & y components of the velocity
|
||||
velocity = np.append( (avg_2d_pt - o.state_abs_pos[:2]) / time_diff, 0)
|
||||
decay = (o.vel_decay,o.vel_decay,1) # neutralize decay (except in the z-axis)
|
||||
# apply filter
|
||||
if np.linalg.norm(velocity - o.state_filtered_velocity) < 4: # otherwise assume it was beamed
|
||||
o.state_filtered_velocity /= decay # neutralize decay
|
||||
o.state_filtered_velocity += o.vel_filter * (velocity-o.state_filtered_velocity)
|
||||
|
||||
# compute robot's position (preferably based on head)
|
||||
if head_is_visible:
|
||||
o.state_abs_pos = bps_apos['head'] # 3D head position, if head is visible
|
||||
else:
|
||||
o.state_abs_pos = avg_2d_pt # 2D avg pos of visible body parts
|
||||
|
||||
# compute robot's horizontal distance (head distance, or avg. distance of visible body parts)
|
||||
o.state_horizontal_dist = np.linalg.norm(r.loc_head_position[:2] - o.state_abs_pos[:2])
|
||||
|
||||
# compute orientation based on pair of lower arms or feet, or average of both
|
||||
lr_vec = None
|
||||
if 'llowerarm' in bps_apos and 'rlowerarm' in bps_apos:
|
||||
lr_vec = bps_apos['rlowerarm'] - bps_apos['llowerarm']
|
||||
|
||||
if 'lfoot' in bps_apos and 'rfoot' in bps_apos:
|
||||
if lr_vec is None:
|
||||
lr_vec = bps_apos['rfoot'] - bps_apos['lfoot']
|
||||
else:
|
||||
lr_vec = (lr_vec + (bps_apos['rfoot'] - bps_apos['lfoot'])) / 2
|
||||
|
||||
if lr_vec is not None:
|
||||
o.state_orientation = atan2(lr_vec[1],lr_vec[0]) * 180 / pi + 90
|
||||
|
||||
# compute projection of player area on ground (circle)
|
||||
if o.state_horizontal_dist < 4: # we don't need precision if the robot is farther than 4m
|
||||
max_dist = np.max(np.linalg.norm(bps_2d_apos_list - avg_2d_pt, axis=1))
|
||||
else:
|
||||
max_dist = 0.2
|
||||
o.state_ground_area = (avg_2d_pt,max_dist)
|
||||
|
||||
# update timestamp
|
||||
o.state_last_update = self.time_local_ms
|
||||
BIN
world/__pycache__/Robot.cpython-313.pyc
Normal file
BIN
world/__pycache__/Robot.cpython-313.pyc
Normal file
Binary file not shown.
BIN
world/__pycache__/World.cpython-313.pyc
Normal file
BIN
world/__pycache__/World.cpython-313.pyc
Normal file
Binary file not shown.
7
world/commons/Body_Part.py
Normal file
7
world/commons/Body_Part.py
Normal file
@@ -0,0 +1,7 @@
|
||||
from math_ops.Matrix_4x4 import Matrix_4x4
|
||||
|
||||
class Body_Part():
|
||||
def __init__(self, mass) -> None:
|
||||
self.mass = float(mass)
|
||||
self.joints = []
|
||||
self.transform = Matrix_4x4() # body part to head transformation matrix
|
||||
345
world/commons/Draw.py
Normal file
345
world/commons/Draw.py
Normal file
@@ -0,0 +1,345 @@
|
||||
import socket
|
||||
from math_ops.Math_Ops import Math_Ops as M
|
||||
import numpy as np
|
||||
|
||||
class Draw():
|
||||
_socket = None
|
||||
|
||||
def __init__(self, is_enabled:bool, unum:int, host:str, port:int) -> None:
|
||||
self.enabled = is_enabled
|
||||
self._is_team_right = None
|
||||
self._unum = unum
|
||||
self._prefix = f'?{unum}_'.encode() # temporary prefix that should never be used in normal circumstances
|
||||
|
||||
#Create one socket for all instances
|
||||
if Draw._socket is None:
|
||||
Draw._socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM )
|
||||
Draw._socket.connect((host, port))
|
||||
Draw.clear_all()
|
||||
|
||||
|
||||
def set_team_side(self, is_right):
|
||||
''' Called by world parser to switch side '''
|
||||
'''
|
||||
Generate an appropriate player ID
|
||||
RoboViz has a bug/feature: we send "swap buffers for player: 'l_1' and RoboViz
|
||||
will swap every buffer that contains 'l_1' in the name, including
|
||||
'l_10' and 'l_11'. To avoid that, we swap the separator to 'l-10', 'l-11'
|
||||
'''
|
||||
self._is_team_right = is_right
|
||||
self._prefix = f"{'r' if is_right else 'l'}{'_' if self._unum < 10 else '-'}{self._unum}_".encode() #e.g. b'l_5', b'l-10'
|
||||
|
||||
|
||||
@staticmethod
|
||||
def _send(msg, id, flush):
|
||||
''' Private method to send message if RoboViz is accessible '''
|
||||
try:
|
||||
if flush:
|
||||
Draw._socket.send(msg + id + b'\x00\x00\x00' + id + b'\x00')
|
||||
else:
|
||||
Draw._socket.send(msg + id + b'\x00')
|
||||
except ConnectionRefusedError:
|
||||
pass
|
||||
|
||||
|
||||
def circle(self, pos2d, radius, thickness, color:bytes, id:str, flush=True):
|
||||
'''
|
||||
Draw circle
|
||||
|
||||
Examples
|
||||
----------
|
||||
Circle in 2D (z=0): circle((-1,2), 3, 2, Draw.Color.red, "my_circle")
|
||||
'''
|
||||
if not self.enabled: return
|
||||
assert type(color)==bytes, "The RGB color must be a bytes object, e.g. red: b'\xFF\x00\x00'"
|
||||
assert not np.isnan(pos2d).any(), "Argument 'pos2d' contains 'nan' values"
|
||||
|
||||
if self._is_team_right:
|
||||
pos2d = (-pos2d[0],-pos2d[1])
|
||||
|
||||
msg = b'\x01\x00' + (
|
||||
f'{f"{pos2d[0] :.4f}":.6s}'
|
||||
f'{f"{pos2d[1] :.4f}":.6s}'
|
||||
f'{f"{radius :.4f}":.6s}'
|
||||
f'{f"{thickness :.4f}":.6s}').encode() + color
|
||||
|
||||
Draw._send(msg, self._prefix + id.encode(), flush)
|
||||
|
||||
|
||||
def line(self, p1, p2, thickness, color:bytes, id:str, flush=True):
|
||||
'''
|
||||
Draw line
|
||||
|
||||
Examples
|
||||
----------
|
||||
Line in 3D: line((0,0,0), (0,0,2), 3, Draw.Color.red, "my_line")
|
||||
Line in 2D (z=0): line((0,0), (0,1), 3, Draw.Color.red, "my_line")
|
||||
'''
|
||||
if not self.enabled: return
|
||||
assert type(color)==bytes, "The RGB color must be a bytes object, e.g. red: b'\xFF\x00\x00'"
|
||||
assert not np.isnan(p1).any(), "Argument 'p1' contains 'nan' values"
|
||||
assert not np.isnan(p2).any(), "Argument 'p2' contains 'nan' values"
|
||||
|
||||
z1 = p1[2] if len(p1)==3 else 0
|
||||
z2 = p2[2] if len(p2)==3 else 0
|
||||
|
||||
if self._is_team_right:
|
||||
p1 = (-p1[0],-p1[1],p1[2]) if len(p1)==3 else (-p1[0],-p1[1])
|
||||
p2 = (-p2[0],-p2[1],p2[2]) if len(p2)==3 else (-p2[0],-p2[1])
|
||||
|
||||
msg = b'\x01\x01' + (
|
||||
f'{f"{p1[0] :.4f}":.6s}'
|
||||
f'{f"{p1[1] :.4f}":.6s}'
|
||||
f'{f"{z1 :.4f}":.6s}'
|
||||
f'{f"{p2[0] :.4f}":.6s}'
|
||||
f'{f"{p2[1] :.4f}":.6s}'
|
||||
f'{f"{z2 :.4f}":.6s}'
|
||||
f'{f"{thickness :.4f}":.6s}').encode() + color
|
||||
|
||||
Draw._send(msg, self._prefix + id.encode(), flush)
|
||||
|
||||
|
||||
def point(self, pos, size, color:bytes, id:str, flush=True):
|
||||
'''
|
||||
Draw point
|
||||
|
||||
Examples
|
||||
----------
|
||||
Point in 3D: point((1,1,1), 3, Draw.Color.red, "my_point")
|
||||
Point in 2D (z=0): point((1,1), 3, Draw.Color.red, "my_point")
|
||||
'''
|
||||
if not self.enabled: return
|
||||
assert type(color)==bytes, "The RGB color must be a bytes object, e.g. red: b'\xFF\x00\x00'"
|
||||
assert not np.isnan(pos).any(), "Argument 'pos' contains 'nan' values"
|
||||
|
||||
z = pos[2] if len(pos)==3 else 0
|
||||
|
||||
if self._is_team_right:
|
||||
pos = (-pos[0],-pos[1],pos[2]) if len(pos)==3 else (-pos[0],-pos[1])
|
||||
|
||||
msg = b'\x01\x02' + (
|
||||
f'{f"{pos[0] :.4f}":.6s}'
|
||||
f'{f"{pos[1] :.4f}":.6s}'
|
||||
f'{f"{z :.4f}":.6s}'
|
||||
f'{f"{size :.4f}":.6s}').encode() + color
|
||||
|
||||
Draw._send(msg, self._prefix + id.encode(), flush)
|
||||
|
||||
|
||||
def sphere(self, pos, radius, color:bytes, id:str, flush=True):
|
||||
'''
|
||||
Draw sphere
|
||||
|
||||
Examples
|
||||
----------
|
||||
Sphere in 3D: sphere((1,1,1), 3, Draw.Color.red, "my_sphere")
|
||||
Sphere in 2D (z=0): sphere((1,1), 3, Draw.Color.red, "my_sphere")
|
||||
'''
|
||||
if not self.enabled: return
|
||||
assert type(color)==bytes, "The RGB color must be a bytes object, e.g. red: b'\xFF\x00\x00'"
|
||||
assert not np.isnan(pos).any(), "Argument 'pos' contains 'nan' values"
|
||||
|
||||
z = pos[2] if len(pos)==3 else 0
|
||||
|
||||
if self._is_team_right:
|
||||
pos = (-pos[0],-pos[1],pos[2]) if len(pos)==3 else (-pos[0],-pos[1])
|
||||
|
||||
msg = b'\x01\x03' + (
|
||||
f'{f"{pos[0] :.4f}":.6s}'
|
||||
f'{f"{pos[1] :.4f}":.6s}'
|
||||
f'{f"{z :.4f}":.6s}'
|
||||
f'{f"{radius :.4f}":.6s}').encode() + color
|
||||
|
||||
Draw._send(msg, self._prefix + id.encode(), flush)
|
||||
|
||||
|
||||
def polygon(self, vertices, color:bytes, alpha:int, id:str, flush=True):
|
||||
'''
|
||||
Draw polygon
|
||||
|
||||
Examples
|
||||
----------
|
||||
Polygon in 3D: polygon(((0,0,0),(1,0,0),(0,1,0)), Draw.Color.red, 255, "my_polygon")
|
||||
'''
|
||||
if not self.enabled: return
|
||||
assert type(color)==bytes, "The RGB color must be a bytes object, e.g. red: b'\xFF\x00\x00'"
|
||||
assert 0<=alpha<=255, "The alpha channel (degree of opacity) must be in range [0,255]"
|
||||
|
||||
if self._is_team_right:
|
||||
vertices = [(-v[0],-v[1],v[2]) for v in vertices]
|
||||
|
||||
msg = b'\x01\x04' + bytes([len(vertices)]) + color + alpha.to_bytes(1,'big')
|
||||
|
||||
for v in vertices:
|
||||
msg += (
|
||||
f'{f"{v[0] :.4f}":.6s}'
|
||||
f'{f"{v[1] :.4f}":.6s}'
|
||||
f'{f"{v[2] :.4f}":.6s}').encode()
|
||||
|
||||
Draw._send(msg, self._prefix + id.encode(), flush)
|
||||
|
||||
|
||||
def annotation(self, pos, text, color:bytes, id:str, flush=True):
|
||||
'''
|
||||
Draw annotation
|
||||
|
||||
Examples
|
||||
----------
|
||||
Annotation in 3D: annotation((1,1,1), "SOMEtext!", Draw.Color.red, "my_annotation")
|
||||
Annotation in 2D (z=0): annotation((1,1), "SOMEtext!", Draw.Color.red, "my_annotation")
|
||||
'''
|
||||
if not self.enabled: return
|
||||
if type(text) != bytes: text = str(text).encode()
|
||||
assert type(color)==bytes, "The RGB color must be a bytes object, e.g. red: b'\xFF\x00\x00'"
|
||||
z = pos[2] if len(pos)==3 else 0
|
||||
|
||||
if self._is_team_right:
|
||||
pos = (-pos[0],-pos[1],pos[2]) if len(pos)==3 else (-pos[0],-pos[1])
|
||||
|
||||
msg = b'\x02\x00' + (
|
||||
f'{f"{pos[0] :.4f}":.6s}'
|
||||
f'{f"{pos[1] :.4f}":.6s}'
|
||||
f'{f"{z :.4f}":.6s}').encode() + color + text + b'\x00'
|
||||
|
||||
Draw._send(msg, self._prefix + id.encode(), flush)
|
||||
|
||||
|
||||
def arrow(self, p1, p2, arrowhead_size, thickness, color:bytes, id:str, flush=True):
|
||||
'''
|
||||
Draw arrow
|
||||
|
||||
Examples
|
||||
----------
|
||||
Arrow in 3D: arrow((0,0,0), (0,0,2), 0.1, 3, Draw.Color.red, "my_arrow")
|
||||
Arrow in 2D (z=0): arrow((0,0), (0,1), 0.1, 3, Draw.Color.red, "my_arrow")
|
||||
'''
|
||||
if not self.enabled: return
|
||||
assert type(color)==bytes, "The RGB color must be a bytes object, e.g. red: b'\xFF\x00\x00'"
|
||||
|
||||
# No need to invert sides, the called shapes will handle that
|
||||
if len(p1)==2: p1 = M.to_3d(p1)
|
||||
else: p1 = np.asarray(p1)
|
||||
if len(p2)==2: p2 = M.to_3d(p2)
|
||||
else: p2 = np.asarray(p2)
|
||||
|
||||
vec = p2-p1
|
||||
vec_size = np.linalg.norm(vec)
|
||||
if vec_size == 0: return #return without warning/error
|
||||
if arrowhead_size > vec_size: arrowhead_size = vec_size
|
||||
|
||||
ground_proj_perpendicular = np.array([ vec[1], -vec[0], 0 ])
|
||||
|
||||
if np.all(ground_proj_perpendicular == 0): #vertical arrow
|
||||
ground_proj_perpendicular = np.array([ arrowhead_size/2, 0, 0 ])
|
||||
else:
|
||||
ground_proj_perpendicular *= arrowhead_size/2 / np.linalg.norm(ground_proj_perpendicular)
|
||||
|
||||
head_start = p2 - vec * (arrowhead_size/vec_size)
|
||||
head_pt1 = head_start + ground_proj_perpendicular
|
||||
head_pt2 = head_start - ground_proj_perpendicular
|
||||
|
||||
self.line(p1,p2,thickness,color,id,False)
|
||||
self.line(p2,head_pt1,thickness,color,id,False)
|
||||
self.line(p2,head_pt2,thickness,color,id,flush)
|
||||
|
||||
|
||||
def flush(self, id):
|
||||
''' Flush specific drawing by ID '''
|
||||
if not self.enabled: return
|
||||
|
||||
Draw._send(b'\x00\x00', self._prefix + id.encode(), False)
|
||||
|
||||
def clear(self, id):
|
||||
''' Clear specific drawing by ID '''
|
||||
if not self.enabled: return
|
||||
|
||||
Draw._send(b'\x00\x00', self._prefix + id.encode(), True) #swap buffer twice
|
||||
|
||||
|
||||
def clear_player(self):
|
||||
''' Clear all drawings made by this player '''
|
||||
if not self.enabled: return
|
||||
|
||||
Draw._send(b'\x00\x00', self._prefix, True) #swap buffer twice
|
||||
|
||||
|
||||
@staticmethod
|
||||
def clear_all():
|
||||
''' Clear all drawings of all players '''
|
||||
if Draw._socket is not None:
|
||||
Draw._send(b'\x00\x00\x00\x00\x00',b'',False) #swap buffer twice using no id
|
||||
|
||||
|
||||
class Color():
|
||||
'''
|
||||
Based on X11 colors
|
||||
The names are restructured to make better suggestions
|
||||
'''
|
||||
pink_violet = b'\xC7\x15\x85'
|
||||
pink_hot = b'\xFF\x14\x93'
|
||||
pink_violet_pale = b'\xDB\x70\x93'
|
||||
pink = b'\xFF\x69\xB4'
|
||||
pink_pale = b'\xFF\xB6\xC1'
|
||||
|
||||
red_dark = b'\x8B\x00\x00'
|
||||
red = b'\xFF\x00\x00'
|
||||
red_brick = b'\xB2\x22\x22'
|
||||
red_crimson = b'\xDC\x14\x3C'
|
||||
red_indian = b'\xCD\x5C\x5C'
|
||||
red_salmon = b'\xFA\x80\x72'
|
||||
|
||||
orange_red = b'\xFF\x45\x00'
|
||||
orange = b'\xFF\x8C\x00'
|
||||
orange_ligth = b'\xFF\xA5\x00'
|
||||
|
||||
yellow_gold = b'\xFF\xD7\x00'
|
||||
yellow = b'\xFF\xFF\x00'
|
||||
yellow_light = b'\xBD\xB7\x6B'
|
||||
|
||||
brown_maroon =b'\x80\x00\x00'
|
||||
brown_dark = b'\x8B\x45\x13'
|
||||
brown = b'\xA0\x52\x2D'
|
||||
brown_gold = b'\xB8\x86\x0B'
|
||||
brown_light = b'\xCD\x85\x3F'
|
||||
brown_pale = b'\xDE\xB8\x87'
|
||||
|
||||
green_dark = b'\x00\x64\x00'
|
||||
green = b'\x00\x80\x00'
|
||||
green_lime = b'\x32\xCD\x32'
|
||||
green_light = b'\x00\xFF\x00'
|
||||
green_lawn = b'\x7C\xFC\x00'
|
||||
green_pale = b'\x90\xEE\x90'
|
||||
|
||||
cyan_dark = b'\x00\x80\x80'
|
||||
cyan_medium = b'\x00\xCE\xD1'
|
||||
cyan = b'\x00\xFF\xFF'
|
||||
cyan_light = b'\xAF\xEE\xEE'
|
||||
|
||||
blue_dark = b'\x00\x00\x8B'
|
||||
blue = b'\x00\x00\xFF'
|
||||
blue_royal = b'\x41\x69\xE1'
|
||||
blue_medium = b'\x1E\x90\xFF'
|
||||
blue_light = b'\x00\xBF\xFF'
|
||||
blue_pale = b'\x87\xCE\xEB'
|
||||
|
||||
purple_violet = b'\x94\x00\xD3'
|
||||
purple_magenta = b'\xFF\x00\xFF'
|
||||
purple_light = b'\xBA\x55\xD3'
|
||||
purple_pale = b'\xDD\xA0\xDD'
|
||||
|
||||
white = b'\xFF\xFF\xFF'
|
||||
gray_10 = b'\xE6\xE6\xE6'
|
||||
gray_20 = b'\xCC\xCC\xCC'
|
||||
gray_30 = b'\xB2\xB2\xB2'
|
||||
gray_40 = b'\x99\x99\x99'
|
||||
gray_50 = b'\x80\x80\x80'
|
||||
gray_60 = b'\x66\x66\x66'
|
||||
gray_70 = b'\x4C\x4C\x4C'
|
||||
gray_80 = b'\x33\x33\x33'
|
||||
gray_90 = b'\x1A\x1A\x1A'
|
||||
black = b'\x00\x00\x00'
|
||||
|
||||
@staticmethod
|
||||
def get(r,g,b):
|
||||
''' Get RGB color (0-255) '''
|
||||
return bytes([int(r),int(g),int(b)])
|
||||
24
world/commons/Joint_Info.py
Normal file
24
world/commons/Joint_Info.py
Normal file
@@ -0,0 +1,24 @@
|
||||
import numpy as np
|
||||
|
||||
class Joint_Info():
|
||||
def __init__(self, xml_element) -> None:
|
||||
self.perceptor = xml_element.attrib['perceptor']
|
||||
self.effector = xml_element.attrib['effector']
|
||||
self.axes = np.array([
|
||||
float(xml_element.attrib['xaxis']),
|
||||
float(xml_element.attrib['yaxis']),
|
||||
float(xml_element.attrib['zaxis'])])
|
||||
self.min = int(xml_element.attrib['min'])
|
||||
self.max = int(xml_element.attrib['max'])
|
||||
|
||||
self.anchor0_part = xml_element[0].attrib['part']
|
||||
self.anchor0_axes = np.array([
|
||||
float(xml_element[0].attrib['y']),
|
||||
float(xml_element[0].attrib['x']),
|
||||
float(xml_element[0].attrib['z'])]) #x and y axes are switched
|
||||
|
||||
self.anchor1_part = xml_element[1].attrib['part']
|
||||
self.anchor1_axes_neg = np.array([
|
||||
-float(xml_element[1].attrib['y']),
|
||||
-float(xml_element[1].attrib['x']),
|
||||
-float(xml_element[1].attrib['z'])]) #x and y axes are switched
|
||||
28
world/commons/Other_Robot.py
Normal file
28
world/commons/Other_Robot.py
Normal file
@@ -0,0 +1,28 @@
|
||||
import numpy as np
|
||||
|
||||
#Note: When other robot is seen, all previous body part positions are deleted
|
||||
# E.g. we see 5 body parts at 0 seconds -> body_parts_cart_rel_pos contains 5 elements
|
||||
# we see 1 body part at 1 seconds -> body_parts_cart_rel_pos contains 1 element
|
||||
|
||||
|
||||
class Other_Robot():
|
||||
def __init__(self, unum, is_teammate) -> None:
|
||||
self.unum = unum # convenient variable to indicate uniform number (same as other robot's index + 1)
|
||||
self.is_self = False # convenient flag to indicate if this robot is self
|
||||
self.is_teammate = is_teammate # convenient variable to indicate if this robot is from our team
|
||||
self.is_visible = False # True if this robot was seen in the last message from the server (it doesn't mean we know its absolute location)
|
||||
self.body_parts_cart_rel_pos = dict() # cartesian relative position of the robot's visible body parts
|
||||
self.body_parts_sph_rel_pos = dict() # spherical relative position of the robot's visible body parts
|
||||
self.vel_filter = 0.3 # EMA filter coefficient applied to self.state_filtered_velocity
|
||||
self.vel_decay = 0.95 # velocity decay at every vision cycle (neutralized if velocity is updated)
|
||||
|
||||
|
||||
# State variables: these are computed when this robot is visible and when the original robot is able to self-locate
|
||||
self.state_fallen = False # true if the robot is lying down (updated when head is visible)
|
||||
self.state_last_update = 0 # World.time_local_ms when the state was last updated
|
||||
self.state_horizontal_dist = 0 # horizontal head distance if head is visible, otherwise, average horizontal distance of visible body parts (the distance is updated by vision or radio when state_abs_pos gets a new value, but also when the other player is not visible, by assuming its last position)
|
||||
self.state_abs_pos = None # 3D head position if head is visible, otherwise, 2D average position of visible body parts, or, 2D radio head position
|
||||
self.state_orientation = 0 # orientation based on pair of lower arms or feet, or average of both (WARNING: may be older than state_last_update)
|
||||
self.state_ground_area = None # (pt_2d,radius) projection of player area on ground (circle), not precise if farther than 3m (for performance), useful for obstacle avoidance when it falls
|
||||
self.state_body_parts_abs_pos = dict() # 3D absolute position of each body part
|
||||
self.state_filtered_velocity = np.zeros(3) # 3D filtered velocity (m/s) (if the head is not visible, the 2D part is updated and v.z decays)
|
||||
583
world/commons/Path_Manager.py
Normal file
583
world/commons/Path_Manager.py
Normal file
@@ -0,0 +1,583 @@
|
||||
from cpp.a_star import a_star
|
||||
from math_ops.Math_Ops import Math_Ops as M
|
||||
from world.World import World
|
||||
import math
|
||||
import numpy as np
|
||||
|
||||
|
||||
class Path_Manager():
|
||||
MODE_CAUTIOUS = 0
|
||||
MODE_DRIBBLE = 1 # safety margins are increased
|
||||
MODE_AGGRESSIVE = 2 # safety margins are reduced for opponents
|
||||
|
||||
STATUS_SUCCESS = 0 # the pathfinding algorithm was executed normally
|
||||
STATUS_TIMEOUT = 1 # timeout before the target was reached (may be impossible)
|
||||
STATUS_IMPOSSIBLE = 2 # impossible to reach target (all options were tested)
|
||||
STATUS_DIRECT = 3 # no obstacles between start and target (path contains only 2 points: the start and target)
|
||||
|
||||
HOT_START_DIST_WALK = 0.05 # hot start prediction distance (when walking)
|
||||
HOT_START_DIST_DRIBBLE = 0.10 # hot start prediction distance (when dribbling)
|
||||
|
||||
def __init__(self, world : World) -> None:
|
||||
self.world = world
|
||||
|
||||
self._draw_obstacles = False # enabled by function 'draw_options'
|
||||
self._draw_path = False # enabled by function 'draw_options'
|
||||
self._use_team_channel = False # enabled by function 'draw_options'
|
||||
|
||||
# internal variables to bootstrap the path to start from a prediction (to reduce path instability)
|
||||
self.last_direction_rad = None
|
||||
self.last_update = 0
|
||||
self.last_start_dist = None
|
||||
|
||||
def draw_options(self, enable_obstacles, enable_path, use_team_drawing_channel=False):
|
||||
'''
|
||||
Enable or disable drawings, and change drawing channel
|
||||
If self.world.draw.enable is False, these options are ignored
|
||||
|
||||
Parameters
|
||||
----------
|
||||
enable_obstacles : bool
|
||||
draw relevant obstacles for path planning
|
||||
enable_path : bool
|
||||
draw computed path
|
||||
use_team_drawing_channel : bool
|
||||
True to use team drawing channel, otherwise use individual channel
|
||||
Using individual channels for each player means that drawings with the same name can coexist
|
||||
With the team channel, drawings with the same name will replace previous drawings, even if drawn by a teammate
|
||||
'''
|
||||
self._draw_obstacles = enable_obstacles
|
||||
self._draw_path = enable_path
|
||||
self._use_team_channel = use_team_drawing_channel
|
||||
|
||||
def get_obstacles(self, include_teammates, include_opponents, include_play_mode_restrictions, max_distance = 4, max_age = 500,
|
||||
ball_safety_margin = 0, goalpost_safety_margin = 0, mode = MODE_CAUTIOUS, priority_unums=[]):
|
||||
'''
|
||||
Parameters
|
||||
----------
|
||||
include_teammates : bool
|
||||
include teammates in the returned list of obstacles
|
||||
include_opponents : bool
|
||||
include opponents in the returned list of obstacles
|
||||
max_distance : float
|
||||
teammates or opponents are only considered if they are closer than `max_distance` (meters)
|
||||
max_age : float
|
||||
teammates or opponents are only considered if they were seen in the last `max_age` (milliseconds)
|
||||
ball_safety_margin : float
|
||||
minimum value for the ball's soft repulsion radius
|
||||
this value is increased when the game is stopped, and when the ball is almost out of bounds
|
||||
default is zero, the ball is ignored
|
||||
goalpost_safety_margin : float
|
||||
hard repulsion radius around the opponents' goalposts
|
||||
default is zero, uses the minimum margin
|
||||
mode : int
|
||||
overall attitude towards safety margins (concerns teammates and opponents)
|
||||
priority_unums : list
|
||||
list of teammates to avoid (since their role is more important)
|
||||
|
||||
Returns
|
||||
-------
|
||||
obstacles : list
|
||||
list of obstacles, where each obstacle is a tuple of 5 floats (x, y, hard radius, soft radius, repulsive force)
|
||||
'''
|
||||
w = self.world
|
||||
|
||||
ball_2d = w.ball_abs_pos[:2]
|
||||
obstacles = []
|
||||
|
||||
# 'comparator' is a variable local to the lambda, which captures the current value of (w.time_local_ms - max_age)
|
||||
check_age = lambda last_update, comparator = w.time_local_ms - max_age : last_update > 0 and last_update >= comparator
|
||||
|
||||
#---------------------------------------------- Get recently seen close teammates
|
||||
if include_teammates:
|
||||
soft_radius = 1.1 if mode == Path_Manager.MODE_DRIBBLE else 0.6 # soft radius: repulsive force is max at center and fades
|
||||
|
||||
def get_hard_radius(t):
|
||||
if t.unum in priority_unums:
|
||||
return 1.0 # extra distance for priority roles
|
||||
else:
|
||||
return t.state_ground_area[1]+0.2
|
||||
|
||||
# Get close teammates (center, hard radius, soft radius, force)
|
||||
obstacles.extend( (*t.state_ground_area[0],
|
||||
get_hard_radius(t),
|
||||
1.5 if t.unum in priority_unums else soft_radius,
|
||||
1.0) # repulsive force
|
||||
for t in w.teammates if not t.is_self and check_age(t.state_last_update) and t.state_horizontal_dist < max_distance)
|
||||
|
||||
#---------------------------------------------- Get recently seen close opponents
|
||||
if include_opponents:
|
||||
|
||||
# soft radius: repulsive force is max at center and fades
|
||||
if mode == Path_Manager.MODE_AGGRESSIVE:
|
||||
soft_radius = 0.6
|
||||
hard_radius = lambda o : 0.2
|
||||
elif mode == Path_Manager.MODE_DRIBBLE:
|
||||
soft_radius = 2.3
|
||||
hard_radius = lambda o : o.state_ground_area[1]+0.9
|
||||
else:
|
||||
soft_radius = 1.0
|
||||
hard_radius = lambda o : o.state_ground_area[1]+0.2
|
||||
|
||||
# Get close opponents (center, hard radius, soft radius, force)
|
||||
obstacles.extend( (*o.state_ground_area[0],
|
||||
hard_radius(o),
|
||||
soft_radius,
|
||||
1.5 if o.unum == 1 else 1.0) # repulsive force (extra for their GK)
|
||||
for o in w.opponents if o.state_last_update > 0 and w.time_local_ms - o.state_last_update <= max_age and o.state_horizontal_dist < max_distance)
|
||||
|
||||
#---------------------------------------------- Get play mode restrictions
|
||||
if include_play_mode_restrictions:
|
||||
if w.play_mode == World.M_THEIR_GOAL_KICK:
|
||||
obstacles.extend((15,i,2.1,0,0) for i in range(-2,3)) # 5 circular obstacles to cover their goal area
|
||||
elif w.play_mode == World.M_THEIR_PASS:
|
||||
obstacles.append((*ball_2d, 1.2, 0, 0))
|
||||
elif w.play_mode in [World.M_THEIR_KICK_IN,World.M_THEIR_CORNER_KICK,World.M_THEIR_FREE_KICK,World.M_THEIR_DIR_FREE_KICK, World.M_THEIR_OFFSIDE]:
|
||||
obstacles.append((*ball_2d, 2.5, 0, 0))
|
||||
|
||||
#---------------------------------------------- Get ball
|
||||
if ball_safety_margin > 0:
|
||||
|
||||
# increase ball safety margin in certain game scenarios
|
||||
if (w.play_mode_group != w.MG_OTHER) or abs(ball_2d[1])>9.5 or abs(ball_2d[0])>14.5:
|
||||
ball_safety_margin += 0.12
|
||||
|
||||
obstacles.append((*ball_2d, 0, ball_safety_margin, 8))
|
||||
|
||||
#---------------------------------------------- Get goal posts
|
||||
if goalpost_safety_margin > 0:
|
||||
obstacles.append((14.75, 1.10,goalpost_safety_margin,0,0))
|
||||
obstacles.append((14.75,-1.10,goalpost_safety_margin,0,0))
|
||||
|
||||
#---------------------------------------------- Draw obstacles
|
||||
if self._draw_obstacles:
|
||||
d = w.team_draw if self._use_team_channel else w.draw
|
||||
if d.enabled:
|
||||
for o in obstacles:
|
||||
if o[3] > 0: d.circle(o[:2],o[3],o[4]/2, d.Color.orange, "path_obstacles", False)
|
||||
if o[2] > 0: d.circle(o[:2],o[2],1, d.Color.red, "path_obstacles", False)
|
||||
d.flush("path_obstacles")
|
||||
|
||||
return obstacles
|
||||
|
||||
def _get_hot_start(self, start_distance):
|
||||
'''
|
||||
Get hot start position for path (considering the previous path)
|
||||
(as opposed to a cold start, where the path starts at the player)
|
||||
'''
|
||||
if self.last_update > 0 and self.world.time_local_ms - self.last_update == 20 and self.last_start_dist == start_distance:
|
||||
return self.world.robot.loc_head_position[:2] + M.vector_from_angle(self.last_direction_rad, is_rad = True) * start_distance
|
||||
else:
|
||||
return self.world.robot.loc_head_position[:2] # return cold start if start_distance was different or the position was not updated in the last step
|
||||
|
||||
def _update_hot_start(self, next_dir_rad, start_distance):
|
||||
''' Update hot start position for next run '''
|
||||
self.last_direction_rad = next_dir_rad
|
||||
self.last_update = self.world.time_local_ms
|
||||
self.last_start_dist = start_distance
|
||||
|
||||
def _extract_target_from_path(self, path, path_len, ret_segments):
|
||||
ret_seg_ceil = math.ceil(ret_segments)
|
||||
|
||||
if path_len >= ret_seg_ceil:
|
||||
i = ret_seg_ceil * 2 # path index of ceil point (x)
|
||||
if ret_seg_ceil == ret_segments:
|
||||
return path[i:i+2]
|
||||
else:
|
||||
floor_w = ret_seg_ceil-ret_segments
|
||||
return path[i-2:i] * floor_w + path[i:i+2] * (1-floor_w)
|
||||
else:
|
||||
return path[-2:] # path end
|
||||
|
||||
|
||||
def get_path_to_ball(self, x_ori = None, x_dev = -0.2, y_dev = 0, torso_ori = None, torso_ori_thrsh = 1,
|
||||
priority_unums:list=[], is_aggressive=True, safety_margin = 0.25, timeout = 3000):
|
||||
'''
|
||||
Get next target from path to ball (next absolute position + next absolute orientation)
|
||||
If the robot is an active player, and close to the ball, it makes sense to be aggressive
|
||||
If the robot is far, it should follow the role_position instead to predict the intersection with ball
|
||||
|
||||
|
||||
Parameters
|
||||
----------
|
||||
x_ori : float
|
||||
(This variable allows the specification of a target position, relative to the ball, in a custom reference frame.)
|
||||
absolute orientation of the custom reference frame's x-axis
|
||||
if None, the orientation is given by the vector (robot->ball)
|
||||
x_dev : float
|
||||
(This variable allows the specification of a target position, relative to the ball, in a custom reference frame.)
|
||||
target position deviation, in the custom reference frame's x-axis
|
||||
y_dev : float
|
||||
(This variable allows the specification of a target position, relative to the ball, in a custom reference frame.)
|
||||
target position deviation, in the custom reference frame's y-axis
|
||||
torso_ori : float
|
||||
torso's target absolute orientation (see `torso_ori_thrsh`)
|
||||
if None, the orientation is given by the vector (robot->target)
|
||||
torso_ori_thrsh : float
|
||||
`torso_ori` will only be applied when the distance between robot and final target is < `torso_ori_thrsh` meters
|
||||
otherwise, the robot will orient itself towards the final target
|
||||
priority_unums : list
|
||||
list of teammates to avoid (since their role is more important)
|
||||
is_aggressive : bool
|
||||
if True, safety margins are reduced for opponents
|
||||
safety_margin : float
|
||||
repulsion radius around ball to avoid colliding with it
|
||||
timeout : float
|
||||
maximum execution time (in microseconds)
|
||||
|
||||
Returns
|
||||
-------
|
||||
next_pos : ndarray
|
||||
next absolute position from path to ball
|
||||
next_ori : float
|
||||
next absolute orientation
|
||||
distance : float
|
||||
minimum between (distance to final target) and (distance to ball)
|
||||
|
||||
|
||||
Example
|
||||
-------
|
||||
----------------------------------------------------------------------------------------------
|
||||
x_ori | x_dev | y_dev | torso_ori | OBS
|
||||
-------------+---------+---------+-------------+----------------------------------------------
|
||||
None => | - | !0 | - | Not recommended. Will not converge.
|
||||
(orient. of: | 0 | 0 | None | Frontal ball chase, expected* slow approach
|
||||
robot->ball) | 0 | 0 | value | Oriented ball chase, expected* slow approach
|
||||
| >0 | 0 | - | Not recommended. Will not converge.
|
||||
| <0 | 0 | None | Frontal ball chase until distance == x_dev
|
||||
| <0 | 0 | value | Oriented ball chase until distance == x_dev
|
||||
-------------+---------+---------+-------------+----------------------------------------------
|
||||
value | - | - | None | Frontal point chase
|
||||
| - | - | value | Oriented point chase
|
||||
----------------------------------------------------------------------------------------------
|
||||
* it depends on the caller function (expected slow walking near target)
|
||||
`torso_ori` will only be applied when the distance between robot and final target is < `torso_ori_thrsh` meters
|
||||
'''
|
||||
|
||||
w = self.world
|
||||
r = w.robot
|
||||
dev = np.array([x_dev,y_dev])
|
||||
dev_len = np.linalg.norm(dev)
|
||||
dev_mult = 1
|
||||
|
||||
# use ball prediction if we are further than 0.5 m and in PlayOn
|
||||
if np.linalg.norm(w.ball_abs_pos[:2] - r.loc_head_position[:2]) > 0.5 and w.play_mode_group == w.MG_OTHER:
|
||||
ball_2d = w.get_intersection_point_with_ball(0.4)[0] # intersection point, while moving at 0.4 m/s
|
||||
else:
|
||||
ball_2d = w.ball_abs_pos[:2]
|
||||
|
||||
# custom reference frame orientation
|
||||
vec_me_ball = ball_2d - r.loc_head_position[:2]
|
||||
if x_ori is None:
|
||||
x_ori = M.vector_angle(vec_me_ball)
|
||||
|
||||
distance_boost = 0 # boost for returned distance to target
|
||||
if torso_ori is not None and dev_len > 0:
|
||||
approach_ori_diff = abs(M.normalize_deg( r.imu_torso_orientation - torso_ori ))
|
||||
if approach_ori_diff > 15: # increase walking speed near target if robot is far from approach orientation
|
||||
distance_boost = 0.15
|
||||
if approach_ori_diff > 30: # increase target distance to ball if robot is far from approach orientation
|
||||
dev_mult = 1.3
|
||||
if approach_ori_diff > 45: # increase safety margin around ball if robot is far from approach orientation
|
||||
safety_margin = max(0.32,safety_margin)
|
||||
|
||||
#------------------------------------------- get target
|
||||
|
||||
front_unit_vec = M.vector_from_angle(x_ori)
|
||||
left_unit_vec = np.array([-front_unit_vec[1], front_unit_vec[0]])
|
||||
|
||||
rel_target = front_unit_vec * dev[0] + left_unit_vec * dev[1]
|
||||
target = ball_2d + rel_target * dev_mult
|
||||
target_vec = target - r.loc_head_position[:2]
|
||||
target_dist = np.linalg.norm(target_vec)
|
||||
|
||||
if self._draw_path:
|
||||
d = self.world.team_draw if self._use_team_channel else self.world.draw
|
||||
d.point(target, 4, d.Color.red, "path_target") # will not draw if drawing object is internally disabled
|
||||
|
||||
#------------------------------------------- get obstacles
|
||||
|
||||
# Ignore ball if we are on the same side of the target (with small margin)
|
||||
if dev_len>0 and np.dot(vec_me_ball, rel_target) < -0.10:
|
||||
safety_margin = 0
|
||||
|
||||
obstacles = self.get_obstacles(include_teammates = True, include_opponents = True, include_play_mode_restrictions = True,
|
||||
ball_safety_margin = safety_margin,
|
||||
mode = Path_Manager.MODE_AGGRESSIVE if is_aggressive else Path_Manager.MODE_CAUTIOUS,
|
||||
priority_unums = priority_unums)
|
||||
|
||||
# Add obstacle on the side opposite to the target
|
||||
if dev_len>0 and safety_margin > 0:
|
||||
center = ball_2d - M.normalize_vec( rel_target ) * safety_margin
|
||||
obstacles.append((*center, 0, safety_margin*0.9, 5))
|
||||
if self._draw_obstacles:
|
||||
d = w.team_draw if self._use_team_channel else w.draw
|
||||
if d.enabled:
|
||||
d.circle(center,safety_margin*0.8,2.5, d.Color.orange, "path_obstacles_1")
|
||||
|
||||
#------------------------------------------- get path
|
||||
|
||||
# see explanation for the context at the hot start update section below
|
||||
start_pos = self._get_hot_start(Path_Manager.HOT_START_DIST_WALK) if target_dist > 0.4 else self.world.robot.loc_head_position[:2]
|
||||
|
||||
path, path_len, path_status, path_cost = self.get_path(start_pos, True, obstacles, target, timeout)
|
||||
path_end = path[-2:] # last position allowed by A*
|
||||
|
||||
#------------------------------------------- get relevant distances
|
||||
|
||||
if w.ball_last_seen > w.time_local_ms - w.VISUALSTEP_MS: # ball is in FOV
|
||||
raw_ball_dist = np.linalg.norm(w.ball_rel_torso_cart_pos[:2]) # - distance between torso center and ball center
|
||||
else: # otherwise use absolute coordinates to compute distance
|
||||
raw_ball_dist = np.linalg.norm(vec_me_ball) # - distance between head center and ball center
|
||||
|
||||
avoid_touching_ball = (w.play_mode_group != w.MG_OTHER)
|
||||
distance_to_final_target = np.linalg.norm(path_end - r.loc_head_position[:2])
|
||||
distance_to_ball = max(0.07 if avoid_touching_ball else 0.14, raw_ball_dist - 0.13)
|
||||
caution_dist = min(distance_to_ball,distance_to_final_target)
|
||||
|
||||
#------------------------------------------- get next target position
|
||||
|
||||
next_pos = self._extract_target_from_path( path, path_len, ret_segments=1 if caution_dist < 1 else 2 )
|
||||
|
||||
#------------------------------------------ get next target orientation
|
||||
|
||||
# use given orientation if it exists, else target's orientation if far enough, else current orientation
|
||||
if torso_ori is not None:
|
||||
|
||||
if caution_dist > torso_ori_thrsh:
|
||||
next_ori = M.vector_angle(target_vec)
|
||||
else:
|
||||
mid_ori = M.normalize_deg( M.vector_angle(vec_me_ball) - M.vector_angle(-dev) - x_ori + torso_ori )
|
||||
mid_ori_diff = abs(M.normalize_deg(mid_ori - r.imu_torso_orientation))
|
||||
final_ori_diff = abs(M.normalize_deg(torso_ori - r.imu_torso_orientation))
|
||||
next_ori = mid_ori if mid_ori_diff + 10 < final_ori_diff else torso_ori
|
||||
|
||||
elif target_dist > 0.1:
|
||||
next_ori = M.vector_angle(target_vec)
|
||||
else:
|
||||
next_ori = r.imu_torso_orientation
|
||||
|
||||
#------------------------------------------ update hot start for next run
|
||||
|
||||
''' Defining the hot start distance:
|
||||
- if path_len is zero, there is no hot start, because we are already there (dist=0)
|
||||
- if the target is close, the hot start is not applied (see above)
|
||||
- if the next pos is very close (due to hard obstacle), the hot start is the next pos (dist<Path_Manager.HOT_START_DIST_WALK)
|
||||
- otherwise, the hot start distance is constant (dist=Path_Manager.HOT_START_DIST_WALK)
|
||||
'''
|
||||
if path_len != 0:
|
||||
next_pos_vec = next_pos - self.world.robot.loc_head_position[:2]
|
||||
next_pos_dist = np.linalg.norm(next_pos_vec)
|
||||
self._update_hot_start(M.vector_angle(next_pos_vec, is_rad=True), min(Path_Manager.HOT_START_DIST_WALK,next_pos_dist))
|
||||
|
||||
return next_pos, next_ori, min(distance_to_ball, distance_to_final_target + distance_boost)
|
||||
|
||||
|
||||
def get_path_to_target(self, target, ret_segments = 1.0, torso_ori = None, priority_unums:list=[], is_aggressive=True, timeout = 3000):
|
||||
'''
|
||||
Get next position from path to target (next absolute position + next absolute orientation)
|
||||
|
||||
Parameters
|
||||
----------
|
||||
ret_segments : float
|
||||
returned target's maximum distance (measured in path segments from hot start position)
|
||||
actual distance: min(ret_segments,path_length)
|
||||
each path segment has 0.10 m or 0.1*sqrt(2) m (if diagonal)
|
||||
if `ret_segments` is 0, the current position is returned
|
||||
torso_ori : float
|
||||
torso's target absolute orientation
|
||||
if None, the orientation is given by the vector (robot->target)
|
||||
priority_unums : list
|
||||
list of teammates to avoid (since their role is more important)
|
||||
is_aggressive : bool
|
||||
if True, safety margins are reduced for opponents
|
||||
timeout : float
|
||||
maximum execution time (in microseconds)
|
||||
'''
|
||||
|
||||
w = self.world
|
||||
|
||||
#------------------------------------------- get target
|
||||
|
||||
target_vec = target - w.robot.loc_head_position[:2]
|
||||
target_dist = np.linalg.norm(target_vec)
|
||||
|
||||
#------------------------------------------- get obstacles
|
||||
|
||||
obstacles = self.get_obstacles(include_teammates = True, include_opponents = True, include_play_mode_restrictions = True,
|
||||
mode = Path_Manager.MODE_AGGRESSIVE if is_aggressive else Path_Manager.MODE_CAUTIOUS, priority_unums = priority_unums)
|
||||
|
||||
#------------------------------------------- get path
|
||||
|
||||
# see explanation for the context at the hot start update section below
|
||||
start_pos = self._get_hot_start(Path_Manager.HOT_START_DIST_WALK) if target_dist > 0.4 else self.world.robot.loc_head_position[:2]
|
||||
|
||||
path, path_len, path_status, path_cost = self.get_path(start_pos, True, obstacles, target, timeout)
|
||||
path_end = path[-2:] # last position allowed by A*
|
||||
|
||||
#------------------------------------------- get next target position
|
||||
next_pos = self._extract_target_from_path(path, path_len, ret_segments)
|
||||
|
||||
#------------------------------------------ get next target orientation
|
||||
|
||||
# use given orientation if it exists, else target's orientation if far enough, else current orientation
|
||||
if torso_ori is not None:
|
||||
next_ori = torso_ori
|
||||
elif target_dist > 0.1:
|
||||
next_ori = M.vector_angle(target_vec)
|
||||
else:
|
||||
next_ori = w.robot.imu_torso_orientation
|
||||
|
||||
#------------------------------------------ update hot start for next run
|
||||
|
||||
''' Defining the hot start distance:
|
||||
- if path_len is zero, there is no hot start, because we are already there (dist=0)
|
||||
- if the target is close, the hot start is not applied (see above)
|
||||
- if the next pos is very close (due to hard obstacle), the hot start is the next pos (dist<Path_Manager.HOT_START_DIST_WALK)
|
||||
- otherwise, the hot start distance is constant (dist=Path_Manager.HOT_START_DIST_WALK)
|
||||
'''
|
||||
if path_len != 0:
|
||||
next_pos_vec = next_pos - self.world.robot.loc_head_position[:2]
|
||||
next_pos_dist = np.linalg.norm(next_pos_vec)
|
||||
self._update_hot_start(M.vector_angle(next_pos_vec, is_rad=True), min(Path_Manager.HOT_START_DIST_WALK,next_pos_dist))
|
||||
|
||||
|
||||
distance_to_final_target = np.linalg.norm(path_end - w.robot.loc_head_position[:2])
|
||||
|
||||
return next_pos, next_ori, distance_to_final_target
|
||||
|
||||
|
||||
def get_dribble_path(self, ret_segments = None, optional_2d_target = None, goalpost_safety_margin=0.4, timeout = 3000):
|
||||
'''
|
||||
Get next position from path to target (next relative orientation)
|
||||
Path is optimized for dribble
|
||||
|
||||
Parameters
|
||||
----------
|
||||
ret_segments : float
|
||||
returned target's maximum distance (measured in path segments from hot start position)
|
||||
actual distance: min(ret_segments,path_length)
|
||||
each path segment has 0.10 m or 0.1*sqrt(2) m (if diagonal)
|
||||
if `ret_segments` is 0, the current position is returned
|
||||
if `ret_segments` is None, it is dynamically set according to the robot's speed
|
||||
optional_2d_target : float
|
||||
2D target
|
||||
if None, the target is the opponent's goal (the specific goal point is decided by the A* algorithm)
|
||||
goalpost_safety_margin : float
|
||||
hard repulsion radius around the opponents' goalposts
|
||||
if zero, the minimum margin is used
|
||||
timeout : float
|
||||
maximum execution time (in microseconds)
|
||||
'''
|
||||
|
||||
r = self.world.robot
|
||||
ball_2d = self.world.ball_abs_pos[:2]
|
||||
|
||||
#------------------------------------------- get obstacles
|
||||
|
||||
obstacles = self.get_obstacles(include_teammates = True, include_opponents = True, include_play_mode_restrictions = False,
|
||||
max_distance=5, max_age=1000, goalpost_safety_margin=goalpost_safety_margin, mode = Path_Manager.MODE_DRIBBLE)
|
||||
|
||||
#------------------------------------------- get path
|
||||
|
||||
start_pos = self._get_hot_start(Path_Manager.HOT_START_DIST_DRIBBLE)
|
||||
|
||||
path, path_len, path_status, path_cost = self.get_path(start_pos, False, obstacles, optional_2d_target, timeout)
|
||||
|
||||
#------------------------------------------- get next target position & orientation
|
||||
|
||||
if ret_segments is None:
|
||||
ret_segments = 2.0
|
||||
|
||||
next_pos = self._extract_target_from_path(path, path_len, ret_segments)
|
||||
next_rel_ori = M.normalize_deg(M.vector_angle(next_pos - ball_2d) - r.imu_torso_orientation)
|
||||
|
||||
#------------------------------------------ update hot start for next run
|
||||
|
||||
if path_len != 0:
|
||||
self._update_hot_start(M.deg_to_rad(r.imu_torso_orientation), Path_Manager.HOT_START_DIST_DRIBBLE)
|
||||
|
||||
#------------------------------------------ draw
|
||||
if self._draw_path and path_status != Path_Manager.STATUS_DIRECT:
|
||||
d = self.world.team_draw if self._use_team_channel else self.world.draw
|
||||
d.point(next_pos, 2, d.Color.pink, "path_next_pos",False) # will not draw if drawing object is internally disabled
|
||||
d.line(ball_2d, next_pos, 2, d.Color.pink, "path_next_pos") # will not draw if drawing object is internally disabled
|
||||
|
||||
return next_pos, next_rel_ori
|
||||
|
||||
|
||||
def get_push_path(self, ret_segments = 1.5, optional_2d_target = None, avoid_opponents=False, timeout = 3000):
|
||||
'''
|
||||
Get next position from path ball to target (next absolute position)
|
||||
Path is optimized for critical push (no obstacles, also for preparation stability)
|
||||
|
||||
Parameters
|
||||
----------
|
||||
ret_segments : float
|
||||
returned target's maximum distance (measured in path segments from hot start position)
|
||||
actual distance: min(ret_segments,path_length)
|
||||
each path segment has 0.10 m or 0.1*sqrt(2) m (if diagonal)
|
||||
if `ret_segments` is 0, the current position is returned
|
||||
optional_2d_target : float
|
||||
2D target
|
||||
if None, the target is the opponent's goal (the specific goal point is decided by the A* algorithm)
|
||||
timeout : float
|
||||
maximum execution time (in microseconds)
|
||||
'''
|
||||
|
||||
ball_2d = self.world.ball_abs_pos[:2]
|
||||
|
||||
#------------------------------------------- get obstacles
|
||||
|
||||
obstacles = self.get_obstacles(include_teammates = False, include_opponents = avoid_opponents, include_play_mode_restrictions = False)
|
||||
|
||||
#------------------------------------------- get path
|
||||
|
||||
path, path_len, path_status, path_cost = self.get_path(ball_2d, False, obstacles, optional_2d_target, timeout)
|
||||
|
||||
#------------------------------------------- get next target position & orientation
|
||||
|
||||
next_pos = self._extract_target_from_path(path, path_len, ret_segments)
|
||||
|
||||
return next_pos
|
||||
|
||||
def get_path(self, start, allow_out_of_bounds, obstacles=[], optional_2d_target = None, timeout = 3000):
|
||||
'''
|
||||
Parameters
|
||||
----------
|
||||
allow_out_of_bounds : bool
|
||||
allow path to go out of bounds, should be False when dribbling
|
||||
obstacles : list
|
||||
list of obstacles, where each obstacle is a tuple of 5 floats (x, y, hard radius, soft radius, repulsive force)
|
||||
optional_2d_target : float
|
||||
2D target
|
||||
if None, the target is the opponent's goal (the specific goal point is decided by the A* algorithm)
|
||||
timeout : float
|
||||
maximum execution time (in microseconds)
|
||||
'''
|
||||
|
||||
go_to_goal = int(optional_2d_target is None)
|
||||
|
||||
if optional_2d_target is None:
|
||||
optional_2d_target = (0,0)
|
||||
|
||||
# flatten obstacles
|
||||
obstacles = sum(obstacles, tuple())
|
||||
assert len(obstacles) % 5 == 0, "Each obstacle should be characterized by exactly 5 float values"
|
||||
|
||||
# Path parameters: start, allow_out_of_bounds, go_to_goal, optional_target, timeout (us), obstacles
|
||||
params = np.array([*start, int(allow_out_of_bounds), go_to_goal, *optional_2d_target, timeout, *obstacles], np.float32)
|
||||
path_ret = a_star.compute(params)
|
||||
path = path_ret[:-2]
|
||||
path_status = path_ret[-2]
|
||||
|
||||
#---------------------------------------------- Draw path segments
|
||||
if self._draw_path:
|
||||
d = self.world.team_draw if self._use_team_channel else self.world.draw
|
||||
if d.enabled:
|
||||
c = {0: d.Color.green_lawn, 1: d.Color.yellow, 2: d.Color.red, 3: d.Color.cyan}[path_status]
|
||||
for j in range(0, len(path)-2, 2):
|
||||
d.line((path[j],path[j+1]),(path[j+2],path[j+3]), 1, c, "path_segments", False)
|
||||
d.flush("path_segments")
|
||||
|
||||
return path, len(path)//2-1, path_status, path_ret[-1] # path, path_len (number of segments), path_status, path_cost (A* cost)
|
||||
BIN
world/commons/__pycache__/Body_Part.cpython-313.pyc
Normal file
BIN
world/commons/__pycache__/Body_Part.cpython-313.pyc
Normal file
Binary file not shown.
BIN
world/commons/__pycache__/Draw.cpython-313.pyc
Normal file
BIN
world/commons/__pycache__/Draw.cpython-313.pyc
Normal file
Binary file not shown.
BIN
world/commons/__pycache__/Joint_Info.cpython-313.pyc
Normal file
BIN
world/commons/__pycache__/Joint_Info.cpython-313.pyc
Normal file
Binary file not shown.
BIN
world/commons/__pycache__/Other_Robot.cpython-313.pyc
Normal file
BIN
world/commons/__pycache__/Other_Robot.cpython-313.pyc
Normal file
Binary file not shown.
BIN
world/commons/__pycache__/Path_Manager.cpython-313.pyc
Normal file
BIN
world/commons/__pycache__/Path_Manager.cpython-313.pyc
Normal file
Binary file not shown.
165
world/commons/robots/nao0.xml
Normal file
165
world/commons/robots/nao0.xml
Normal file
@@ -0,0 +1,165 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
|
||||
<agentmodel name="nao" type="humanoid" rsgfile="nao/nao_hetero.rsg 0">
|
||||
|
||||
<bodypart name="head" mass="0.35"/>
|
||||
<bodypart name="neck" mass="0.05"/>
|
||||
<bodypart name="torso" mass="1.2171"/>
|
||||
<bodypart name="lshoulder" mass="0.07"/>
|
||||
<bodypart name="rshoulder" mass="0.07"/>
|
||||
<bodypart name="lupperarm" mass="0.15"/>
|
||||
<bodypart name="rupperarm" mass="0.15"/>
|
||||
<bodypart name="lelbow" mass="0.035"/>
|
||||
<bodypart name="relbow" mass="0.035"/>
|
||||
<bodypart name="llowerarm" mass="0.2"/>
|
||||
<bodypart name="rlowerarm" mass="0.2"/>
|
||||
<bodypart name="lhip1" mass="0.09"/>
|
||||
<bodypart name="rhip1" mass="0.09"/>
|
||||
<bodypart name="lhip2" mass="0.125"/>
|
||||
<bodypart name="rhip2" mass="0.125"/>
|
||||
<bodypart name="lthigh" mass="0.275"/>
|
||||
<bodypart name="rthigh" mass="0.275"/>
|
||||
<bodypart name="lshank" mass="0.225"/>
|
||||
<bodypart name="rshank" mass="0.225"/>
|
||||
<bodypart name="lankle" mass="0.125"/>
|
||||
<bodypart name="rankle" mass="0.125"/>
|
||||
<bodypart name="lfoot" mass="0.2"/>
|
||||
<bodypart name="rfoot" mass="0.2"/>
|
||||
|
||||
<!-- joint 0 -->
|
||||
<joint name="head1" perceptor="hj1" effector="he1" xaxis="0" yaxis="0" zaxis="-1" min="-120" max="120">
|
||||
<anchor index="0" part="neck" x="0" y="0" z="0.0"/>
|
||||
<anchor index="1" part="torso" x="0" y="0" z="0.09"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 1 -->
|
||||
<joint name="head2" perceptor="hj2" effector="he2" xaxis="0" yaxis="1" zaxis="0" min="-45" max="45">
|
||||
<anchor index="0" part="head" x="0" y="0" z="-0.005"/>
|
||||
<anchor index="1" part="neck" x="0" y="0" z="0.065"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 2 -->
|
||||
<joint name="lleg1" perceptor="llj1" effector="lle1" yaxis="0.7071" xaxis="0" zaxis="-0.7071" min="-90" max="1">
|
||||
<anchor index="0" part="torso" x="0.055" y="-0.01" z="-0.115"/>
|
||||
<anchor index="1" part="lhip1" x="0" y="0" z="0"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 3 -->
|
||||
<joint name="rleg1" perceptor="rlj1" effector="rle1" yaxis="0.7071" xaxis="0" zaxis="0.7071" min="-90" max="1">
|
||||
<anchor index="0" part="torso" x="-0.055" y="-0.01" z="-0.115"/>
|
||||
<anchor index="1" part="rhip1" x="0" y="0" z="0"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 4 -->
|
||||
<joint name="lleg2" perceptor="llj2" effector="lle2" yaxis="0" xaxis="1" zaxis="0" min="-25" max="45">
|
||||
<anchor index="0" part="lhip1" x="0" y="0" z="0"/>
|
||||
<anchor index="1" part="lhip2" x="0" y="0" z="0"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 5 -->
|
||||
<joint name="rleg2" perceptor="rlj2" effector="rle2" yaxis="0" xaxis="1" zaxis="0" min="-45" max="25">
|
||||
<anchor index="0" part="rhip1" x="0" y="0" z="0"/>
|
||||
<anchor index="1" part="rhip2" x="0" y="0" z="0"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 6 -->
|
||||
<joint name="lleg3" perceptor="llj3" effector="lle3" xaxis="0" yaxis="-1" zaxis="0" min="-25" max="100">
|
||||
<anchor index="0" part="lhip2" x="0" y="0" z="0"/>
|
||||
<anchor index="1" part="lthigh" x="0" y="-0.01" z="0.04"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 7 -->
|
||||
<joint name="rleg3" perceptor="rlj3" effector="rle3" xaxis="0" yaxis="-1" zaxis="0" min="-25" max="100">
|
||||
<anchor index="0" part="rhip2" x="0" y="0" z="0"/>
|
||||
<anchor index="1" part="rthigh" x="0" y="-0.01" z="0.04"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 8 -->
|
||||
<joint name="lleg4" perceptor="llj4" effector="lle4" xaxis="0" yaxis="-1" zaxis="0" min="-130" max="1">
|
||||
<anchor index="0" part="lthigh" x="0" y="-0.005" z="-0.08"/>
|
||||
<anchor index="1" part="lshank" x="0" y="-0.01" z="0.045"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 9 -->
|
||||
<joint name="rleg4" perceptor="rlj4" effector="rle4" xaxis="0" yaxis="-1" zaxis="0" min="-130" max="1">
|
||||
<anchor index="0" part="rthigh" x="0" y="-0.005" z="-0.08"/>
|
||||
<anchor index="1" part="rshank" x="0" y="-0.01" z="0.045"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 10 -->
|
||||
<joint name="lleg5" perceptor="llj5" effector="lle5" xaxis="0" yaxis="-1" zaxis="0" min="-45" max="75">
|
||||
<anchor index="0" part="lshank" x="0" y="-0.01" z="-0.055"/>
|
||||
<anchor index="1" part="lankle" x="0" y="0" z="0"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 11 -->
|
||||
<joint name="rleg5" perceptor="rlj5" effector="rle5" xaxis="0" yaxis="-1" zaxis="0" min="-45" max="75">
|
||||
<anchor index="0" part="rshank" x="0" y="-0.01" z="-0.055"/>
|
||||
<anchor index="1" part="rankle" x="0" y="0" z="0"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 12 -->
|
||||
<joint name="lleg6" perceptor="llj6" effector="lle6" yaxis="0" xaxis="1" zaxis="0" min="-45" max="25">
|
||||
<anchor index="0" part="lankle" x="0" y="0" z="0"/>
|
||||
<anchor index="1" part="lfoot" x="0" y="-0.03" z="0.04"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 13 -->
|
||||
<joint name="rleg6" perceptor="rlj6" effector="rle6" yaxis="0" xaxis="1" zaxis="0" min="-25" max="45">
|
||||
<anchor index="0" part="rankle" x="0" y="0" z="0"/>
|
||||
<anchor index="1" part="rfoot" x="0" y="-0.03" z="0.04"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 14 -->
|
||||
<joint name="larm1" perceptor="laj1" effector="lae1" yaxis="-1" xaxis="0" zaxis="0" min="-120" max="120">
|
||||
<anchor index="0" part="torso" x="0.098" y="0" z="0.075"/>
|
||||
<anchor index="1" part="lshoulder" x="0" y="0" z="0"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 15 -->
|
||||
<joint name="rarm1" perceptor="raj1" effector="rae1" yaxis="-1" xaxis="0" zaxis="0" min="-120" max="120">
|
||||
<anchor index="0" part="torso" x="-0.098" y="0" z="0.075"/>
|
||||
<anchor index="1" part="rshoulder" x="0" y="0" z="0"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 16 -->
|
||||
<joint name="larm2" perceptor="laj2" effector="lae2" xaxis="0" yaxis="0" zaxis="1" min="-1" max="95">
|
||||
<anchor index="0" part="lshoulder" x="0" y="0" z="0"/>
|
||||
<anchor index="1" part="lupperarm" x="-0.01" y="-0.02" z="0"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 17 -->
|
||||
<joint name="rarm2" perceptor="raj2" effector="rae2" xaxis="0" yaxis="0" zaxis="1" min="-95" max="1">
|
||||
<anchor index="0" part="rshoulder" x="0" y="0" z="0"/>
|
||||
<anchor index="1" part="rupperarm" x="0.01" y="-0.02" z="0"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 18 -->
|
||||
<joint name="larm3" perceptor="laj3" effector="lae3" yaxis="0" xaxis="1" zaxis="0" min="-120" max="120">
|
||||
<anchor index="0" part="lupperarm" x="-0.01" y="0.07" z="0.009"/>
|
||||
<anchor index="1" part="lelbow" x="0" y="0" z="0"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 19 -->
|
||||
<joint name="rarm3" perceptor="raj3" effector="rae3" yaxis="0" xaxis="1" zaxis="0" min="-120" max="120">
|
||||
<anchor index="0" part="rupperarm" x="0.01" y="0.07" z="0.009"/>
|
||||
<anchor index="1" part="relbow" x="0" y="0" z="0"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 20 -->
|
||||
<joint name="larm4" perceptor="laj4" effector="lae4" xaxis="0" yaxis="0" zaxis="1" min="-90" max="1">
|
||||
<anchor index="0" part="lelbow" x="0" y="0" z="0"/>
|
||||
<anchor index="1" part="llowerarm" x="0" y="-0.05" z="0"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 21 -->
|
||||
<joint name="rarm4" perceptor="raj4" effector="rae4" xaxis="0" yaxis="0" zaxis="1" min="-1" max="90">
|
||||
<anchor index="0" part="relbow" x="0" y="0" z="0"/>
|
||||
<anchor index="1" part="rlowerarm" x="0" y="-0.05" z="0"/>
|
||||
</joint>
|
||||
|
||||
|
||||
<joint name="lleg7" perceptor="llj7" effector="lle7" xaxis="0" yaxis="0" zaxis="0" min="0" max="0"/>
|
||||
<joint name="rleg7" perceptor="rlj7" effector="rle7" xaxis="0" yaxis="0" zaxis="0" min="0" max="0"/>
|
||||
|
||||
</agentmodel>
|
||||
161
world/commons/robots/nao1.xml
Normal file
161
world/commons/robots/nao1.xml
Normal file
@@ -0,0 +1,161 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
|
||||
<agentmodel name="nao" type="humanoid" rsgfile="nao/nao_hetero.rsg 1">
|
||||
|
||||
<bodypart name="head" mass="0.35"/>
|
||||
<bodypart name="neck" mass="0.05"/>
|
||||
<bodypart name="torso" mass="1.2171"/>
|
||||
<bodypart name="lshoulder" mass="0.07"/>
|
||||
<bodypart name="rshoulder" mass="0.07"/>
|
||||
<bodypart name="lupperarm" mass="0.15"/>
|
||||
<bodypart name="rupperarm" mass="0.15"/>
|
||||
<bodypart name="lelbow" mass="0.035"/>
|
||||
<bodypart name="relbow" mass="0.035"/>
|
||||
<bodypart name="llowerarm" mass="0.2"/>
|
||||
<bodypart name="rlowerarm" mass="0.2"/>
|
||||
<bodypart name="lhip1" mass="0.09"/>
|
||||
<bodypart name="rhip1" mass="0.09"/>
|
||||
<bodypart name="lhip2" mass="0.125"/>
|
||||
<bodypart name="rhip2" mass="0.125"/>
|
||||
<bodypart name="lthigh" mass="0.275"/>
|
||||
<bodypart name="rthigh" mass="0.275"/>
|
||||
<bodypart name="lshank" mass="0.225"/>
|
||||
<bodypart name="rshank" mass="0.225"/>
|
||||
<bodypart name="lankle" mass="0.125"/>
|
||||
<bodypart name="rankle" mass="0.125"/>
|
||||
<bodypart name="lfoot" mass="0.2"/>
|
||||
<bodypart name="rfoot" mass="0.2"/>
|
||||
|
||||
<!-- joint 0 -->
|
||||
<joint name="head1" perceptor="hj1" effector="he1" xaxis="0" yaxis="0" zaxis="-1" min="-120" max="120">
|
||||
<anchor index="0" part="neck" x="0" y="0" z="0.0"/>
|
||||
<anchor index="1" part="torso" x="0" y="0" z="0.09"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 1 -->
|
||||
<joint name="head2" perceptor="hj2" effector="he2" xaxis="0" yaxis="1" zaxis="0" min="-45" max="45">
|
||||
<anchor index="0" part="head" x="0" y="0" z="-0.005"/>
|
||||
<anchor index="1" part="neck" x="0" y="0" z="0.065"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 2 -->
|
||||
<joint name="lleg1" perceptor="llj1" effector="lle1" yaxis="0.7071" xaxis="0" zaxis="-0.7071" min="-90" max="1">
|
||||
<anchor index="0" part="torso" x="0.055" y="-0.01" z="-0.115"/>
|
||||
<anchor index="1" part="lhip1" x="0" y="0" z="0"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 3 -->
|
||||
<joint name="rleg1" perceptor="rlj1" effector="rle1" yaxis="0.7071" xaxis="0" zaxis="0.7071" min="-90" max="1">
|
||||
<anchor index="0" part="torso" x="-0.055" y="-0.01" z="-0.115"/>
|
||||
<anchor index="1" part="rhip1" x="0" y="0" z="0"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 4 -->
|
||||
<joint name="lleg2" perceptor="llj2" effector="lle2" yaxis="0" xaxis="1" zaxis="0" min="-25" max="45">
|
||||
<anchor index="0" part="lhip1" x="0" y="0" z="0"/>
|
||||
<anchor index="1" part="lhip2" x="0" y="0" z="0"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 5 -->
|
||||
<joint name="rleg2" perceptor="rlj2" effector="rle2" yaxis="0" xaxis="1" zaxis="0" min="-45" max="25">
|
||||
<anchor index="0" part="rhip1" x="0" y="0" z="0"/>
|
||||
<anchor index="1" part="rhip2" x="0" y="0" z="0"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 6 -->
|
||||
<joint name="lleg3" perceptor="llj3" effector="lle3" xaxis="0" yaxis="-1" zaxis="0" min="-25" max="100">
|
||||
<anchor index="0" part="lhip2" x="0" y="0" z="0"/>
|
||||
<anchor index="1" part="lthigh" x="0" y="-0.01" z="0.05832"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 7 -->
|
||||
<joint name="rleg3" perceptor="rlj3" effector="rle3" xaxis="0" yaxis="-1" zaxis="0" min="-25" max="100">
|
||||
<anchor index="0" part="rhip2" x="0" y="0" z="0"/>
|
||||
<anchor index="1" part="rthigh" x="0" y="-0.01" z="0.05832"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 8 -->
|
||||
<joint name="lleg4" perceptor="llj4" effector="lle4" xaxis="0" yaxis="-1" zaxis="0" min="-130" max="1">
|
||||
<anchor index="0" part="lthigh" x="0" y="-0.005" z="-0.08"/>
|
||||
<anchor index="1" part="lshank" x="0" y="-0.01" z="0.045"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 9 -->
|
||||
<joint name="rleg4" perceptor="rlj4" effector="rle4" xaxis="0" yaxis="-1" zaxis="0" min="-130" max="1">
|
||||
<anchor index="0" part="rthigh" x="0" y="-0.005" z="-0.08"/>
|
||||
<anchor index="1" part="rshank" x="0" y="-0.01" z="0.045"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 10 -->
|
||||
<joint name="lleg5" perceptor="llj5" effector="lle5" xaxis="0" yaxis="-1" zaxis="0" min="-45" max="75">
|
||||
<anchor index="0" part="lshank" x="0" y="-0.01" z="-0.07332"/>
|
||||
<anchor index="1" part="lankle" x="0" y="0" z="0"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 11 -->
|
||||
<joint name="rleg5" perceptor="rlj5" effector="rle5" xaxis="0" yaxis="-1" zaxis="0" min="-45" max="75">
|
||||
<anchor index="0" part="rshank" x="0" y="-0.01" z="-0.07332"/>
|
||||
<anchor index="1" part="rankle" x="0" y="0" z="0"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 12 -->
|
||||
<joint name="lleg6" perceptor="llj6" effector="lle6" yaxis="0" xaxis="1" zaxis="0" min="-45" max="25">
|
||||
<anchor index="0" part="lankle" x="0" y="0" z="0"/>
|
||||
<anchor index="1" part="lfoot" x="0" y="-0.03" z="0.04"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 13 -->
|
||||
<joint name="rleg6" perceptor="rlj6" effector="rle6" yaxis="0" xaxis="1" zaxis="0" min="-25" max="45">
|
||||
<anchor index="0" part="rankle" x="0" y="0" z="0"/>
|
||||
<anchor index="1" part="rfoot" x="0" y="-0.03" z="0.04"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 14 -->
|
||||
<joint name="larm1" perceptor="laj1" effector="lae1" yaxis="-1" xaxis="0" zaxis="0" min="-120" max="120">
|
||||
<anchor index="0" part="torso" x="0.098" y="0" z="0.075"/>
|
||||
<anchor index="1" part="lshoulder" x="0" y="0" z="0"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 15 -->
|
||||
<joint name="rarm1" perceptor="raj1" effector="rae1" yaxis="-1" xaxis="0" zaxis="0" min="-120" max="120">
|
||||
<anchor index="0" part="torso" x="-0.098" y="0" z="0.075"/>
|
||||
<anchor index="1" part="rshoulder" x="0" y="0" z="0"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 16 -->
|
||||
<joint name="larm2" perceptor="laj2" effector="lae2" xaxis="0" yaxis="0" zaxis="1" min="-1" max="95">
|
||||
<anchor index="0" part="lshoulder" x="0" y="0" z="0"/>
|
||||
<anchor index="1" part="lupperarm" x="-0.01" y="-0.02" z="0"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 17 -->
|
||||
<joint name="rarm2" perceptor="raj2" effector="rae2" xaxis="0" yaxis="0" zaxis="1" min="-95" max="1">
|
||||
<anchor index="0" part="rshoulder" x="0" y="0" z="0"/>
|
||||
<anchor index="1" part="rupperarm" x="0.01" y="-0.02" z="0"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 18 -->
|
||||
<joint name="larm3" perceptor="laj3" effector="lae3" yaxis="0" xaxis="1" zaxis="0" min="-120" max="120">
|
||||
<anchor index="0" part="lupperarm" x="-0.01" y="0.10664" z="0.009"/>
|
||||
<anchor index="1" part="lelbow" x="0" y="0" z="0"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 19 -->
|
||||
<joint name="rarm3" perceptor="raj3" effector="rae3" yaxis="0" xaxis="1" zaxis="0" min="-120" max="120">
|
||||
<anchor index="0" part="rupperarm" x="0.01" y="0.10664" z="0.009"/>
|
||||
<anchor index="1" part="relbow" x="0" y="0" z="0"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 20 -->
|
||||
<joint name="larm4" perceptor="laj4" effector="lae4" xaxis="0" yaxis="0" zaxis="1" min="-90" max="1">
|
||||
<anchor index="0" part="lelbow" x="0" y="0" z="0"/>
|
||||
<anchor index="1" part="llowerarm" x="0" y="-0.05" z="0"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 21 -->
|
||||
<joint name="rarm4" perceptor="raj4" effector="rae4" xaxis="0" yaxis="0" zaxis="1" min="-1" max="90">
|
||||
<anchor index="0" part="relbow" x="0" y="0" z="0"/>
|
||||
<anchor index="1" part="rlowerarm" x="0" y="-0.05" z="0"/>
|
||||
</joint>
|
||||
|
||||
</agentmodel>
|
||||
164
world/commons/robots/nao2.xml
Normal file
164
world/commons/robots/nao2.xml
Normal file
@@ -0,0 +1,164 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
|
||||
<agentmodel name="nao" type="humanoid" rsgfile="nao/nao_hetero.rsg 2">
|
||||
|
||||
<bodypart name="head" mass="0.35"/>
|
||||
<bodypart name="neck" mass="0.05"/>
|
||||
<bodypart name="torso" mass="1.2171"/>
|
||||
<bodypart name="lshoulder" mass="0.07"/>
|
||||
<bodypart name="rshoulder" mass="0.07"/>
|
||||
<bodypart name="lupperarm" mass="0.15"/>
|
||||
<bodypart name="rupperarm" mass="0.15"/>
|
||||
<bodypart name="lelbow" mass="0.035"/>
|
||||
<bodypart name="relbow" mass="0.035"/>
|
||||
<bodypart name="llowerarm" mass="0.2"/>
|
||||
<bodypart name="rlowerarm" mass="0.2"/>
|
||||
<bodypart name="lhip1" mass="0.09"/>
|
||||
<bodypart name="rhip1" mass="0.09"/>
|
||||
<bodypart name="lhip2" mass="0.125"/>
|
||||
<bodypart name="rhip2" mass="0.125"/>
|
||||
<bodypart name="lthigh" mass="0.275"/>
|
||||
<bodypart name="rthigh" mass="0.275"/>
|
||||
<bodypart name="lshank" mass="0.225"/>
|
||||
<bodypart name="rshank" mass="0.225"/>
|
||||
<bodypart name="lankle" mass="0.125"/>
|
||||
<bodypart name="rankle" mass="0.125"/>
|
||||
<bodypart name="lfoot" mass="0.2"/>
|
||||
<bodypart name="rfoot" mass="0.2"/>
|
||||
|
||||
<!-- joint 0 -->
|
||||
<joint name="head1" perceptor="hj1" effector="he1" xaxis="0" yaxis="0" zaxis="-1" min="-120" max="120">
|
||||
<anchor index="0" part="neck" x="0" y="0" z="0.0"/>
|
||||
<anchor index="1" part="torso" x="0" y="0" z="0.09"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 1 -->
|
||||
<joint name="head2" perceptor="hj2" effector="he2" xaxis="0" yaxis="1" zaxis="0" min="-45" max="45">
|
||||
<anchor index="0" part="head" x="0" y="0" z="-0.005"/>
|
||||
<anchor index="1" part="neck" x="0" y="0" z="0.065"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 2 -->
|
||||
<joint name="lleg1" perceptor="llj1" effector="lle1" yaxis="0.7071" xaxis="0" zaxis="-0.7071" min="-90" max="1">
|
||||
<anchor index="0" part="torso" x="0.055" y="-0.01" z="-0.115"/>
|
||||
<anchor index="1" part="lhip1" x="0" y="0" z="0"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 3 -->
|
||||
<joint name="rleg1" perceptor="rlj1" effector="rle1" yaxis="0.7071" xaxis="0" zaxis="0.7071" min="-90" max="1">
|
||||
<anchor index="0" part="torso" x="-0.055" y="-0.01" z="-0.115"/>
|
||||
<anchor index="1" part="rhip1" x="0" y="0" z="0"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 4 -->
|
||||
<joint name="lleg2" perceptor="llj2" effector="lle2" yaxis="0" xaxis="1" zaxis="0" min="-25" max="45">
|
||||
<anchor index="0" part="lhip1" x="0" y="0" z="0"/>
|
||||
<anchor index="1" part="lhip2" x="0" y="0" z="0"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 5 -->
|
||||
<joint name="rleg2" perceptor="rlj2" effector="rle2" yaxis="0" xaxis="1" zaxis="0" min="-45" max="25">
|
||||
<anchor index="0" part="rhip1" x="0" y="0" z="0"/>
|
||||
<anchor index="1" part="rhip2" x="0" y="0" z="0"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 6 -->
|
||||
<joint name="lleg3" perceptor="llj3" effector="lle3" xaxis="0" yaxis="-1" zaxis="0" min="-25" max="100">
|
||||
<anchor index="0" part="lhip2" x="0" y="0" z="0"/>
|
||||
<anchor index="1" part="lthigh" x="0" y="-0.01" z="0.04"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 7 -->
|
||||
<joint name="rleg3" perceptor="rlj3" effector="rle3" xaxis="0" yaxis="-1" zaxis="0" min="-25" max="100">
|
||||
<anchor index="0" part="rhip2" x="0" y="0" z="0"/>
|
||||
<anchor index="1" part="rthigh" x="0" y="-0.01" z="0.04"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 8 -->
|
||||
<joint name="lleg4" perceptor="llj4" effector="lle4" xaxis="0" yaxis="-1" zaxis="0" min="-130" max="1">
|
||||
<anchor index="0" part="lthigh" x="0" y="-0.005" z="-0.08"/>
|
||||
<anchor index="1" part="lshank" x="0" y="-0.01" z="0.045"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 9 -->
|
||||
<joint name="rleg4" perceptor="rlj4" effector="rle4" xaxis="0" yaxis="-1" zaxis="0" min="-130" max="1">
|
||||
<anchor index="0" part="rthigh" x="0" y="-0.005" z="-0.08"/>
|
||||
<anchor index="1" part="rshank" x="0" y="-0.01" z="0.045"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 10 -->
|
||||
<joint name="lleg5" perceptor="llj5" effector="lle5" xaxis="0" yaxis="-1" zaxis="0" min="-45" max="75">
|
||||
<anchor index="0" part="lshank" x="0" y="-0.01" z="-0.055"/>
|
||||
<anchor index="1" part="lankle" x="0" y="0" z="0"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 11 -->
|
||||
<joint name="rleg5" perceptor="rlj5" effector="rle5" xaxis="0" yaxis="-1" zaxis="0" min="-45" max="75">
|
||||
<anchor index="0" part="rshank" x="0" y="-0.01" z="-0.055"/>
|
||||
<anchor index="1" part="rankle" x="0" y="0" z="0"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 12 -->
|
||||
<joint name="lleg6" perceptor="llj6" effector="lle6" yaxis="0" xaxis="1" zaxis="0" min="-45" max="25">
|
||||
<anchor index="0" part="lankle" x="0" y="0" z="0"/>
|
||||
<anchor index="1" part="lfoot" x="0" y="-0.03" z="0.04"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 13 -->
|
||||
<joint name="rleg6" perceptor="rlj6" effector="rle6" yaxis="0" xaxis="1" zaxis="0" min="-25" max="45">
|
||||
<anchor index="0" part="rankle" x="0" y="0" z="0"/>
|
||||
<anchor index="1" part="rfoot" x="0" y="-0.03" z="0.04"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 14 -->
|
||||
<joint name="larm1" perceptor="laj1" effector="lae1" yaxis="-1" xaxis="0" zaxis="0" min="-120" max="120">
|
||||
<anchor index="0" part="torso" x="0.098" y="0" z="0.075"/>
|
||||
<anchor index="1" part="lshoulder" x="0" y="0" z="0"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 15 -->
|
||||
<joint name="rarm1" perceptor="raj1" effector="rae1" yaxis="-1" xaxis="0" zaxis="0" min="-120" max="120">
|
||||
<anchor index="0" part="torso" x="-0.098" y="0" z="0.075"/>
|
||||
<anchor index="1" part="rshoulder" x="0" y="0" z="0"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 16 -->
|
||||
<joint name="larm2" perceptor="laj2" effector="lae2" xaxis="0" yaxis="0" zaxis="1" min="-1" max="95">
|
||||
<anchor index="0" part="lshoulder" x="0" y="0" z="0"/>
|
||||
<anchor index="1" part="lupperarm" x="-0.01" y="-0.02" z="0"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 17 -->
|
||||
<joint name="rarm2" perceptor="raj2" effector="rae2" xaxis="0" yaxis="0" zaxis="1" min="-95" max="1">
|
||||
<anchor index="0" part="rshoulder" x="0" y="0" z="0"/>
|
||||
<anchor index="1" part="rupperarm" x="0.01" y="-0.02" z="0"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 18 -->
|
||||
<joint name="larm3" perceptor="laj3" effector="lae3" yaxis="0" xaxis="1" zaxis="0" min="-120" max="120">
|
||||
<anchor index="0" part="lupperarm" x="-0.01" y="0.07" z="0.009"/>
|
||||
<anchor index="1" part="lelbow" x="0" y="0" z="0"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 19 -->
|
||||
<joint name="rarm3" perceptor="raj3" effector="rae3" yaxis="0" xaxis="1" zaxis="0" min="-120" max="120">
|
||||
<anchor index="0" part="rupperarm" x="0.01" y="0.07" z="0.009"/>
|
||||
<anchor index="1" part="relbow" x="0" y="0" z="0"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 20 -->
|
||||
<joint name="larm4" perceptor="laj4" effector="lae4" xaxis="0" yaxis="0" zaxis="1" min="-90" max="1">
|
||||
<anchor index="0" part="lelbow" x="0" y="0" z="0"/>
|
||||
<anchor index="1" part="llowerarm" x="0" y="-0.05" z="0"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 21 -->
|
||||
<joint name="rarm4" perceptor="raj4" effector="rae4" xaxis="0" yaxis="0" zaxis="1" min="-1" max="90">
|
||||
<anchor index="0" part="relbow" x="0" y="0" z="0"/>
|
||||
<anchor index="1" part="rlowerarm" x="0" y="-0.05" z="0"/>
|
||||
</joint>
|
||||
|
||||
<joint name="lleg7" perceptor="llj7" effector="lle7" xaxis="0" yaxis="0" zaxis="0" min="0" max="0"/>
|
||||
<joint name="rleg7" perceptor="rlj7" effector="rle7" xaxis="0" yaxis="0" zaxis="0" min="0" max="0"/>
|
||||
|
||||
</agentmodel>
|
||||
161
world/commons/robots/nao3.xml
Normal file
161
world/commons/robots/nao3.xml
Normal file
@@ -0,0 +1,161 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
|
||||
<agentmodel name="nao" type="humanoid" rsgfile="nao/nao_hetero.rsg 3">
|
||||
|
||||
<bodypart name="head" mass="0.35"/>
|
||||
<bodypart name="neck" mass="0.05"/>
|
||||
<bodypart name="torso" mass="1.2171"/>
|
||||
<bodypart name="lshoulder" mass="0.07"/>
|
||||
<bodypart name="rshoulder" mass="0.07"/>
|
||||
<bodypart name="lupperarm" mass="0.15"/>
|
||||
<bodypart name="rupperarm" mass="0.15"/>
|
||||
<bodypart name="lelbow" mass="0.035"/>
|
||||
<bodypart name="relbow" mass="0.035"/>
|
||||
<bodypart name="llowerarm" mass="0.2"/>
|
||||
<bodypart name="rlowerarm" mass="0.2"/>
|
||||
<bodypart name="lhip1" mass="0.09"/>
|
||||
<bodypart name="rhip1" mass="0.09"/>
|
||||
<bodypart name="lhip2" mass="0.125"/>
|
||||
<bodypart name="rhip2" mass="0.125"/>
|
||||
<bodypart name="lthigh" mass="0.275"/>
|
||||
<bodypart name="rthigh" mass="0.275"/>
|
||||
<bodypart name="lshank" mass="0.225"/>
|
||||
<bodypart name="rshank" mass="0.225"/>
|
||||
<bodypart name="lankle" mass="0.125"/>
|
||||
<bodypart name="rankle" mass="0.125"/>
|
||||
<bodypart name="lfoot" mass="0.2"/>
|
||||
<bodypart name="rfoot" mass="0.2"/>
|
||||
|
||||
<!-- joint 0 -->
|
||||
<joint name="head1" perceptor="hj1" effector="he1" xaxis="0" yaxis="0" zaxis="-1" min="-120" max="120">
|
||||
<anchor index="0" part="neck" x="0" y="0" z="0.0"/>
|
||||
<anchor index="1" part="torso" x="0" y="0" z="0.09"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 1 -->
|
||||
<joint name="head2" perceptor="hj2" effector="he2" xaxis="0" yaxis="1" zaxis="0" min="-45" max="45">
|
||||
<anchor index="0" part="head" x="0" y="0" z="-0.005"/>
|
||||
<anchor index="1" part="neck" x="0" y="0" z="0.065"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 2 -->
|
||||
<joint name="lleg1" perceptor="llj1" effector="lle1" yaxis="0.7071" xaxis="0" zaxis="-0.7071" min="-90" max="1">
|
||||
<anchor index="0" part="torso" x="0.072954143" y="-0.01" z="-0.115"/>
|
||||
<anchor index="1" part="lhip1" x="0" y="0" z="0"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 3 -->
|
||||
<joint name="rleg1" perceptor="rlj1" effector="rle1" yaxis="0.7071" xaxis="0" zaxis="0.7071" min="-90" max="1">
|
||||
<anchor index="0" part="torso" x="-0.072954143" y="-0.01" z="-0.115"/>
|
||||
<anchor index="1" part="rhip1" x="0" y="0" z="0"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 4 -->
|
||||
<joint name="lleg2" perceptor="llj2" effector="lle2" yaxis="0" xaxis="1" zaxis="0" min="-25" max="45">
|
||||
<anchor index="0" part="lhip1" x="0" y="0" z="0"/>
|
||||
<anchor index="1" part="lhip2" x="0" y="0" z="0"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 5 -->
|
||||
<joint name="rleg2" perceptor="rlj2" effector="rle2" yaxis="0" xaxis="1" zaxis="0" min="-45" max="25">
|
||||
<anchor index="0" part="rhip1" x="0" y="0" z="0"/>
|
||||
<anchor index="1" part="rhip2" x="0" y="0" z="0"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 6 -->
|
||||
<joint name="lleg3" perceptor="llj3" effector="lle3" xaxis="0" yaxis="-1" zaxis="0" min="-25" max="100">
|
||||
<anchor index="0" part="lhip2" x="0" y="0" z="0"/>
|
||||
<anchor index="1" part="lthigh" x="0" y="-0.01" z="0.067868424"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 7 -->
|
||||
<joint name="rleg3" perceptor="rlj3" effector="rle3" xaxis="0" yaxis="-1" zaxis="0" min="-25" max="100">
|
||||
<anchor index="0" part="rhip2" x="0" y="0" z="0"/>
|
||||
<anchor index="1" part="rthigh" x="0" y="-0.01" z="0.067868424"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 8 -->
|
||||
<joint name="lleg4" perceptor="llj4" effector="lle4" xaxis="0" yaxis="-1" zaxis="0" min="-130" max="1">
|
||||
<anchor index="0" part="lthigh" x="0" y="-0.005" z="-0.08"/>
|
||||
<anchor index="1" part="lshank" x="0" y="-0.01" z="0.045"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 9 -->
|
||||
<joint name="rleg4" perceptor="rlj4" effector="rle4" xaxis="0" yaxis="-1" zaxis="0" min="-130" max="1">
|
||||
<anchor index="0" part="rthigh" x="0" y="-0.005" z="-0.08"/>
|
||||
<anchor index="1" part="rshank" x="0" y="-0.01" z="0.045"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 10 -->
|
||||
<joint name="lleg5" perceptor="llj5" effector="lle5" xaxis="0" yaxis="-1" zaxis="0" min="-45" max="75">
|
||||
<anchor index="0" part="lshank" x="0" y="-0.01" z="-0.082868424"/>
|
||||
<anchor index="1" part="lankle" x="0" y="0" z="0"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 11 -->
|
||||
<joint name="rleg5" perceptor="rlj5" effector="rle5" xaxis="0" yaxis="-1" zaxis="0" min="-45" max="75">
|
||||
<anchor index="0" part="rshank" x="0" y="-0.01" z="-0.082868424"/>
|
||||
<anchor index="1" part="rankle" x="0" y="0" z="0"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 12 -->
|
||||
<joint name="lleg6" perceptor="llj6" effector="lle6" yaxis="0" xaxis="1" zaxis="0" min="-45" max="25">
|
||||
<anchor index="0" part="lankle" x="0" y="0" z="0"/>
|
||||
<anchor index="1" part="lfoot" x="0" y="-0.03" z="0.04"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 13 -->
|
||||
<joint name="rleg6" perceptor="rlj6" effector="rle6" yaxis="0" xaxis="1" zaxis="0" min="-25" max="45">
|
||||
<anchor index="0" part="rankle" x="0" y="0" z="0"/>
|
||||
<anchor index="1" part="rfoot" x="0" y="-0.03" z="0.04"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 14 -->
|
||||
<joint name="larm1" perceptor="laj1" effector="lae1" yaxis="-1" xaxis="0" zaxis="0" min="-120" max="120">
|
||||
<anchor index="0" part="torso" x="0.098" y="0" z="0.075"/>
|
||||
<anchor index="1" part="lshoulder" x="0" y="0" z="0"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 15 -->
|
||||
<joint name="rarm1" perceptor="raj1" effector="rae1" yaxis="-1" xaxis="0" zaxis="0" min="-120" max="120">
|
||||
<anchor index="0" part="torso" x="-0.098" y="0" z="0.075"/>
|
||||
<anchor index="1" part="rshoulder" x="0" y="0" z="0"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 16 -->
|
||||
<joint name="larm2" perceptor="laj2" effector="lae2" xaxis="0" yaxis="0" zaxis="1" min="-1" max="95">
|
||||
<anchor index="0" part="lshoulder" x="0" y="0" z="0"/>
|
||||
<anchor index="1" part="lupperarm" x="-0.01" y="-0.02" z="0"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 17 -->
|
||||
<joint name="rarm2" perceptor="raj2" effector="rae2" xaxis="0" yaxis="0" zaxis="1" min="-95" max="1">
|
||||
<anchor index="0" part="rshoulder" x="0" y="0" z="0"/>
|
||||
<anchor index="1" part="rupperarm" x="0.01" y="-0.02" z="0"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 18 -->
|
||||
<joint name="larm3" perceptor="laj3" effector="lae3" yaxis="0" xaxis="1" zaxis="0" min="-120" max="120">
|
||||
<anchor index="0" part="lupperarm" x="-0.01" y="0.125736848" z="0.009"/>
|
||||
<anchor index="1" part="lelbow" x="0" y="0" z="0"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 19 -->
|
||||
<joint name="rarm3" perceptor="raj3" effector="rae3" yaxis="0" xaxis="1" zaxis="0" min="-120" max="120">
|
||||
<anchor index="0" part="rupperarm" x="0.01" y="0.125736848" z="0.009"/>
|
||||
<anchor index="1" part="relbow" x="0" y="0" z="0"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 20 -->
|
||||
<joint name="larm4" perceptor="laj4" effector="lae4" xaxis="0" yaxis="0" zaxis="1" min="-90" max="1">
|
||||
<anchor index="0" part="lelbow" x="0" y="0" z="0"/>
|
||||
<anchor index="1" part="llowerarm" x="0" y="-0.05" z="0"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 21 -->
|
||||
<joint name="rarm4" perceptor="raj4" effector="rae4" xaxis="0" yaxis="0" zaxis="1" min="-1" max="90">
|
||||
<anchor index="0" part="relbow" x="0" y="0" z="0"/>
|
||||
<anchor index="1" part="rlowerarm" x="0" y="-0.05" z="0"/>
|
||||
</joint>
|
||||
|
||||
</agentmodel>
|
||||
175
world/commons/robots/nao4.xml
Normal file
175
world/commons/robots/nao4.xml
Normal file
@@ -0,0 +1,175 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
|
||||
<agentmodel name="nao" type="humanoid" rsgfile="nao/nao_hetero.rsg 4">
|
||||
|
||||
<bodypart name="head" mass="0.35"/>
|
||||
<bodypart name="neck" mass="0.05"/>
|
||||
<bodypart name="torso" mass="1.2171"/>
|
||||
<bodypart name="lshoulder" mass="0.07"/>
|
||||
<bodypart name="rshoulder" mass="0.07"/>
|
||||
<bodypart name="lupperarm" mass="0.15"/>
|
||||
<bodypart name="rupperarm" mass="0.15"/>
|
||||
<bodypart name="lelbow" mass="0.035"/>
|
||||
<bodypart name="relbow" mass="0.035"/>
|
||||
<bodypart name="llowerarm" mass="0.2"/>
|
||||
<bodypart name="rlowerarm" mass="0.2"/>
|
||||
<bodypart name="lhip1" mass="0.09"/>
|
||||
<bodypart name="rhip1" mass="0.09"/>
|
||||
<bodypart name="lhip2" mass="0.125"/>
|
||||
<bodypart name="rhip2" mass="0.125"/>
|
||||
<bodypart name="lthigh" mass="0.275"/>
|
||||
<bodypart name="rthigh" mass="0.275"/>
|
||||
<bodypart name="lshank" mass="0.225"/>
|
||||
<bodypart name="rshank" mass="0.225"/>
|
||||
<bodypart name="lankle" mass="0.125"/>
|
||||
<bodypart name="rankle" mass="0.125"/>
|
||||
<bodypart name="lfoot" mass="0.2"/>
|
||||
<bodypart name="rfoot" mass="0.2"/>
|
||||
<bodypart name="ltoe" mass="0.022"/>
|
||||
<bodypart name="rtoe" mass="0.022"/>
|
||||
|
||||
<!-- joint 0 -->
|
||||
<joint name="head1" perceptor="hj1" effector="he1" xaxis="0" yaxis="0" zaxis="-1" min="-120" max="120">
|
||||
<anchor index="0" part="neck" x="0" y="0" z="0.0"/>
|
||||
<anchor index="1" part="torso" x="0" y="0" z="0.09"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 1 -->
|
||||
<joint name="head2" perceptor="hj2" effector="he2" xaxis="0" yaxis="1" zaxis="0" min="-45" max="45">
|
||||
<anchor index="0" part="head" x="0" y="0" z="-0.005"/>
|
||||
<anchor index="1" part="neck" x="0" y="0" z="0.065"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 2 -->
|
||||
<joint name="lleg1" perceptor="llj1" effector="lle1" yaxis="0.7071" xaxis="0" zaxis="-0.7071" min="-90" max="1">
|
||||
<anchor index="0" part="torso" x="0.055" y="-0.01" z="-0.115"/>
|
||||
<anchor index="1" part="lhip1" x="0" y="0" z="0"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 3 -->
|
||||
<joint name="rleg1" perceptor="rlj1" effector="rle1" yaxis="0.7071" xaxis="0" zaxis="0.7071" min="-90" max="1">
|
||||
<anchor index="0" part="torso" x="-0.055" y="-0.01" z="-0.115"/>
|
||||
<anchor index="1" part="rhip1" x="0" y="0" z="0"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 4 -->
|
||||
<joint name="lleg2" perceptor="llj2" effector="lle2" yaxis="0" xaxis="1" zaxis="0" min="-25" max="45">
|
||||
<anchor index="0" part="lhip1" x="0" y="0" z="0"/>
|
||||
<anchor index="1" part="lhip2" x="0" y="0" z="0"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 5 -->
|
||||
<joint name="rleg2" perceptor="rlj2" effector="rle2" yaxis="0" xaxis="1" zaxis="0" min="-45" max="25">
|
||||
<anchor index="0" part="rhip1" x="0" y="0" z="0"/>
|
||||
<anchor index="1" part="rhip2" x="0" y="0" z="0"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 6 -->
|
||||
<joint name="lleg3" perceptor="llj3" effector="lle3" xaxis="0" yaxis="-1" zaxis="0" min="-25" max="100">
|
||||
<anchor index="0" part="lhip2" x="0" y="0" z="0"/>
|
||||
<anchor index="1" part="lthigh" x="0" y="-0.01" z="0.04"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 7 -->
|
||||
<joint name="rleg3" perceptor="rlj3" effector="rle3" xaxis="0" yaxis="-1" zaxis="0" min="-25" max="100">
|
||||
<anchor index="0" part="rhip2" x="0" y="0" z="0"/>
|
||||
<anchor index="1" part="rthigh" x="0" y="-0.01" z="0.04"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 8 -->
|
||||
<joint name="lleg4" perceptor="llj4" effector="lle4" xaxis="0" yaxis="-1" zaxis="0" min="-130" max="1">
|
||||
<anchor index="0" part="lthigh" x="0" y="-0.005" z="-0.08"/>
|
||||
<anchor index="1" part="lshank" x="0" y="-0.01" z="0.045"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 9 -->
|
||||
<joint name="rleg4" perceptor="rlj4" effector="rle4" xaxis="0" yaxis="-1" zaxis="0" min="-130" max="1">
|
||||
<anchor index="0" part="rthigh" x="0" y="-0.005" z="-0.08"/>
|
||||
<anchor index="1" part="rshank" x="0" y="-0.01" z="0.045"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 10 -->
|
||||
<joint name="lleg5" perceptor="llj5" effector="lle5" xaxis="0" yaxis="-1" zaxis="0" min="-45" max="75">
|
||||
<anchor index="0" part="lshank" x="0" y="-0.01" z="-0.055"/>
|
||||
<anchor index="1" part="lankle" x="0" y="0" z="0"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 11 -->
|
||||
<joint name="rleg5" perceptor="rlj5" effector="rle5" xaxis="0" yaxis="-1" zaxis="0" min="-45" max="75">
|
||||
<anchor index="0" part="rshank" x="0" y="-0.01" z="-0.055"/>
|
||||
<anchor index="1" part="rankle" x="0" y="0" z="0"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 12 -->
|
||||
<joint name="lleg6" perceptor="llj6" effector="lle6" yaxis="0" xaxis="1" zaxis="0" min="-45" max="25">
|
||||
<anchor index="0" part="lankle" x="0" y="0" z="0"/>
|
||||
<anchor index="1" part="lfoot" x="0" y="-0.012241172" z="0.04"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 13 -->
|
||||
<joint name="rleg6" perceptor="rlj6" effector="rle6" yaxis="0" xaxis="1" zaxis="0" min="-25" max="45">
|
||||
<anchor index="0" part="rankle" x="0" y="0" z="0"/>
|
||||
<anchor index="1" part="rfoot" x="0" y="-0.012241172" z="0.04"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 14 -->
|
||||
<joint name="larm1" perceptor="laj1" effector="lae1" yaxis="-1" xaxis="0" zaxis="0" min="-120" max="120">
|
||||
<anchor index="0" part="torso" x="0.098" y="0" z="0.075"/>
|
||||
<anchor index="1" part="lshoulder" x="0" y="0" z="0"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 15 -->
|
||||
<joint name="rarm1" perceptor="raj1" effector="rae1" yaxis="-1" xaxis="0" zaxis="0" min="-120" max="120">
|
||||
<anchor index="0" part="torso" x="-0.098" y="0" z="0.075"/>
|
||||
<anchor index="1" part="rshoulder" x="0" y="0" z="0"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 16 -->
|
||||
<joint name="larm2" perceptor="laj2" effector="lae2" xaxis="0" yaxis="0" zaxis="1" min="-1" max="95">
|
||||
<anchor index="0" part="lshoulder" x="0" y="0" z="0"/>
|
||||
<anchor index="1" part="lupperarm" x="-0.01" y="-0.02" z="0"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 17 -->
|
||||
<joint name="rarm2" perceptor="raj2" effector="rae2" xaxis="0" yaxis="0" zaxis="1" min="-95" max="1">
|
||||
<anchor index="0" part="rshoulder" x="0" y="0" z="0"/>
|
||||
<anchor index="1" part="rupperarm" x="0.01" y="-0.02" z="0"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 18 -->
|
||||
<joint name="larm3" perceptor="laj3" effector="lae3" yaxis="0" xaxis="1" zaxis="0" min="-120" max="120">
|
||||
<anchor index="0" part="lupperarm" x="-0.01" y="0.07" z="0.009"/>
|
||||
<anchor index="1" part="lelbow" x="0" y="0" z="0"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 19 -->
|
||||
<joint name="rarm3" perceptor="raj3" effector="rae3" yaxis="0" xaxis="1" zaxis="0" min="-120" max="120">
|
||||
<anchor index="0" part="rupperarm" x="0.01" y="0.07" z="0.009"/>
|
||||
<anchor index="1" part="relbow" x="0" y="0" z="0"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 20 -->
|
||||
<joint name="larm4" perceptor="laj4" effector="lae4" xaxis="0" yaxis="0" zaxis="1" min="-90" max="1">
|
||||
<anchor index="0" part="lelbow" x="0" y="0" z="0"/>
|
||||
<anchor index="1" part="llowerarm" x="0" y="-0.05" z="0"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 21 -->
|
||||
<joint name="rarm4" perceptor="raj4" effector="rae4" xaxis="0" yaxis="0" zaxis="1" min="-1" max="90">
|
||||
<anchor index="0" part="relbow" x="0" y="0" z="0"/>
|
||||
<anchor index="1" part="rlowerarm" x="0" y="-0.05" z="0"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 22 -->
|
||||
<joint name="lleg7" perceptor="llj7" effector="lle7" xaxis="0" yaxis="-1" zaxis="0" min="-1" max="70">
|
||||
<anchor index="0" part="lfoot" x="0" y="0.062241172" z="-0.01"/>
|
||||
<anchor index="1" part="ltoe" x="0" y="-0.017758828" z="-0.005"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 23 -->
|
||||
<joint name="rleg7" perceptor="rlj7" effector="rle7" xaxis="0" yaxis="-1" zaxis="0" min="-1" max="70">
|
||||
<anchor index="0" part="rfoot" x="0" y="0.062241172" z="-0.01"/>
|
||||
<anchor index="1" part="rtoe" x="0" y="-0.017758828" z="-0.005"/>
|
||||
</joint>
|
||||
|
||||
</agentmodel>
|
||||
Reference in New Issue
Block a user