diff --git a/dart/utils/Parser.cpp b/dart/utils/Parser.cpp index f8b474eb7369e..d3b2db05cccba 100644 --- a/dart/utils/Parser.cpp +++ b/dart/utils/Parser.cpp @@ -83,18 +83,34 @@ std::string toString(char _v) return boost::lexical_cast(_v); } +//============================================================================== std::string toString(const Eigen::Vector2d& _v) { - Eigen::RowVector2d rowVector2d = _v.transpose(); - - return boost::lexical_cast(rowVector2d); + return boost::lexical_cast(_v.transpose()); } +//============================================================================== std::string toString(const Eigen::Vector3d& _v) { - Eigen::RowVector3d rowVector3d = _v.transpose(); + return boost::lexical_cast(_v.transpose()); +} + +//============================================================================== +std::string toString(const Eigen::Vector3i& _v) +{ + return boost::lexical_cast(_v.transpose()); +} - return boost::lexical_cast(rowVector3d); +//============================================================================== +std::string toString(const Eigen::Vector6d& _v) +{ + return boost::lexical_cast(_v.transpose()); +} + +//============================================================================== +std::string toString(const Eigen::VectorXd& _v) +{ + return boost::lexical_cast(_v.transpose()); } std::string toString(const Eigen::Isometry3d& _v) @@ -282,6 +298,40 @@ Eigen::Vector6d toVector6d(const std::string& _str) return ret; } +//============================================================================== +Eigen::VectorXd toVectorXd(const std::string& _str) +{ + std::vector pieces; + std::string trimedStr = boost::trim_copy(_str); + boost::split(pieces, trimedStr, boost::is_any_of(" "), + boost::token_compress_on); + assert(pieces.size() > 0); + + Eigen::VectorXd ret(pieces.size()); + + for (size_t i = 0; i < pieces.size(); ++i) + { + if (pieces[i] != "") + { + try + { + ret(i) = boost::lexical_cast(pieces[i].c_str()); + } + catch(boost::bad_lexical_cast& e) + { + std::cerr << "value [" + << pieces[i] + << "] is not a valid double for Eigen::VectorXd[" + << i + << "]" + << std::endl; + } + } + } + + return ret; +} + Eigen::Isometry3d toIsometry3d(const std::string& _str) { Eigen::Isometry3d T = Eigen::Isometry3d::Identity(); @@ -478,6 +528,18 @@ Eigen::Vector6d getValueVector6d(tinyxml2::XMLElement* _parentElement, const std return toVector6d(str); } +//============================================================================== +Eigen::VectorXd getValueVectorXd(tinyxml2::XMLElement* _parentElement, + const std::string& _name) +{ + assert(_parentElement != NULL); + assert(!_name.empty()); + + std::string str = _parentElement->FirstChildElement(_name.c_str())->GetText(); + + return toVectorXd(str); +} + Eigen::Vector3d getValueVec3(tinyxml2::XMLElement* _parentElement, const std::string& _name) { assert(_parentElement != NULL); diff --git a/dart/utils/Parser.h b/dart/utils/Parser.h index 8ae86dfec82e2..27d0074dc80c0 100644 --- a/dart/utils/Parser.h +++ b/dart/utils/Parser.h @@ -56,6 +56,9 @@ std::string toString(double _v); std::string toString(char _v); std::string toString(const Eigen::Vector2d& _v); std::string toString(const Eigen::Vector3d& _v); +std::string toString(const Eigen::Vector3i& _v); +std::string toString(const Eigen::Vector6d& _v); +std::string toString(const Eigen::VectorXd& _v); std::string toString(const Eigen::Isometry3d& _v); bool toBool (const std::string& _str); @@ -68,6 +71,7 @@ Eigen::Vector2d toVector2d (const std::string& _str); Eigen::Vector3d toVector3d (const std::string& _str); Eigen::Vector3i toVector3i (const std::string& _str); Eigen::Vector6d toVector6d (const std::string& _str); +Eigen::VectorXd toVectorXd (const std::string& _str); // TODO: The definition of _str is not clear for transform (see: #250) Eigen::Isometry3d toIsometry3d(const std::string& _str); Eigen::Isometry3d toIsometry3dWithExtrinsicRotation(const std::string& _str); @@ -83,6 +87,7 @@ Eigen::Vector2d getValueVector2d (tinyxml2::XMLElement* _parentElement, const Eigen::Vector3d getValueVector3d (tinyxml2::XMLElement* _parentElement, const std::string& _name); Eigen::Vector3i getValueVector3i (tinyxml2::XMLElement* _parentElement, const std::string& _name); Eigen::Vector6d getValueVector6d (tinyxml2::XMLElement* _parentElement, const std::string& _name); +Eigen::VectorXd getValueVectorXd (tinyxml2::XMLElement* _parentElement, const std::string& _name); Eigen::Isometry3d getValueIsometry3d(tinyxml2::XMLElement* _parentElement, const std::string& _name); Eigen::Isometry3d getValueIsometry3dWithExtrinsicRotation(tinyxml2::XMLElement* _parentElement, const std::string& _name); diff --git a/unittests/testParser.cpp b/unittests/testParser.cpp index 5c25603492d1a..532c9b0500ca7 100644 --- a/unittests/testParser.cpp +++ b/unittests/testParser.cpp @@ -56,58 +56,45 @@ using namespace utils; //============================================================================== TEST(Parser, DataStructure) { - bool v1 = true; - int v2 = -3; - unsigned int v3 = 1; - float v4 = -3.140f; - double v5 = 1.4576640; - char v6 = 'd'; - Eigen::Vector2d v7 = Eigen::Vector2d::Ones(); - Eigen::Vector3d v8 = Eigen::Vector3d::Ones(); - //Eigen::Vector3d v9 = Eigen::Vector3d::Ones(); - //math::SO3 v10; - Eigen::Isometry3d v11 = Eigen::Isometry3d::Identity(); - - std::string str1 = toString(v1); - std::string str2 = toString(v2); - std::string str3 = toString(v3); - std::string str4 = toString(v4); - std::string str5 = toString(v5); - std::string str6 = toString(v6); - std::string str7 = toString(v7); - std::string str8 = toString(v8); - //std::string str9 = toString(v9); - //std::string str10 = toString(v10); - std::string str11 = toString(v11); - - bool b = toBool(str1); - int i = toInt(str2); - unsigned int ui = toUInt(str3); - float f = toFloat(str4); - double d = toDouble(str5); - char c = toChar(str6); - Eigen::Vector2d vec2 = toVector2d(str7); - Eigen::Vector3d vec3 = toVector3d(str8); - //Eigen::Vector3d valso3 = toVector3d(str9); - //math::SO3 valSO3 = toSO3(str10); - Eigen::Isometry3d valSE3 = toIsometry3d(str11); - - EXPECT_EQ(b, v1); - EXPECT_EQ(i, v2); - EXPECT_EQ(ui, v3); - EXPECT_EQ(f, v4); - EXPECT_EQ(d, v5); - EXPECT_EQ(c, v6); - for (int i = 0; i < 2; i++) - EXPECT_EQ(vec2[i], v7[i]); - EXPECT_EQ(vec3, v8); - //EXPECT_EQ(valso3, v9); - //EXPECT_EQ(valSO3, v10); - for (int i = 0; i < 4; ++i) - { - for (int j = 0; j < 4; ++j) - EXPECT_EQ(valSE3(i,j), v11(i,j)); - } + bool valBool = true; + int valInt = -3; + unsigned int valUInt = 1; + float valFloat = -3.140f; + double valDouble = 1.4576640; + char valChar = 'd'; + Eigen::Vector2d valVector2d = Eigen::Vector2d::Random(); + Eigen::Vector3d valVector3d = Eigen::Vector3d::Random(); + Eigen::Vector3i valVector3i = Eigen::Vector3i::Random(); + Eigen::Vector6d valVector6d = Eigen::Vector6d::Random(); + Eigen::VectorXd valVectorXd = Eigen::VectorXd::Random(10); + Eigen::Isometry3d valIsometry3d = Eigen::Isometry3d::Identity(); + + std::string strBool = toString(valBool); + std::string strInt = toString(valInt); + std::string strUInt = toString(valUInt); + std::string strFloat = toString(valFloat); + std::string strDouble = toString(valDouble); + std::string strChar = toString(valChar); + std::string strVector2d = toString(valVector2d); + std::string strVector3d = toString(valVector3d); + std::string strVector3i = toString(valVector3i); + std::string strVector6d = toString(valVector6d); + std::string strVectorXd = toString(valVectorXd); + std::string strIsometry3d = toString(valIsometry3d); + + EXPECT_EQ(valBool, toBool(strBool)); + EXPECT_EQ(valInt, toInt(strInt)); + EXPECT_EQ(valUInt, toUInt(strUInt)); + EXPECT_EQ(valFloat, toFloat(strFloat)); + EXPECT_EQ(valDouble, toDouble(strDouble)); + EXPECT_EQ(valChar, toChar(strChar)); + EXPECT_TRUE(equals(valVector2d, toVector2d(strVector2d))); + EXPECT_TRUE(equals(valVector3d, toVector3d(strVector3d))); + EXPECT_EQ(valVector3i, toVector3i(strVector3i)); + EXPECT_TRUE(equals(valVector6d, toVector6d(strVector6d))); + EXPECT_TRUE(equals(valVectorXd, toVectorXd(strVectorXd))); + EXPECT_TRUE(equals(valIsometry3d.matrix(), + toIsometry3d(strIsometry3d).matrix())); } //==============================================================================