38 #include <urdf_parser/urdf_parser.h> 41 #include <boost/lexical_cast.hpp> 43 #define EXPECT_TRUE(arg) if (!(arg)) throw std::runtime_error("Assertion failed at line " + boost::lexical_cast<std::string>(__LINE__)) 45 #ifndef TEST_RESOURCE_LOCATION 46 # define TEST_RESOURCE_LOCATION "." 49 urdf::ModelInterfaceSharedPtr
loadURDF(
const std::string& filename)
53 std::fstream xml_file(filename.c_str(), std::fstream::in);
54 if (xml_file.is_open())
56 while (xml_file.good())
59 std::getline( xml_file, line);
60 xml_string += (line +
"\n");
63 return urdf::parseURDF(xml_string);
67 throw std::runtime_error(
"Could not open file " + filename +
" for parsing.");
68 return urdf::ModelInterfaceSharedPtr();
116 for (std::size_t i = 0 ; i < s.
getGroups().size() ; ++i)
124 if (s.
getGroups()[i].name_ ==
"l_end_effector" || s.
getGroups()[i].name_ ==
"r_end_effector")
129 if (s.
getGroups()[i].name_ ==
"whole_body")
144 const std::vector<double> &v = s.
getGroupStates()[index].joint_values_.find(
"l_shoulder_pan_joint")->second;
147 const std::vector<double> &w = s.
getGroupStates()[1-index].joint_values_.find(
"world_joint")->second;
159 int main(
int argc,
char **argv)
#define TEST_RESOURCE_LOCATION
int main(int argc, char **argv)
Representation of semantic information about the robot.
const std::vector< VirtualJoint > & getVirtualJoints() const
Get the list of virtual joints defined for this model.
bool initFile(const urdf::ModelInterface &urdf_model, const std::string &filename)
Load Model given a filename.
__attribute__((deprecated)) std const std::vector< Group > & getGroups() const
Get the list of groups defined for this model.
const std::vector< DisabledCollision > & getDisabledCollisionPairs() const
Get the list of pairs of links that need not be checked for collisions (because they can never touch ...
def xml_string(rootXml, addHeader=True)
urdf::ModelInterfaceSharedPtr loadURDF(const std::string &filename)
const std::vector< GroupState > & getGroupStates() const
Get the list of group states defined for this model.
const std::vector< EndEffector > & getEndEffectors() const
Get the list of end effectors defined for this model.