diff --git a/src/Connector.cpp b/src/Connector.cpp index 7bf70bb6bde0e2b02e779208565e9ca3eea1d0d9..0cdd3f00c3d7db408d5e0765310972f72cb43522 100644 --- a/src/Connector.cpp +++ b/src/Connector.cpp @@ -115,11 +115,14 @@ Connector::Connector(const ros::NodeHandle &nodeHandle, const std::string &cellN } bool Connector::pickAndPlace(Object &robot, Object &object, Object &location, bool simulateOnly) { - if (object.orientation().w() == 1 && location.orientation().w() == 1) { - // if w is 1 (and the quaternion is normalized) the object is NOT rotated + if (object.orientation().x() == 0 && object.orientation().y() == 0 && location.orientation().x() == 0 && location.orientation().y() == 0) { + // the objects must not be rotated around z + // TODO improve float comparison with 0 object.mutable_pos()->set_x(location.pos().x()); object.mutable_pos()->set_y(location.pos().y()); object.mutable_pos()->set_z(location.pos().z() - location.size().height() / 2 + object.size().height() / 2); + object.mutable_orientation()->set_z(location.orientation().z()); + object.mutable_orientation()->set_w(location.orientation().w()); return true; } else { return false;