vary place pose depending on object shape

- boxes, cylinders: flip up/down, rotate about world's z
- spheres: rotate about world's z
This commit is contained in:
Robert Haschke 2019-02-22 02:29:35 +01:00
parent 168ff6f3f4
commit 29ecec7403

View File

@ -93,7 +93,7 @@ void GeneratePlacePose::compute() {
const moveit::core::AttachedBody* object
= robot_state.getAttachedBody(props.get<std::string>("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<geometry_msgs::PoseStamped>("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<const shapes::Box&>(*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);
}
} } }