ChenXi/FCP_gym
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Init gym

Browse Source
This commit is contained in:
ChenXi
2025-11-19 08:08:22 -05:00
commit eaaa5519bd
256 changed files with 46657 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
world/commons/Body_Part.py Normal file
View 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
View 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
View 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
View 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
View 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
View File

Binary file not shown.

BIN
world/commons/__pycache__/Draw.cpython-313.pyc Normal file
View File

Binary file not shown.

BIN
world/commons/__pycache__/Joint_Info.cpython-313.pyc Normal file
View File

Binary file not shown.

BIN
world/commons/__pycache__/Other_Robot.cpython-313.pyc Normal file
View File

Binary file not shown.

BIN
world/commons/__pycache__/Path_Manager.cpython-313.pyc Normal file
View File

Binary file not shown.

165
world/commons/robots/nao0.xml Normal file
View 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
View 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
View 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
View 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
View 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>