/// processes the 'camera' tag
void process_camera_tag(XMLTreeConstPtr node)
{
if (!ONSCREEN_RENDER)
return;
// read all attributes
const XMLAttrib* target_attr = node->get_attrib("target");
const XMLAttrib* position_attr = node->get_attrib("position");
const XMLAttrib* up_attr = node->get_attrib("up");
if (!target_attr || !position_attr || !up_attr)
return;
// get the actual values
Vector3 target, position, up;
target_attr->get_vector_value(target);
position_attr->get_vector_value(position);
up_attr->get_vector_value(up);
// setup osg vectors
#ifdef USE_OSG
osg::Vec3d position_osg(position[0], position[1], position[2]);
osg::Vec3d target_osg(target[0], target[1], target[2]);
osg::Vec3d up_osg(up[0], up[1], up[2]);
// set the camera view
if (viewer_pointer && viewer_pointer->getCameraManipulator())
{
osg::Camera* camera = viewer_pointer->getCamera();
camera->setViewMatrixAsLookAt(position_osg, target_osg, up_osg);
// setup the manipulator using the camera, if necessary
viewer_pointer->getCameraManipulator()->setHomePosition(position_osg, target_osg, up_osg);
}
#endif
}
/// Implements Base::load_from_xml()
void OSGGroupWrapper::load_from_xml(XMLTreeConstPtr node, std::map<std::string, BasePtr>& id_map)
{
// load the Base data
Base::load_from_xml(node, id_map);
// verify the node name
assert(strcasecmp(node->name.c_str(), "OSGGroup") == 0);
// if there is no visualization data filename, return now
const XMLAttrib* viz_fname_attr = node->get_attrib("filename");
if (!viz_fname_attr)
return;
// get the filename
const std::string& fname = viz_fname_attr->get_string_value();
// open the filename and read in the file
osg::Node* osgnode = osgDB::readNodeFile(fname);
if (!osgnode)
{
std::cerr << "OSGGroupWrapper::load_from_xml() - unable to read from ";
std::cerr << fname << "!" << std::endl;
return;
}
// remove all children from the root separator
_group->removeChildren(0, _group->getNumChildren());
// read in the transform, if specified
const XMLAttrib* transform_attr = node->get_attrib("transform");
if (transform_attr)
{
Matrix4 T;
transform_attr->get_matrix_value(T);
if (!Matrix4::valid_transform(T))
throw InvalidTransformException(T);
// create the SoTransform and add it to the root separator
osg::Matrixd m;
to_osg_matrix(T, m);
osg::MatrixTransform* mgroup = new osg::MatrixTransform;
mgroup->setMatrix(m);
_group->unref();
_group = mgroup;
_group->ref();
}
// add the read node to the group
_group->addChild(osgnode);
}
/// Implements Base::load_from_xml()
void SphericalJoint::load_from_xml(XMLTreeConstPtr node, std::map<std::string, BasePtr>& id_map)
{
// read the information from the articulated body joint
Joint::load_from_xml(node, id_map);
// verify that the node name is correct
assert(strcasecmp(node->name.c_str(), "SphericalJoint") == 0);
// read local joint axes
const XMLAttrib* laxis_attrib = node->get_attrib("local-axis");
if (laxis_attrib)
{
Vector3 laxis;
laxis_attrib->get_vector_value(laxis);
set_axis_local(laxis, eAxis1);
}
const XMLAttrib* laxis1_attrib = node->get_attrib("local-axis1");
if (laxis1_attrib)
{
Vector3 laxis1;
laxis1_attrib->get_vector_value(laxis1);
set_axis_local(laxis1, eAxis1);
}
const XMLAttrib* laxis2_attrib = node->get_attrib("local-axis2");
if (laxis2_attrib)
{
Vector3 laxis2;
laxis2_attrib->get_vector_value(laxis2);
set_axis_local(laxis2, eAxis2);
}
// read the global joint axes, if given
const XMLAttrib* gaxis_attrib = node->get_attrib("global-axis");
if (gaxis_attrib)
{
Vector3 gaxis;
gaxis_attrib->get_vector_value(gaxis);
set_axis_global(gaxis, eAxis1);
}
const XMLAttrib* gaxis1_attrib = node->get_attrib("global-axis1");
if (gaxis1_attrib)
{
Vector3 gaxis1;
gaxis1_attrib->get_vector_value(gaxis1);
set_axis_global(gaxis1, eAxis1);
}
const XMLAttrib* gaxis2_attrib = node->get_attrib("global-axis2");
if (gaxis2_attrib)
{
Vector3 gaxis2;
gaxis2_attrib->get_vector_value(gaxis2);
set_axis_global(gaxis2, eAxis2);
}
// compute _q_tare if necessary
determine_q_tare();
}
/// processes the 'window' tag
void process_window_tag(XMLTreeConstPtr node)
{
// don't process if not onscreen rendering
if (!ONSCREEN_RENDER)
return;
// read window location
const XMLAttrib* loc_attr = node->get_attrib("location");
// read window size
const XMLAttrib* size_attr = node->get_attrib("size");
// get the actual values
Vector2 loc(0,0), size(640,480);
if (loc_attr)
loc_attr->get_vector_value(loc);
if (size_attr)
size_attr->get_vector_value(size);
#ifdef USE_OSG
// setup the window
viewer_pointer->setUpViewInWindow(loc[0], loc[1], size[0], size[1]);
#endif
}
/**
* \param node the subtree under which all data necessary to load this
* object is stored
* \param id_map a map from node IDs to read objects
*/
void Base::load_from_xml(XMLTreeConstPtr node, std::map<std::string, BasePtr>& id_map)
{
// read the ID, if specified
const XMLAttrib* id_attrib = node->get_attrib("id");
if (id_attrib)
id = id_attrib->get_string_value();
// verify that the ID does not already exist in the map
if (id_map.find(id) != id_map.end())
{
std::cerr << "Base::load_from_xml() - \"unique\" ID '" << id << "' ";
std::cerr << " already exists in the id_map! Adding anyway...";
std::cerr << std::endl;
}
// add the ID to the ID map
id_map[id] = shared_from_this();
}
/**
* \pre node is named JointPlugin
*/
void XMLReader::read_joint_plugin(XMLTreeConstPtr node, std::map<std::string, BasePtr>& id_map)
{
// sanity check
assert(strcasecmp(node->name.c_str(), "JointPlugin") == 0);
// get the name of the plugin to load
const XMLAttrib* plugin_attr = node->get_attrib("plugin");
if (!plugin_attr)
{
std::cerr << "XMLReader::read_joint_plugin() - no plugin attribute!" << std::endl;
return;
}
std::string pluginname = plugin_attr->get_string_value();
// verify that the plugin can be found
struct stat filestatus;
if (stat(pluginname.c_str(), &filestatus) != 0)
{
std::cerr << "XMLReader::read_joint_plugin() - unable to find plugin '" << pluginname << "'" << std::endl;
return;
}
// load the plugin
void* plugin = dlopen(pluginname.c_str(), RTLD_LAZY);
if (!plugin)
{
std::cerr << "XMLReader::read_joint_plugin()- cannot load plugin: " << dlerror() << std::endl;
return;
}
// load the factory symbol
boost::shared_ptr<Joint> (*factory)(void) = (boost::shared_ptr<Joint> (*) (void)) dlsym(plugin, "factory");
if (!factory)
{
std::cerr << "XMLReader::read_joint_plugin()- factory() not found in " << pluginname << std::endl;
return;
}
// create a new Joint object
boost::shared_ptr<Joint> joint_plugin = factory();
// populate the object
joint_plugin->load_from_xml(node, id_map);
}
/// Gets the tuple type from a node
XMLReader::TupleType XMLReader::get_tuple(XMLTreeConstPtr node)
{
std::string type;
// get the 'type' attribute
const XMLAttrib* type_attr = node->get_attrib("type");
if (type_attr)
type = type_attr->get_string_value();
// look for possible tuple types
if (strcasecmp(type.c_str(), "VectorN") == 0)
return eVectorN;
if (strcasecmp(type.c_str(), "Vector3") == 0)
return eVector3;
if (strcasecmp(type.c_str(), "Quat") == 0)
return eQuat;
// still here? not one of the above...
return eNone;
}
/// Implements Base::load_from_xml()
void Visualizable::load_from_xml(XMLTreeConstPtr node, std::map<std::string, BasePtr>& id_map)
{
OSGGroupWrapperPtr wrap;
// load parent data
Base::load_from_xml(node, id_map);
// get the relevant attributes
const XMLAttrib* viz_id_attr = node->get_attrib("visualization-id");
const XMLAttrib* vfile_id_attr = node->get_attrib("visualization-filename");
// get whether there are any visualization nodes
std::list<XMLTreeConstPtr> viz_nodes = node->find_child_nodes("Visualization");
// check that some visualization data exists
if (!viz_id_attr && !vfile_id_attr && viz_nodes.empty())
return;
// check for mix-and-match
if ((viz_id_attr && vfile_id_attr) || (viz_id_attr && vfile_id_attr) ||
(vfile_id_attr && !viz_nodes.empty()))
{
std::cerr << "Visualizable::load_from_xml() - found mix and match of ";
std::cerr << "visualization-id, visualization-filename, " << std::endl;
std::cerr << " and/or Visualization node in offending node: " << std::endl;
std::cerr << *node;
return;
}
#ifdef USE_OSG
// if one of the attributes has been set, get the group
if (viz_id_attr || vfile_id_attr)
{
osg::Group* group = construct_from_node(node, id_map);
if (group)
set_visualization_data(group);
}
else
{
// if we are here, then one or more Visualization nodes encountered
// create a new group to hold all data read..
osg::Group* root = NULL;
// handle all Visualization nodes
for (std::list<XMLTreeConstPtr>::const_iterator i = viz_nodes.begin(); i != viz_nodes.end(); i++)
{
// get the group from the child node
osg::Group* child_group = construct_from_node(*i, id_map);
// if the child group is NULL, continue looping
if (!child_group)
continue;
// look for a visualization-rel-transform attribute
const XMLAttrib* rel_trans_attr = (*i)->get_attrib("visualization-rel-transform");
if (rel_trans_attr)
{
// create a new transform group
osg::MatrixTransform* xgroup = new osg::MatrixTransform;
// create the transform and set it
Matrix4 Tx;
rel_trans_attr->get_matrix_value(Tx);
osg::Matrixd T;
to_osg_matrix(Tx, T);
xgroup->setMatrix(T);
// add the child to the transform group
xgroup->addChild(child_group);
// point child_sep to new_sep (its parent)
child_group = xgroup;
}
// create the root, if need be
if (!root)
root = new osg::Group;
// add the child group to the root group
root->addChild(child_group);
}
// set the visualization, if available
if (root)
set_visualization_data(root);
}
#endif
// update the visualization transform
update_visualization();
}
/// Implements Base::load_from_xml()
void Joint::load_from_xml(XMLTreeConstPtr node, std::map<std::string, BasePtr>& id_map)
{
std::map<std::string, BasePtr>::const_iterator id_iter;
// ***********************************************************************
// don't verify that the node is correct, b/c Joint will
// be subclassed
// ***********************************************************************
// load the parent data
Visualizable::load_from_xml(node, id_map);
// read the lower limits, if given
const XMLAttrib* lolimit_attr = node->get_attrib("lower-limits");
if (lolimit_attr)
lolimit_attr->get_vector_value(lolimit);
// read the upper limits, if given
const XMLAttrib* hilimit_attr = node->get_attrib("upper-limits");
if (hilimit_attr)
hilimit_attr->get_vector_value(hilimit);
// read the maximum actuator force, if given
const XMLAttrib* maxforce_attr = node->get_attrib("max-forces");
if (maxforce_attr)
maxforce_attr->get_vector_value(maxforce);
// read the joint positions, if given
const XMLAttrib* q_attr = node->get_attrib("q");
if (q_attr)
q_attr->get_vector_value(q);
else
q.set_zero(num_dof());
// read the joint velocities, if given
const XMLAttrib* qd_attr = node->get_attrib("qd");
if (qd_attr)
qd_attr->get_vector_value(qd);
// read the joint positions, if given
const XMLAttrib* q_init_attr = node->get_attrib("q-tare");
if (q_init_attr)
{
_determine_q_tare = false;
q_init_attr->get_vector_value(_q_tare);
}
else
_determine_q_tare = true;
// read the Coulomb friction coefficient, if given
const XMLAttrib* fc_attr = node->get_attrib("coulomb-friction-coeff");
if (fc_attr)
mu_fc = fc_attr->get_real_value();
// read the viscous friction coefficient, if given
const XMLAttrib* fv_attr = node->get_attrib("viscous-friction-coeff");
if (fv_attr)
mu_fv = fv_attr->get_real_value();
// read the restitution coefficient, if given
const XMLAttrib* resti_attr = node->get_attrib("restitution-coeff");
if (resti_attr)
limit_restitution = resti_attr->get_real_value();
// read the articulated body, if given
const XMLAttrib* ab_attr = node->get_attrib("articulated-body-id");
if (ab_attr)
{
// get the ID
const std::string& ID = ab_attr->get_string_value();
// look for the ID -- only warn if it is not found
if ((id_iter = id_map.find(ID)) == id_map.end())
{
#ifdef _DEBUG_XML_
std::cout << "Joint::load_from_xml() warning - ";
std::cout << "articulated body" << std::endl << " '" << ID << "' not ";
std::cout << "found" << std::endl << " ** This warning could result ";
std::cout << "from joints being constructed before articulated bodies ";
std::cout << std::endl << " and may not be serious..." << std::endl;
std::cout << " offending node: " << std::endl << *node;
#endif
}
else
set_articulated_body(boost::dynamic_pointer_cast<RCArticulatedBody>(id_iter->second));
}
// read the inboard link id, if given
const XMLAttrib* inboard_attr = node->get_attrib("inboard-link-id");
if (inboard_attr)
{
// get the ID of the inboard link
const std::string& id = inboard_attr->get_string_value();
// complain if the link not found but don't treat it as an error-- there
// are circular dependencies
if ((id_iter = id_map.find(id)) == id_map.end())
{
#ifdef _DEBUG_XML_
std::cout << "Joint::load_from_xml() warning - link ";
std::cout << id << " not found" << std::endl;
std::cout << " ** This warning could result from joints being ";
std::cout << " constructed before links" << std::endl << " and may ";
std::cout << "not be serious" << std::endl;
std::cout << " offending node: " << std::endl << *node;
#endif
}
else
{
RigidBodyPtr inboard(boost::dynamic_pointer_cast<RigidBody>(id_iter->second));
set_inboard_link(inboard);
}
}
// read the outboard link id, if given
const XMLAttrib* outboard_attr = node->get_attrib("outboard-link-id");
if (outboard_attr)
{
// get the ID of the outboard link
const std::string& id = outboard_attr->get_string_value();
// complain if the link not found but don't treat it as an error-- there
// are circular dependencies
if ((id_iter = id_map.find(id)) == id_map.end())
{
#ifdef _DEBUG_XML_
std::cout << "Joint::load_from_xml() warning - link ";
std::cout << id << " not found" << std::endl;
std::cout << " ** This warning could result from joints being ";
std::cout << " constructed before links" << std::endl << " and may ";
std::cout << "not be serious" << std::endl;
std::cout << " offending node: " << std::endl << *node;
#endif
}
else
{
RigidBodyPtr outboard(boost::dynamic_pointer_cast<RigidBody>(id_iter->second));
set_outboard_link(outboard);
}
}
// get the global position of the joint, if possible
const XMLAttrib* pos_attr = node->get_attrib("global-position");
if (pos_attr)
{
// get the position of the joint
Vector3 position;
pos_attr->get_vector_value(position);
// make sure that both inboard and outboard links have been set
if (!get_inboard_link() || !get_outboard_link())
{
std::cerr << "Joint::load_from_xml() - global position";
std::cerr << " specified w/o " << std::endl << " inboard and/or";
std::cerr << " outboard links set!" << std::endl;
return;
}
// get the inboard and outboard links
RigidBodyPtr inboard(get_inboard_link());
RigidBodyPtr outboard(get_outboard_link());
// get the transforms for the two bodies
const Matrix4& Ti = inboard->get_transform();
const Matrix4& To = outboard->get_transform();
// determine the vector from the inboard link to the joint (link coords)
Vector3 inboard_to_joint = Ti.inverse_mult_point(position);
// determine the vector from the joint to the outboard link (link coords)
Vector3 joint_to_outboard_lf = -To.inverse_mult_point(position);
// NOTE: the calculation immediately below assumes that the induced
// transform (i.e., the transform that the joint applies) is initally
// identity
// compute the vector from the joint to the outboard link in joint frame
Vector3 joint_to_outboard_jf = (Ti.inverse_transform() * To).get_translation() - inboard_to_joint;
// add/replace this as an inner joint
inboard->add_outer_joint(outboard, get_this(), inboard_to_joint);
outboard->add_inner_joint(inboard, get_this(), joint_to_outboard_jf, joint_to_outboard_lf);
}
// get the spatial axis in link coordinates; note that this must be done
// after the link IDs are set
const XMLAttrib* sa_link_attr = node->get_attrib("spatial-axis-link");
if (sa_link_attr)
{
SMatrix6N si;
sa_link_attr->get_matrix_value(si);
if (si.columns() != num_dof())
throw std::runtime_error("Incorrect spatial matrix size reading XML attribute spatial-axis-link");
_si = si;
}
}
