Skip to content
Snippets Groups Projects
Commit d9f1a7c2 authored by Johannes Mey's avatar Johannes Mey
Browse files

also dummy-move scenes with rotated objects

parent 85a55298
Branches
No related tags found
No related merge requests found
...@@ -115,11 +115,14 @@ Connector::Connector(const ros::NodeHandle &nodeHandle, const std::string &cellN ...@@ -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) { bool Connector::pickAndPlace(Object &robot, Object &object, Object &location, bool simulateOnly) {
if (object.orientation().w() == 1 && location.orientation().w() == 1) { if (object.orientation().x() == 0 && object.orientation().y() == 0 && location.orientation().x() == 0 && location.orientation().y() == 0) {
// if w is 1 (and the quaternion is normalized) the object is NOT rotated // 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_x(location.pos().x());
object.mutable_pos()->set_y(location.pos().y()); 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_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; return true;
} else { } else {
return false; return false;
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment