-
Notifications
You must be signed in to change notification settings - Fork 38
Config File Parameters
We use a configuration file to make changing between robots and location specific parameters easy. The configuration file loaded by the code is whatever file is named Player/Config/Config.lua. Normally we use a soft link to set the configuration file to one of the configurations we have.
For example, to set the config file to the Nao Webots one:
ln -s Config_Webots.lua Config.lua
The parameters are just table values contained in the Lua module.
As the project has grown in size so has the number of configuration parameters. Keeping all the parameters in one file can become unwieldy. We provide a method for splitting up the complete configuration between multiple files.
The loadconfig function will load the parameters from another config file into the current one:
function loadconfig(configName)
local localConfig=require(configName);
for k,v in pairs(localConfig) do
Config[k]=localConfig[k];
end
end
loadconfig('Config_Nao_World')
This is also beneficial for sharing sets of parameters between two different configurations without having to keep two copies. (e.g. the Config_Nao_World.lua configuration is used by both the Webots Nao code and the actual Nao robot code)
NOTE: any parameter types that have a superscript (e.g. arrayspecial ) are tables or arrays that have a special format. You can find a description of these formats in the next section.
Table | Table Overview | ||
---|---|---|---|
Parameter Key | Type | Description | |
platform | Name of the current running robot platform. | ||
name | string | Name of the robot platform. (e.g. 'nao', 'op', 'webots') | |
dev | Names of the platform dependent device interfaces. | ||
body | string | Name of the platform dependent Body module. | |
camera | string | Name of the platform dependent Camera module. | |
kinematics | string | Name of the platform dependent Kinematics module. | |
comm | string | Name of the platform dependent Comm module. | |
monitor_comm | string | Name of the platform dependent MonitorComm module. | |
game_control | string | Name of the platform dependent GameControl module. | |
walk | string | Name of the platform dependent walk module. | |
kick | string | Name of the platform dependent kick module. | |
game | Parameters relevant to the game including player id and team number. | ||
teamNumber | int | Robot's team number. | |
playerID | int | Robot's player ID. Used by team coordination. | |
robotID | int | Robot's robot ID. | |
teamColor | int | Default team color (1/0). | |
nPlayers | int | Number of players on each team. | |
fsm | Names of the finite state machines to use. | ||
game | string | Name of the GameFSM to run. | |
body | array | Array of possible BodyFSM names to use. | |
head | array | Array of possible HeadFSM names to use. | |
camera | Physical camera parameters including focal length, image size and camera settings. | ||
ncamera | int | Number of cameras on the robot. | |
switchFreq | int | Frequency to switch between the cameras in auto switch mode. Will switch every switchFreq images. | |
width | int | Width of the raw YUYV image in pixels. | |
height | int | Height of the raw YUYV image in pixels. | |
x_center | int | Horizontal center of the raw YUYV image in pixels. | |
y_center | int | Vertical center of the raw YUYV image in pixels. | |
focal_length | int | Focal length of the camera in pixels. | |
focal_base | int | Width, in pixels, of the image size used in the focal length calculation.. | |
auto_param | tablecam_param | Array of camera parameter tables. These should be the parameters for any auto settings for the camera (auto exposure, auto gain, etc.). These parameters are set before those in the param array. | |
param | tablecam_param | Array of camera parameter tables. The array should contain all other available parameters not specified in the auto_param array.These parameters are set after those in the auto_param array. | |
lut_file | string | Name of the look up table to use for color classification. | |
vision | Parameters relevant to the Vision system. | ||
maxFPS | int | Maximum frame rate to allow the Vision algorithms to run at. Allows the user to throttle the vision process if it is taking up too much CPU. | |
scaleB | int | Size, in pixels, to scale the labeled image in the bit-or process. | |
ball_diameter | float | Diameter of the ball in meters. | |
ball_height_max | float | Maximum height we expect to detect the ball, in the robots Head frame. | |
check_for_ground | int | Flag to indicate that the ball detector should use the ground check. | |
yellow_goal_count_thres | int | Yellow pixel threshold to run the yellow goal detection. If there are less than this number of yellow pixels in the image the yellow goal detection is skipped. | |
enable_line_detection | int | Flag to indicate if the line detection should be run. | |
enable_spot_detection | int | Flag to indicate if the spot detection should be run. | |
enable_midfield_landmark_detection | int | Flag to indicate if the midfield landmark detection should be run. (used in the NSL) | |
copy_image_to_shm | int | Flag to enable copying the images to shared memory for debuggging. | |
store_all_images | int | Flag to indicate that all images should be copied to shared memory. | |
store_goal_detections | int | Flag to indicate that images where a goal was detected should be copied to shared memory. | |
store_ball_detections | int | Flag to indicate that images where a ball was detected should be copied to shared memory. | |
ballColor | int | The label color of the ball. | |
goal1Color | int | The label color of the first goal. (Should be consistent with the goal order in the world configuration) | |
goal2Color | int | The label color of the second goal. (Should be consistent with the goal order in the _world_ configuration) | |
world | Localization parameters and landmark positions. | ||
n | int | Number of particles to use in the particle filter. | |
xLineBoundary | float | Distance to the x-axis boundary from the center. | |
yLineBoundary | float | Distance to the y-axis boundary from the center. | |
xMax | float | Maximum valid distance the robot can be along the x-axis, from the center. | |
yMax | float | Maximum valid distance the robot can be along the y-axis, from the center. | |
goalWidth | float | Width of the goal (post center to center). | |
postYellow | array | Position of each yellow post in the world | |
postCyan | array | Position of each cyan post in the world | |
spot | array | Position of each spot in the world | |
landmarkCyan | array | Position of the cyan landmark in the world. | |
landmarkYellow | array | Position of the yellow landmark in the world. | |
cResample | int | Interval to resample the particle filter. | |
odomScale | array | Scaling factor of the odometry estimates. `{x, y, a}` | |
rGoalFilter | float | Radius weight to use for goal detections in the particle filter. | |
aGoalFilter | float | Angle weight to use for goal detections in the particle filter. | |
rPostFilter | float | Radius weight to use for post detections in the particle filter. | |
aPostFilter | float | Angle weight to use for post detections in the particle filter. | |
team | Team coordination parameters. | ||
msgTimeout | float | Timeout to use for determining if we have lost communication with a robot. | |
nonAttackerPenalty | float | Penalty, in seconds, to give to robots that are not the current attacker in determing roles. | |
nonDefenderPenalty | float | Penalty, in meters, to give to robots that are not the current defender in determing roles. | |
head | Head parameters including camera positions and neck limits. | ||
camOffsetZ | float | TODO | |
pitchMin | float | Min pitch of the neck, in radians | |
pitchMax | float | Max pitch of the neck, in radians | |
yawMin | float | Min yaw of the neck, in radians | |
yawMax | float | Max yaw of the neck, in radians | |
cameraPos | arraycam_pos | TODO | |
cameraAngle | arraycam_angle | TODO | |
neckX | float | Distance from the robot center of mass to the neck joint along the robot x-axis. | |
neckZ | float | Distance from the robot center of mass to the neck joint along the robot z-axis. | |
bodyTilt | float | Normal tilt of the robot while standing, in radians. | |
km | Names of keyframe files. | ||
kick_right | string | Filename of the right kick keyframe. | |
kick_left | string | Filename of the left kick keyframe. | |
standup_front | string | Filename of the standup from front keyframe. | |
standup_back | string | Filename of the standup from back keyframe. | |
sit | Parameters relevant to the sit module. | ||
bodyHeight | float | Target body height for the robot to sit to, in meters. | |
supportX | float | The distance the ankles rest behind the torso, in meters. | |
dpLimit | array | The max speed at which the torso can move in meters/second. | |
stance | Parameters relevant to the stance module. | ||
dpLimit | array | The max speed at which the torso can move in meters/second. | |
delay | float | Amount of time, in milliseconds, to stand still after standing to regain balance. | |
kick | Parameters relevant to the IK kick engine module. | ||
tSensorDelay | float | The delay between sending an actuator command and sensing the movement. | |
torsoSensorParamX | array | The gain for join encoder feedback in the x direction. | |
torsoSensorParamY | array | The gain for join encoder feedback in the y direction. | |
gyroFactor | float | Ratio to normalize gyro gains into units of degree/second | |
ankleImuParamX | array | This is used to apply torque in the X-direction from the ankles. | |
kneeImuParamX | array | This is used to apply torque in the X-direction from the knees. | |
ankleImuParamY | array | This is used to apply torque in the y-direction on the ankles. | |
hipImuParamY | array | This is used to apply torque in the y-direction on the hip. | |
armImuParamX | array | Gain in x direction for using arms to stabilize. | |
armImuParamY | array | Gain in y direction for using arms to stabilize. | |
qLArm | array | Kick arm pose for left arm | |
qRArm | array | Kick arm pose for right arm | |
armGain | float | Controls amount of arm swinging during kick. Smaller value = larger swing. | |
supportCompL | array | Change in (x,y,z) coordinates of left foot from calculated position for better stabilization during kick. | |
supportCompR | array | Change in (x,y,z) coordinates of right foot from calculated position for better stabilization during kick. | |
kickLeft | array | Kick states for left-forward kick. | |
kickRight | array | Kick states for right-forward kick. | |
kickSideLeft | array | Kick states for left-side kick. | |
kickSideRight | array | Kick states for right-side kick. | |
walk | Parameters relevant to the ZMP walk engine module. | ||
tStep | float | The tStep defines how long it will take for a robot to take its next step, in seconds. | |
bodyHeight | float | The body height of the robot, in meters; changing this will alter how high the robot's natural stance is. | |
stepHeight | float | The height to which the robot raises its foot at each step. This parameter is very sensitive in terms of balance. | |
footY | float | The width of the robot's rest stance in meters. | |
supportX | float | The distance the ankles rest behind the torso, in meters. | |
supportY | float | How far from the center of the foot the center of mass is placed during each step. | |
phSingle | array | The ratio of double support time: single support time (standing on two feet vs. balancing on one foot). | |
maxX | array | {min, max} walk velocity (m/s) in the x-axis of the robot, in the robot frame. | |
maxY | array | {min, max} walk velocity (m/s) in the y-axis of the robot, in the robot frame. | |
maxZ | array | {min, max} walk velocity (m/s) in the z-axis of the robot, in the robot frame. | |
delay | float | The amount of seconds to delay walking after standing from fall. | |
ankleImuParamX | arraygyro_stab | This is used to apply torque in the X-direction from the ankles. | |
kneeImuParamX | arraygyro_stab | This is used to apply torque in the X-direction from the knees. | |
ankleImuParamY | arraygyro_stab | This is used to apply torque in the y-direction on the ankles. | |
hipImuParamY | arraygyro_stab | This is used to apply torque in the y-direction on the hip. | |
imuOn | bool | Flag to enable/disable the gyro based stabilization. | |
fsrOn | bool | Flag to enable/disable the foot sensor based stabilization. | |
jointFeedbackOn | bool | Flag to enable/disable the joint encoder based feedback. |
Param Type | Decription | ||
---|---|---|---|
tablecam_param |
A cam_param table is an array of tables that have two entries: key and val. Where key is the string name of the camera parameter and val is the value to set. The following is a simple example of a cam_param table to set the camera brightness and gain:cam_param = {{key='brightness', val=100}, {key='gain', val=64}};
|
||
arraycam_pos |
The cam_pos parameter is an array of position vectors, {x, y, z} , indicating the position of the camera.
|
||
arraycam_angle |
The cam_angle parameter is an array of Euler angles, {x, y, z} , indicating the rotation of the camera.
|
||
arraygyro_stab |
The gyro_stab, gyro stability, parameter format is an array with the following format: {alpha, gain, xxx, deadband}
|
The following is an example of a complete Config file:
module(..., package.seeall);
require('vector')
require('parse_hostname')
-- Color Label Indexes
color = {};
color.orange = 1;
color.yellow = 2;
color.cyan = 4;
color.field = 8;
color.white = 16;
-- Platform Parameters
platform = {};
platform.name = 'nao'
-- Devive Interface Libraries
dev = {};
dev.body = 'NaoBody';
dev.camera = 'NaoCam';
dev.kinematics = 'NaoKinematics';
dev.comm = 'NaoComm';
dev.monitor_comm = 'NaoMonitorComm';
dev.game_control = 'NaoGameControl';
dev.walk = 'NaoWalk';
dev.kick = 'NaoKick';
-- Game Parameters
game = {};
game.teamNumber = 26;
game.playerID = parse_hostname.get_player_id();
game.robotID = game.playerID;
game.teamColor = parse_hostname.get_team_color();
game.nPlayers = 4;
-- FSM Parameters
fsm = {};
fsm.game = 'RoboCup';
if (game.playerID == 1) then
fsm.body = {'NaoGoalie'};
fsm.head = {'NaoGoalie'};
else
fsm.body = {'NaoDribble'};
fsm.head = {'NaoPlayer'};
end
-- Team Parameters
team = {};
team.msgTimeout = 5.0;
team.nonAttackerPenalty = 6.0;
team.nonDefenderPenalty = 0.5;
--Head Parameters
head = {};
head.camOffsetZ = 0.41;
head.pitchMin = -35*math.pi/180;
head.pitchMax = 30*math.pi/180;
head.yawMin = -120*math.pi/180;
head.yawMax = 120*math.pi/180;
head.cameraPos = {{0.05390, 0.0, 0.06790},
{0.04880, 0.0, 0.02381}};
head.cameraAngle = {{0.0, 0.0, 0.0},
{0.0, 40*math.pi/180, 0.0}};
head.neckZ = 0.14;
head.neckX = 0;
head.bodyTilt = 0;
-- Keyframe Files
km = {};
km.kick_right = 'km_Nao_KickForwardRight.lua';
km.kick_left = 'km_Nao_KickForwardLeft.lua';
km.standup_front = 'km_Nao_StandupFromFrontFaster.lua';
km.standup_back = 'km_Nao_StandupFromBackFasterNew.lua';
-- Sitting Parameters
sit = {};
sit.bodyHeight = 0.22;
sit.supportX = 0;
sit.dpLimit = vector.new({.1,.01,.03,.1,.3,.1});
-- Standing Parameters
stance = {};
stance.dpLimit = vector.new({.04, .03, .04, .05, .4, .1});
stance.delay = 80;
-- Camera Parameters
camera = {};
camera.ncamera = 2;
camera.switchFreq = 5;
camera.width = 320;
camera.height = 240;
camera.x_center = 160;
camera.y_center = 120;
camera.focal_length = 383;
camera.focal_base = 320;
camera.auto_param = {};
camera.auto_param[1] = {key='auto_exposure', val={0, 0}};
camera.auto_param[2] = {key='auto_white_balance', val={0, 0}};
camera.auto_param[3] = {key='autogain', val={0, 0}};
camera.param = {};
camera.param[1] = {key='exposure', val={150, 150}};
camera.param[2] = {key='gain', val={113, 113}};
camera.param[3] = {key='brightness', val={89, 89}};
camera.param[4] = {key='contrast', val={64, 64}};
camera.param[5] = {key='saturation', val={215, 215}};
camera.param[6] = {key='red_balance', val={67, 67}};
camera.param[7] = {key='blue_balance', val={160, 160}};
camera.param[8] = {key='hue', val={0, 0}};
camera.lut_file = 'lut_grasp_green_lines.raw';
-- Vision Parameters
vision = {};
vision.maxFPS = 30;
vision.scaleB = 4;
vision.ball_diameter = 0.065;
vision.ball_height_max = -0.20;
vision.yellow_goal_count_thres = 150;
vision.enable_line_detection = 1;
vision.enable_spot_detection = 0;
vision.enable_midfield_landmark_detection = 0;
vision.enable_velocity_detection = 0;
vision.copy_image_to_shm = 1;
vision.store_all_images = 1;
vision.store_goal_detections = 0;
vision.store_ball_detections = 0;
vision.check_for_ground = 1;
vision.use_point_goal = 0;
vision.ballColor = color.orange;
vision.goal1Color = color.yellow;
vision.goal2Color = color.cyan;
-- World Parameters
world = {};
world.n = 100;
world.xLineBoundary = 3.0;
world.yLineBoundary = 2.0;
world.xMax = 3.2;
world.yMax = 2.2;
world.goalWidth = 1.40;
world.ballYellow= {{3.0,0.0}};
world.ballCyan= {{-3.0,0.0}};
world.postYellow = {};
world.postYellow[1] = {3.0, 0.70};
world.postYellow[2] = {3.0, -0.70};
world.postCyan = {};
world.postCyan[1] = {-3.0, -0.70};
world.postCyan[2] = {-3.0, 0.70};
world.spot = {};
world.spot[1] = {-1.20, 0};
world.spot[2] = {1.20, 0};
world.landmarkCyan = {0.0, -2.4};
world.landmarkYellow = {0.0, 2.4};
world.cResample = 10;
world.odomScale = {1.06, 1.0, 0.97};
world.rGoalFilter = 0.02;
world.aGoalFilter = 0.05;
world.rPostFilter = 0.02;
world.aPostFilter = 0.20;
-- Walk Parameters
walk = {};
walk.tStep = 0.45;
walk.tZmp = 0.17;
walk.bodyHeight = 0.31;
walk.stepHeight = 0.018;
walk.footY = 0.0475;
walk.supportX = 0.020;
walk.supportY = 0.003;
walk.hipPitchCompensation = 0;
walk.anklePitchCompensation = -3*math.pi/180;
walk.anklePitchComp = {0, 0};
walk.tSensorDelay = 0.035;
walk.torsoSensorGainX = 0.0;
walk.torsoSensorGainY = 0.01;
walk.phSingle = {0.16, 0.84};
walk.maxX = {-.06, .06};
walk.maxY = {-.045, .045};
walk.maxZ = {-.3, .3};
walk.fsr_threshold = 0.3;
walk.tDelayBalance = .6;
walk.gyroFactor = 0.001;
walk.ankleImuParamX = {0.15, -0.40*walk.gyroFactor, 1*math.pi/180, 5*math.pi/180};
walk.kneeImuParamX = {0.1, -0.3*walk.gyroFactor, .5*math.pi/180, 5*math.pi/180};
walk.ankleImuParamY = {0.10, -2.5*walk.gyroFactor, .5*math.pi/180, 5*math.pi/180};
walk.hipImuParamY = {0.1, -0.3*walk.gyroFactor, .5*math.pi/180, 5*math.pi/180};
walk.delay = 2;
walk.imuOn = true;
walk.fsrOn = true;
walk.jointFeedbackOn = false;
walk.qLArm = math.pi/180*vector.new({105, 12, -85, -30});
walk.qRArm = math.pi/180*vector.new({105, -12, 85, 30});
-- Kick Parameters
kick={}
kick.tSensorDelay = 0.10;
kick.torsoSensorParamX = {1-math.exp(-.010/0.2), 0}
kick.torsoSensorParamY = {1-math.exp(-.010/0.2), 0}
gyroFactor = 0.001;
kick.ankleImuParamX = {0.1, -0.3*gyroFactor, 1*math.pi/180, 5*math.pi/180};
kick.kneeImuParamX = {0.1, -0.4*gyroFactor, .5*math.pi/180, 5*math.pi/180};
kick.ankleImuParamY = {0.1, -0.7*gyroFactor, .5*math.pi/180, 5*math.pi/180};
kick.hipImuParamY = {0.1, -0.3*gyroFactor, .5*math.pi/180, 5*math.pi/180};
kick.armImuParamX = {0.0, -10*gyroFactor, 20*math.pi/180, 45*math.pi/180};
kick.armImuParamY = {0.0, -10*gyroFactor, 20*math.pi/180, 45*math.pi/180};
kick.qLArm = math.pi/180*vector.new({105, 20, -85, -30});
kick.qRArm = math.pi/180*vector.new({105, -20, 85, 30});
kick.armGain = 0.20;
kick.supportCompL = vector.new({0, 0, 0});
kick.supportCompR = vector.new({0, 0, 0} );
kick.kickLeft = { {1, 0.6, {0,0,0}}, --Stabilize
{1, 0.6, {0,-0.06,0}}, --COM slide
{2, 0.3, {0,-0.06,0}, {-0.07,-0.03,0}, 0.07, 20*math.pi/180}, --Lifting
{4, 0.3, {0,-0.06,0}, {0.20,0,0}, 0.04, -10*math.pi/180}, --Kicking
{2, 0.6, {0,-0.06,0}, {-0.13,0.03,0}, 0, 0}, --Landing
{1, 0.6, {0.00,-0.0, 0}}, --COM slide
{1, 0.6, {0.00,-0.0, 0}}} --Stabilize
kick.kickRight = {{1, 0.6, {0,0,0}}, --Stabilize
{1, 0.6, {0,0.060,0}}, --COM slide
{3, 0.3, {0,0.060,0}, {-0.07,0.03,0}, 0.07, 20*math.pi/180}, --Lifting
{5, 0.3, {0,0.060,0}, {0.20,0,0}, 0.04, -10*math.pi/180}, --Kicking
{3, 0.6, {0,0.060,0}, {-0.13,-0.03,0}, 0, 0}, --Landing
{1, 0.6, {0.00, 0.0, 0}}, --COM slide
{1, 0.6, {0.00, 0.0, 0}}} --Stabilize
kick.kickSideLeft = { {1, 1, {0,0,0}}, --Stabilize
{1, 0.6, {0,-0.06,0}}, --COM slide
{2, 0.6, {0,-0.06,0}, {-0.07,-0.03,0}, 0.07, 20*math.pi/180}, --Lifting
{4, 0.3, {0,-0.06,0}, {0.20,0,0}, 0.04, -10*math.pi/180}, --Kicking
{2, 0.6, {0,-0.06,0}, {-0.07,0.030,0}, 0, 0}, --Landing
{1, 0.6, {0.03,0, 0}}, --COM slide
{1, 0.6, {0.03,0, 0}}} --Stabilize
kick.kickSideRight = {{1, 1, {0,0,0}}, --Stabilize
{1, 0.6, {0,0.060,0}}, --COM slide
{3, 0.6, {0,0.060,0}, {-0.07,0.03,0}, 0.07, 20*math.pi/180}, --Lifting
{5, 0.3, {0,0.060,0}, {0.20,0,0}, 0.04, -10*math.pi/180}, --Kicking
{3, 0.6, {0,0.060,0}, {-0.07,-0.030,0}, 0, 0}, --Landing
{1, 0.6, {0.03, 0, 0}}, --COM slide
{1, 0.6, {0.03, 0, 0}}} --Stabilize