diff --git a/core/src/stages/generate_place_pose.cpp b/core/src/stages/generate_place_pose.cpp index 23c46d74..39683eb0 100644 --- a/core/src/stages/generate_place_pose.cpp +++ b/core/src/stages/generate_place_pose.cpp @@ -93,7 +93,7 @@ void GeneratePlacePose::compute() { const moveit::core::AttachedBody* object = robot_state.getAttachedBody(props.get("object")); // current object_pose w.r.t. planning frame - const Eigen::Isometry3d& object_pose = object->getGlobalCollisionBodyTransforms()[0]; + const Eigen::Isometry3d& orig_object_pose = object->getGlobalCollisionBodyTransforms()[0]; const geometry_msgs::PoseStamped& pose_msg = props.get("pose"); Eigen::Isometry3d target_pose; @@ -105,21 +105,59 @@ void GeneratePlacePose::compute() { Eigen::Isometry3d ik_frame; tf::poseMsgToEigen(ik_frame_msg.pose, ik_frame); ik_frame = robot_state.getGlobalLinkTransform(ik_frame_msg.header.frame_id) * ik_frame; + Eigen::Isometry3d object_to_ik = orig_object_pose.inverse() * ik_frame; - // target ik_frame's pose w.r.t. planning frame - geometry_msgs::PoseStamped target_pose_msg; - target_pose_msg.header.frame_id = scene->getPlanningFrame(); - tf::poseEigenToMsg(target_pose * object_pose.inverse() * ik_frame, target_pose_msg.pose); + // spawn the nominal target object pose, considering flip about z and rotations about z-axis + auto spawner = [&s, &scene, &object_to_ik, this] + (const Eigen::Isometry3d& nominal, uint z_flips, uint z_rotations = 10) { + for (uint flip = 0; flip < z_flips; ++flip) { + // flip about object's x-axis + Eigen::Isometry3d object = nominal * Eigen::AngleAxisd(flip * M_PI, Eigen::Vector3d::UnitX()); + for (uint i = 0; i < z_rotations; ++i) { + // rotate object at target pose about world's z-axis + Eigen::Vector3d pos = object.translation(); + object.pretranslate(-pos) + .prerotate(Eigen::AngleAxisd(i * 2.*M_PI / z_rotations, Eigen::Vector3d::UnitZ())) + .pretranslate(pos); - InterfaceState state(scene); - forwardProperties(*s.end(), state); // forward properties from inner solutions - state.properties().set("target_pose", target_pose_msg); + // target ik_frame's pose w.r.t. planning frame + geometry_msgs::PoseStamped target_pose_msg; + target_pose_msg.header.frame_id = scene->getPlanningFrame(); + tf::poseEigenToMsg(object * object_to_ik, target_pose_msg.pose); - SubTrajectory trajectory; - trajectory.setCost(0.0); - rviz_marker_tools::appendFrame(trajectory.markers(), target_pose_msg, 0.1, "place frame"); + InterfaceState state(scene); + forwardProperties(*s.end(), state); // forward properties from inner solutions + state.properties().set("target_pose", target_pose_msg); - spawn(std::move(state), std::move(trajectory)); + SubTrajectory trajectory; + trajectory.setCost(0.0); + rviz_marker_tools::appendFrame(trajectory.markers(), target_pose_msg, 0.1, "place frame"); + + spawn(std::move(state), std::move(trajectory)); + } + } + }; + + if (object->getShapes().size() == 1) { + switch (object->getShapes()[0]->type) { + case shapes::CYLINDER: + spawner(target_pose, 2); + return; + + case shapes::BOX: { // consider 180/90 degree rotations about z axis + const double *dims = static_cast(*object->getShapes()[0]).size; + spawner(target_pose, 2, (std::abs(dims[0] - dims[1]) < 1e-5) ? 4 : 2); + return; + } + case shapes::SPHERE: // keep original orientation and rotate about world's z + target_pose.linear() = orig_object_pose.linear(); + spawner(target_pose, 1); + return; + } + } + + // any other case: only try given target pose + spawner(target_pose, 1, 1); } } } }