bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *config, ErrorLogger* logger)
{
const char* linkName = config->Attribute("name");
if (!linkName)
{
logger->reportError("Link with no name");
return false;
}
link.m_name = linkName;
if (m_parseSDF) {
TiXmlElement* pose = config->FirstChildElement("pose");
if (0==pose)
{
link.m_linkTransformInWorld.setIdentity();
}
else
{
parseTransform(link.m_linkTransformInWorld, pose,logger,m_parseSDF);
}
}
// Inertial (optional)
TiXmlElement *i = config->FirstChildElement("inertial");
if (i)
{
if (!parseInertia(link.m_inertia, i,logger))
{
logger->reportError("Could not parse inertial element for Link:");
logger->reportError(link.m_name.c_str());
return false;
}
} else
{
if ((strlen(linkName)==5) && (strncmp(linkName, "world", 5))==0)
{
link.m_inertia.m_mass = 0.f;
link.m_inertia.m_linkLocalFrame.setIdentity();
link.m_inertia.m_ixx = 0.f;
link.m_inertia.m_iyy = 0.f;
link.m_inertia.m_izz= 0.f;
} else
{
logger->reportWarning("No inertial data for link, using mass=1, localinertiadiagonal = 1,1,1, identity local inertial frame");
link.m_inertia.m_mass = 1.f;
link.m_inertia.m_linkLocalFrame.setIdentity();
link.m_inertia.m_ixx = 1.f;
link.m_inertia.m_iyy = 1.f;
link.m_inertia.m_izz= 1.f;
logger->reportWarning(link.m_name.c_str());
}
}
// Multiple Visuals (optional)
for (TiXmlElement* vis_xml = config->FirstChildElement("visual"); vis_xml; vis_xml = vis_xml->NextSiblingElement("visual"))
{
UrdfVisual visual;
if (parseVisual(model, visual, vis_xml,logger))
{
link.m_visualArray.push_back(visual);
}
else
{
logger->reportError("Could not parse visual element for Link:");
logger->reportError(link.m_name.c_str());
return false;
}
}
// Multiple Collisions (optional)
for (TiXmlElement* col_xml = config->FirstChildElement("collision"); col_xml; col_xml = col_xml->NextSiblingElement("collision"))
{
UrdfCollision col;
if (parseCollision(col, col_xml,logger))
{
link.m_collisionArray.push_back(col);
}
else
{
logger->reportError("Could not parse collision element for Link:");
logger->reportError(link.m_name.c_str());
return false;
}
}
return true;
}
bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *config, ErrorLogger* logger)
{
const char* linkName = config->Attribute("name");
if (!linkName)
{
logger->reportError("Link with no name");
return false;
}
link.m_name = linkName;
if (m_parseSDF) {
TiXmlElement* pose = config->FirstChildElement("pose");
if (0==pose)
{
link.m_linkTransformInWorld.setIdentity();
}
else
{
parseTransform(link.m_linkTransformInWorld, pose,logger,m_parseSDF);
}
}
{
//optional 'contact' parameters
TiXmlElement* ci = config->FirstChildElement("contact");
if (ci)
{
TiXmlElement *damping_xml = ci->FirstChildElement("inertia_scaling");
if (damping_xml)
{
if (m_parseSDF)
{
link.m_contactInfo.m_inertiaScaling = urdfLexicalCast<double>(damping_xml->GetText());
link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_INERTIA_SCALING;
} else
{
if (!damping_xml->Attribute("value"))
{
logger->reportError("Link/contact: damping element must have value attribute");
return false;
}
link.m_contactInfo.m_inertiaScaling = urdfLexicalCast<double>(damping_xml->Attribute("value"));
link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_INERTIA_SCALING;
}
}
{
TiXmlElement *friction_xml = ci->FirstChildElement("lateral_friction");
if (friction_xml)
{
if (m_parseSDF)
{
link.m_contactInfo.m_lateralFriction = urdfLexicalCast<double>(friction_xml->GetText());
} else
{
if (!friction_xml->Attribute("value"))
{
logger->reportError("Link/contact: lateral_friction element must have value attribute");
return false;
}
link.m_contactInfo.m_lateralFriction = urdfLexicalCast<double>(friction_xml->Attribute("value"));
}
}
}
}
}
// Inertial (optional)
TiXmlElement *i = config->FirstChildElement("inertial");
if (i)
{
if (!parseInertia(link.m_inertia, i,logger))
{
logger->reportError("Could not parse inertial element for Link:");
logger->reportError(link.m_name.c_str());
return false;
}
} else
{
if ((strlen(linkName)==5) && (strncmp(linkName, "world", 5))==0)
{
link.m_inertia.m_mass = 0.f;
link.m_inertia.m_linkLocalFrame.setIdentity();
link.m_inertia.m_ixx = 0.f;
link.m_inertia.m_iyy = 0.f;
link.m_inertia.m_izz= 0.f;
} else
{
logger->reportWarning("No inertial data for link, using mass=1, localinertiadiagonal = 1,1,1, identity local inertial frame");
link.m_inertia.m_mass = 1.f;
link.m_inertia.m_linkLocalFrame.setIdentity();
link.m_inertia.m_ixx = 1.f;
link.m_inertia.m_iyy = 1.f;
link.m_inertia.m_izz= 1.f;
logger->reportWarning(link.m_name.c_str());
}
}
// Multiple Visuals (optional)
for (TiXmlElement* vis_xml = config->FirstChildElement("visual"); vis_xml; vis_xml = vis_xml->NextSiblingElement("visual"))
{
UrdfVisual visual;
if (parseVisual(model, visual, vis_xml,logger))
{
link.m_visualArray.push_back(visual);
}
else
{
logger->reportError("Could not parse visual element for Link:");
logger->reportError(link.m_name.c_str());
return false;
}
}
// Multiple Collisions (optional)
for (TiXmlElement* col_xml = config->FirstChildElement("collision"); col_xml; col_xml = col_xml->NextSiblingElement("collision"))
{
UrdfCollision col;
if (parseCollision(col, col_xml,logger))
{
link.m_collisionArray.push_back(col);
}
else
{
logger->reportError("Could not parse collision element for Link:");
logger->reportError(link.m_name.c_str());
return false;
}
}
return true;
}
/*!
*/
void
BodySensor::parse1( const char * msg,
const double & version,
const GameTime & current )
{
// ver. 4 & under
// (sense_body 0 (view_mode high normal) (stamina 4000 1) (speed 0)
// (kick 0) (dash 0) (turn 0) (say 0))
// ver. 5
// (sense_body 0 (view_mode high normal) (stamina 4000 1) (speed 0) (head_angle 0)
// (kick 0) (dash 0) (turn 0) (say 0) (turn_neck 0))
// ver. 6
// (sense_body 0 (view_mode high normal) (stamina 4000 1) (speed 0 0) (head_angle 0)
// (kick 0) (dash 0) (turn 0) (say 0) (turn_neck 0))
// ver. 7
// (sense_body 0 (view_mode high normal) (stamina 4000 1) (speed 0 0) (head_angle 0)
// (kick 0) (dash 0) (turn 0) (say 0) (turn_neck 0) (catch 0) (move 1) (change_view 0))
// ver. 8
// (sense_body 66 (view_mode high normal) (stamina 3503.4 1) (speed 0.06 -79)
// (head_angle 89) (kick 4) (dash 20) (turn 24) (say 0) (turn_neck 28) (catch 0)
// (move 1) (change_view 16) (arm (movable 0) (expires 0) (target 0 0) (count 0))
// (focus (target none) (count 0)) (tackle (expires 0) (count 0)))
// ver. 12
// (sense_body 66 (view_mode high normal) (stamina 3503.4 1) (speed 0.06 -79)
// (head_angle 89) (kick 4) (dash 20) (turn 24) (say 0) (turn_neck 28) (catch 0)
// (move 1) (change_view 16) (arm (movable 0) (expires 0) (target 0 0) (count 0))
// (focus (target none) (count 0)) (tackle (expires 0) (count 0))
// (collision {none|[(ball)][player][post]}))
// ver. 13
// (sense_body 66 (view_mode high normal) (stamina 3503.4 1 124000) (speed 0.06 -79)
// (head_angle 89) (kick 4) (dash 20) (turn 24) (say 0) (turn_neck 28) (catch 0)
// (move 1) (change_view 16) (arm (movable 0) (expires 0) (target 0 0) (count 0))
// (focus (target none) (count 0)) (tackle (expires 0) (count 0))
// (collision {none|[(ball)][player][post]}))
// ver. 14
// (sense_body 66 (view_mode high normal) (stamina 3503.4 1 124000) (speed 0.06 -79)
// (head_angle 89) (kick 4) (dash 20) (turn 24) (say 0) (turn_neck 28) (catch 0)
// (move 1) (change_view 16) (arm (movable 0) (expires 0) (target 0 0) (count 0))
// (focus (target none) (count 0)) (tackle (expires 0) (count 0))
// (collision {none|[(ball)][player][post]}))
// (foul (charged 0) (card {none|yellow|red}))
//char ss[8];
M_time = current;
++msg; // skip first paren
while ( *msg != '\0' && *msg != '(' ) ++msg; // skip "sense_body <time> "
while ( *msg != '\0' && *msg != ' ' ) ++msg; // skip "(view_mode"
++msg; // skip space
// parse view quality
switch ( *msg ) {
case 'h': // high
M_view_quality = ViewQuality::HIGH;
break;
case 'l': // low
M_view_quality = ViewQuality::LOW;
break;
default:
std::cerr << "sense_body: Unknown View Quality" << std::endl;
break;
}
while ( *msg != '\0' && *msg != ' ' ) ++msg; // skip view_quality string
++msg; // skip space
// parse view width
switch ( *( msg + 1 ) ) {
case 'o': // "normal"
M_view_width = ViewWidth::NORMAL;
break;
case 'a': // "narrow"
M_view_width = ViewWidth::NARROW;
break;
case 'i': // "wide"
M_view_width = ViewWidth::WIDE;
break;
default:
std::cerr << "sense_body: Unknown View Width" << std::endl;
break;
}
char *next;
// read stamina values
while ( *msg != '\0' && *msg != '(' ) ++msg;
while ( *msg != '\0' && *msg != ' ' ) ++msg; // skip "(stamina"
M_stamina = std::strtod( msg, &next ); msg = next;
M_effort = std::strtod( msg, &next ); msg = next;
if ( version >= 13.0 )
{
if ( *msg != ')' )
{
M_stamina_capacity = std::strtod( msg, &next ); msg = next;
}
}
// read speed values
while ( *msg != '\0' && *msg != '(' ) ++msg;
while ( *msg != '\0' && *msg != ' ' ) ++msg; // skip "(speed"
M_speed_mag = std::strtod( msg, &next );
msg = next; // this value is quantized by 0.01
if ( version >= 6.0 )
{
// Sensed speed_dir is the velocity dir relative to player's face angle
// global_vel_dir = (sensed_speed_dir + my_global_neck_angle)
M_speed_dir_relative = std::strtod( msg, &next );
msg = next;
}
if ( version >= 5.0 )
{
while ( *msg != '(' ) ++msg;
while ( *msg != ' ' ) ++msg; // skip "(head_angle"
M_neck_relative = std::strtod( msg, &next );
msg = next;
}
while ( *msg != '\0' && *msg != '(' ) ++msg;
while ( *msg != '\0' && *msg != ' ' ) ++msg; // skip "(kick"
M_kick_count = static_cast< int >( std::strtol( msg, &next, 10 ) );
msg = next;
while ( *msg != '\0' && *msg != '(' ) ++msg;
while ( *msg != '\0' && *msg != ' ' ) ++msg; // skip "(dash"
M_dash_count = static_cast< int >( std::strtol( msg, &next, 10 ) );
msg = next;
while ( *msg != '\0' && *msg != '(' ) ++msg;
while ( *msg != '\0' && *msg != ' ' ) ++msg; // skip "(turn"
M_turn_count = static_cast< int >( std::strtol( msg, &next, 10 ) );
msg = next;
while ( *msg != '\0' && *msg != '(' ) ++msg;
while ( *msg != '\0' && *msg != ' ' ) ++msg; // skip "(say"
M_say_count = static_cast< int >( std::strtol( msg, &next, 10 ) );
msg = next;
if ( version < 5.0 )
{
return;
}
while ( *msg != '\0' && *msg != '(' ) ++msg;
while ( *msg != '\0' && *msg != ' ' ) ++msg; // skip "(turn_neck"
M_turn_neck_count = static_cast< int >( std::strtol( msg, &next, 10 ) );
msg = next;
if ( version < 7.0 )
{
return;
}
while ( *msg != '\0' && *msg != '(' ) ++msg;
while ( *msg != '\0' && *msg != ' ' ) ++msg; // skip "(catch"
M_catch_count = static_cast< int >( std::strtol( msg, &next, 10 ) );
msg = next;
while ( *msg != '\0' && *msg != '(' ) ++msg;
while ( *msg != '\0' && *msg != ' ' ) ++msg; // skip "(move"
M_move_count = static_cast< int >( std::strtol( msg, &next, 10 ) );
msg = next;
while ( *msg != '(' ) ++msg;
while ( *msg != ' ' ) ++msg; // skip "(chage_view"
M_change_view_count = static_cast< int >( std::strtol( msg, &next, 10 ) );
msg = next;
if ( version < 8.0 )
{
return;
}
// `(arm (movable <MOVABLE>) (expires <EXPIRES>)
// (target <DIST> <DIR>) (count <COUNT>))'
while ( *msg != '\0' && *msg != '(' ) ++msg;
while ( *msg != '\0' && *msg != ' ' ) ++msg; // skip "(arm"
while ( *msg != '\0' && *msg != '(' ) ++msg;
while ( *msg != '\0' && *msg != ' ' ) ++msg; // skip "(movable"
M_arm_movable = static_cast< int >( std::strtol( msg, &next, 10 ) );
msg = next;
while ( *msg != '\0' && *msg != '(' ) ++msg;
while ( *msg != '\0' && *msg != ' ' ) ++msg; // skip "(expires"
M_arm_expires = static_cast< int >( std::strtol( msg, &next, 10 ) );
msg = next;
while ( *msg != '\0' && *msg != '(' ) ++msg;
while ( *msg != '\0' && *msg != ' ' ) ++msg; // skip "(target"
++msg;
M_pointto_dist = std::strtod( msg, &next );
msg = next;
M_pointto_dir = std::strtod( msg, &next );
msg = next;
while ( *msg != '\0' && *msg != '(' ) ++msg;
while ( *msg != '\0' && *msg != ' ' ) ++msg; // skip "(count"
M_pointto_count = static_cast< int >( std::strtol( msg, &next, 10 ) );
msg = next;
// `(focus (target <SIDE> [<UNUM>]) (count <COUNT>)'
// <SIDE> := "none" | "l" | "r"
while ( *msg != '\0' && *msg != '(' ) ++msg;
while ( *msg != '\0' && *msg != ' ' ) ++msg; // skip "(focus"
while ( *msg != '\0' && *msg != '(' ) ++msg;
while ( *msg != '\0' && *msg != ' ' ) ++msg; // skip "(target"
++msg; // skip space
if ( *msg == 'n' ) // "none"
{
M_attentionto_side = NEUTRAL;
M_attentionto_unum = Unum_Unknown;
}
else if ( *msg == 'l' )
{
M_attentionto_side = LEFT;
++msg;
M_attentionto_unum = static_cast< int >( std::strtol( msg, &next, 10 ) );
msg = next;
}
else if ( *msg == 'r' )
{
M_attentionto_side = RIGHT;
++msg;
M_attentionto_unum = static_cast< int >( std::strtol( msg, &next, 10 ) );
msg = next;
}
else
{
std::cerr << "sense_body: focus ?? [" << msg << std::endl;
}
while ( *msg != '\0' && *msg != '(' ) ++msg;
while ( *msg != '\0' && *msg != ' ' ) ++msg; // skip "(count"
M_attentionto_count = static_cast< int >( std::strtol( msg, &next, 10 ) );
msg = next;
// `(tackle (expires <EXPIRES>) (count <COUNT>))'
while ( *msg != '\0' && *msg != '(' ) ++msg;
while ( *msg != '\0' && *msg != ' ' ) ++msg; // skip "(tackle"
while ( *msg != '\0' && *msg != '(' ) ++msg;
while ( *msg != '\0' && *msg != ' ' ) ++msg; // skip "(expires"
M_tackle_expires = static_cast< int >( std::strtol( msg, &next, 10 ) );
msg = next;
while ( *msg != '\0' && *msg != '(' ) ++msg;
while ( *msg != '\0' && *msg != ' ' ) ++msg; // skip "(count"
M_tackle_count = static_cast< int >( std::strtol( msg, &next, 10 ) );
msg = next;
if ( version < 12.0 )
{
return;
}
while ( *msg != '\0' && *msg != '(' ) ++msg;
//
// (collision {none|[(ball)][(player)][(post)]})
//
parseCollision( msg, &next );
msg = next;
if ( version < 14.0 )
{
return;
}
//
// (foul (charged 0) (card {none|yellow|red}))
//
parseFoul( msg, &next );
}
bool parseLink(Link &link, TiXmlElement* config)
{
link.clear();
const char *name_char = config->Attribute("name");
if (!name_char)
{
logError("No name given for the link.");
return false;
}
link.name = std::string(name_char);
// Inertial (optional)
TiXmlElement *i = config->FirstChildElement("inertial");
if (i)
{
resetPtr(link.inertial,new Inertial());
if (!parseInertial(*link.inertial, i))
{
logError("Could not parse inertial element for Link [%s]", link.name.c_str());
return false;
}
}
// Multiple Visuals (optional)
for (TiXmlElement* vis_xml = config->FirstChildElement("visual"); vis_xml; vis_xml = vis_xml->NextSiblingElement("visual"))
{
VisualPtr vis;
resetPtr(vis,new Visual());
if (parseVisual(*vis, vis_xml))
{
link.visual_array.push_back(vis);
}
else
{
resetPtr(vis);
logError("Could not parse visual element for Link [%s]", link.name.c_str());
return false;
}
}
// Visual (optional)
// Assign the first visual to the .visual ptr, if it exists
if (!link.visual_array.empty())
link.visual = link.visual_array[0];
// Multiple Collisions (optional)
for (TiXmlElement* col_xml = config->FirstChildElement("collision"); col_xml; col_xml = col_xml->NextSiblingElement("collision"))
{
CollisionPtr col;
resetPtr(col,new Collision());
if (parseCollision(*col, col_xml))
{
link.collision_array.push_back(col);
}
else
{
resetPtr(col);
logError("Could not parse collision element for Link [%s]", link.name.c_str());
return false;
}
}
// Collision (optional)
// Assign the first collision to the .collision ptr, if it exists
if (!link.collision_array.empty())
link.collision = link.collision_array[0];
}
/*!
*/
void
BodySensor::parse2( const char * msg,
const double & version,
const GameTime & current )
{
long sense_time;
char vqual[8], vwidth[8];
char attentionto_target[8];
M_time = current;
int n_read = 0;
if ( version >= 8.0
&& std::sscanf( msg,
" (sense_body %ld (view_mode %7[^ ] %7[^)])"
" (stamina %lf %lf)"
" (speed %lf %lf) (head_angle %lf)"
" (kick %d) (dash %d) (turn %d) (say %d) (turn_neck %d)"
" (catch %d) (move %d) (change_view %d)"
" (arm (movable %d) (expires %d)"
" (target %lf %lf) (count %d))"
" (focus (target %[^)]) (count %d))"
" (tackle (expires %d) (count %d)) %n ",
&sense_time, vqual, vwidth,
&M_stamina, &M_effort,
&M_speed_mag, &M_speed_dir_relative,
&M_neck_relative,
&M_kick_count, &M_dash_count,
&M_turn_count, &M_say_count,
&M_turn_neck_count, &M_catch_count,
&M_move_count, &M_change_view_count,
&M_arm_movable , &M_arm_expires,
&M_pointto_dist, &M_pointto_dir,
&M_pointto_count,
attentionto_target , &M_attentionto_count,
&M_tackle_expires, &M_tackle_count,
&n_read ) == 25 )
{
if ( attentionto_target[0] == 'n' )
{
M_attentionto_side = NEUTRAL;
M_attentionto_unum = Unum_Unknown;
}
else if ( attentionto_target[0] == 'l' )
{
M_attentionto_side = LEFT;
M_attentionto_unum
= static_cast< int >( std::strtol( attentionto_target + 2, NULL, 10 ) );
}
else if ( attentionto_target[0] == 'r' )
{
M_attentionto_side = RIGHT;
M_attentionto_unum
= static_cast< int >( std::strtol( attentionto_target + 2, NULL, 10 ) );
}
else
{
std::cerr << "Received unknown attentionto target ["
<< attentionto_target << "]" << std::endl;
M_attentionto_side = NEUTRAL;
M_attentionto_unum = Unum_Unknown;
}
if ( version >= 12.0 )
{
parseCollision( msg + n_read, NULL );
}
}
else if ( version >= 7.0
&& std::sscanf( msg,
"(sense_body %ld (view_mode %7[^ ] %7[^)])"
" (stamina %lf %lf)"
" (speed %lf %lf) (head_angle %lf)"
" (kick %d) (dash %d) (turn %d) (say %d) (turn_neck %d)"
" (catch %d) (move %d) (change_view %d))",
&sense_time, vqual, vwidth,
&M_stamina, &M_effort,
&M_speed_mag, &M_speed_dir_relative,
&M_neck_relative,
&M_kick_count, &M_dash_count,
&M_turn_count, &M_say_count,
&M_turn_neck_count, &M_catch_count,
&M_move_count, &M_change_view_count) == 16 )
{
}
else if ( version >= 6.0
&& std::sscanf( msg,
"(sense_body %ld (view_mode %7[^ ] %7[^)])"
" (stamina %lf %lf)"
" (speed %lf %lf) (head_angle %lf)"
" (kick %d) (dash %d) (turn %d) (say %d) (turn_neck %d)",
&sense_time, vqual, vwidth,
&M_stamina, &M_effort,
&M_speed_mag, &M_speed_dir_relative,
&M_neck_relative,
&M_kick_count, &M_dash_count,
&M_turn_count, &M_say_count,
&M_turn_neck_count) == 13 )
{
}
else if ( version >= 5.0
&& std::sscanf( msg,
"(sense_body %ld (view_mode %7[^ ] %7[^)])"
" (stamina %lf %lf)"
" (speed %lf) (head_angle %lf)"
" (kick %d) (dash %d) (turn %d) (say %d) (turn_neck %d)",
&sense_time, vqual, vwidth,
&M_stamina, &M_effort,
&M_speed_mag,
&M_neck_relative,
&M_kick_count, &M_dash_count,
&M_turn_count, &M_say_count,
&M_turn_neck_count) == 12 )
{
}
else if ( std::sscanf( msg,
"(sense_body %ld (view_mode %7[^ ] %7[^)])"
" (stamina %lf %lf)"
" (speed %lf)"
" (kick %d) (dash %d) (turn %d) (say %d)",
&sense_time, vqual, vwidth,
&M_stamina, &M_effort,
&M_speed_mag,
&M_kick_count, &M_dash_count,
&M_turn_count, &M_say_count) == 10 )
{
}
else
{
std::cerr << "parse2 error" << std::endl;
return;
}
switch ( vqual[0] ) {
case 'h':
M_view_quality = ViewQuality::HIGH;
break;
case 'l':
M_view_quality = ViewQuality::LOW;
//std::cerr << "CAUTION!! sense_body: view quality is LOW" << std::endl;
break;
default:
std::cerr << "sense_body: Unknown View Quality" << std::endl;
break;
}
switch ( vwidth[1] ) {
case 'o': // "normal"
M_view_width = ViewWidth::NORMAL;
break;
case 'a': // "narrow"
M_view_width = ViewWidth::NARROW;
break;
case 'i': // "wide"
M_view_width = ViewWidth::WIDE;
break;
default:
std::cerr << "sense_body: Unknown View Width" << std::endl;
break;
}
}
