/// 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; } }