37 #ifndef URDF_INTERFACE_POSE_H 38 #define URDF_INTERFACE_POSE_H 44 #include <KrisLibrary/utils/stringutils.h> 45 #include <KrisLibrary/utils/AnyValue.h> 46 #include "urdf_exception.h" 49 #define M_PI 3.14159265358979323846 57 Vector3(
double _x,
double _y,
double _z) {this->x=_x;this->y=_y;this->z=_z;};
63 void set(
double _x,
double _y,
double _z) {this->x=_x;this->y=_y;this->z=_z;};
64 void set(
const Vector3& vec) {this->x=vec.x;this->y=vec.y;this->z=vec.z;};
65 void clear() {this->x=this->y=this->z=0.0;};
66 void init(
const std::string &vector_str)
69 std::vector<std::string> pieces;
70 std::vector<double> xyz;
71 pieces = Split( vector_str,
" ");
72 for (
unsigned int i = 0; i < pieces.size(); ++i){
75 if(LexicalCast(pieces[i],val))
78 throw ParseError(
"Unable to parse component [" + pieces[i] +
"] to a double (while parsing a vector value)");
83 throw ParseError(
"Parser found " + LexicalCast(xyz.size()) +
" elements but 3 expected while parsing vector [" + vector_str +
"]");
92 return Vector3(this->x+vec.x,this->y+vec.y,this->z+vec.z);
99 Rotation(
double _x,
double _y,
double _z,
double _w) {this->x=_x;this->y=_y;this->z=_z;this->w=_w;};
102 void set(
double _x,
double _y,
double _z,
double _w) {this->x=_x;this->y=_y;this->z=_z;this->w=_w;};
103 void set(
const Rotation& rot) {this->x=rot.x;this->y=rot.y;this->z=rot.z;this->w=rot.w;};
104 void getQuaternion(
double &quat_x,
double &quat_y,
double &quat_z,
double &quat_w)
const 111 void getRPY(
double &roll,
double &pitch,
double &yaw)
const 118 sqx = this->x * this->x;
119 sqy = this->y * this->y;
120 sqz = this->z * this->z;
121 sqw = this->w * this->w;
123 roll = atan2(2 * (this->y*this->z + this->w*this->x), sqw - sqx - sqy + sqz);
124 double sarg = -2 * (this->x*this->z - this->w*this->y);
125 pitch = sarg <= -1.0 ? -0.5*M_PI : (sarg >= 1.0 ? 0.5*M_PI : asin(sarg));
126 yaw = atan2(2 * (this->x*this->y + this->w*this->z), sqw + sqx - sqy - sqz);
129 void setFromQuaternion(
double quat_x,
double quat_y,
double quat_z,
double quat_w)
137 void setFromRPY(
double roll,
double pitch,
double yaw)
139 double phi, the, psi;
145 this->x = sin(phi) * cos(the) * cos(psi) - cos(phi) * sin(the) * sin(psi);
146 this->y = cos(phi) * sin(the) * cos(psi) + sin(phi) * cos(the) * sin(psi);
147 this->z = cos(phi) * cos(the) * sin(psi) - sin(phi) * sin(the) * cos(psi);
148 this->w = cos(phi) * cos(the) * cos(psi) + sin(phi) * sin(the) * sin(psi);
155 void init(
const std::string &rotation_str)
159 rpy.init(rotation_str);
160 setFromRPY(rpy.x, rpy.y, rpy.z);
163 void clear() { this->x=this->y=this->z=0.0;this->w=1.0; }
167 double s = sqrt(this->x * this->x +
192 c.x = this->w * qt.x + this->x * qt.w + this->y * qt.z - this->z * qt.y;
193 c.y = this->w * qt.y - this->x * qt.z + this->y * qt.w + this->z * qt.x;
194 c.z = this->w * qt.z + this->x * qt.y - this->y * qt.x + this->z * qt.w;
195 c.w = this->w * qt.w - this->x * qt.x - this->y * qt.y - this->z * qt.z;
210 tmp = (*this) * (tmp * this->GetInverse());
223 double norm = this->w*this->w+this->x*this->x+this->y*this->y+this->z*this->z;
227 q.w = this->w / norm;
228 q.x = -this->x / norm;
229 q.y = -this->y / norm;
230 q.z = -this->z / norm;
242 Pose() { this->clear(); };
249 this->position.clear();
250 this->rotation.clear();
Definition: urdf_exception.h:11
Definition: urdf_pose.h:96
Definition: urdf_color.h:46
Vector3 operator*(Vector3 vec) const
Rotate a vector using the quaternion.
Definition: urdf_pose.h:200
Definition: urdf_pose.h:239
Definition: urdf_pose.h:54