Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add Vector4 and initQuaternions to be able to parse quaternions from URDF files #51

Closed
wants to merge 2 commits into from
Closed

Add Vector4 and initQuaternions to be able to parse quaternions from URDF files #51

Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
f6cc41a
Add Vector4 and initQuaternions to be able to parse quaternions from …
VictorLamoine Feb 2, 2019
c6b7e08
Rename to initQuaternion (singular)
VictorLamoine Feb 25, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
49 changes: 49 additions & 0 deletions urdf_model/include/urdf_model/pose.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -88,6 +88,43 @@ class Vector3
};
};

class Vector4
{
public:
Vector4(double _x,double _y, double _z, double _w) {this->x=_x;this->y=_y;this->z=_z;this->w=_w;};
Vector4() {this->clear();};
double x;
double y;
double z;
double w;

void clear() {this->x=this->y=this->z=this->w=0.0;};
void init(const std::string &vector_str)
{
this->clear();
std::vector<std::string> pieces;
std::vector<double> xyzw;
urdf::split_string( pieces, vector_str, " ");
for (unsigned int i = 0; i < pieces.size(); ++i){
if (pieces[i] != ""){
try {
xyzw.push_back(strToDouble(pieces[i].c_str()));
} catch(std::runtime_error &) {
throw ParseError("Unable to parse component [" + pieces[i] + "] to a double (while parsing a vector value)");
}
}
}

if (xyzw.size() != 4)
throw ParseError("Parser found " + std::to_string(xyzw.size()) + " elements but 4 expected while parsing vector [" + vector_str + "]");

this->x = xyzw[0];
this->y = xyzw[1];
this->z = xyzw[2];
this->w = xyzw[3];
}
};

class Rotation
{
public:
Expand Down Expand Up @@ -164,6 +201,18 @@ class Rotation
setFromRPY(rpy.x, rpy.y, rpy.z);
}

void initQuaternion(const std::string &rotation_str)
{
this->clear();
Vector4 xyzw_quat;
xyzw_quat.init(rotation_str);
this->x = xyzw_quat.x;
this->y = xyzw_quat.y;
this->z = xyzw_quat.z;
this->w = xyzw_quat.w;
this->normalize();
}

void clear() { this->x=this->y=this->z=0.0;this->w=1.0; }

void normalize()
Expand Down