diff --git a/urdf/include/urdf/pose.h b/urdf/include/urdf/pose.h index 48e57ab..89e0ec9 100644 --- a/urdf/include/urdf/pose.h +++ b/urdf/include/urdf/pose.h @@ -168,10 +168,20 @@ public: this->y * this->y + this->z * this->z + this->w * this->w); - this->x /= s; - this->y /= s; - this->z /= s; - this->w /= s; + if (s == 0.0) + { + this->x = 0.0; + this->y = 0.0; + this->z = 0.0; + this->w = 1.0; + } + else + { + this->x /= s; + this->y /= s; + this->z /= s; + this->w /= s; + } }; };