mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
GeneratePlacePose: add property 'allow_z_flip'
This commit is contained in:
parent
9630f4d789
commit
184fab8e0a
@ -55,6 +55,7 @@ GeneratePlacePose::GeneratePlacePose(const std::string& name) : GeneratePose(nam
|
||||
auto& p = properties();
|
||||
p.declare<std::string>("object");
|
||||
p.declare<geometry_msgs::PoseStamped>("ik_frame");
|
||||
p.declare<bool>("allow_z_flip", false, "allow placing objects upside down");
|
||||
}
|
||||
|
||||
void GeneratePlacePose::onNewSolution(const SolutionBase& s) {
|
||||
@ -110,7 +111,7 @@ void GeneratePlacePose::compute() {
|
||||
// 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) {
|
||||
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) {
|
||||
@ -138,20 +139,21 @@ void GeneratePlacePose::compute() {
|
||||
}
|
||||
};
|
||||
|
||||
uint z_flips = props.get<bool>("allow_z_flip") ? 1 : 0;
|
||||
if (object->getShapes().size() == 1) {
|
||||
switch (object->getShapes()[0]->type) {
|
||||
case shapes::CYLINDER:
|
||||
spawner(target_pose, 2);
|
||||
spawner(target_pose, z_flips);
|
||||
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);
|
||||
spawner(target_pose, z_flips, (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);
|
||||
spawner(target_pose, z_flips);
|
||||
return;
|
||||
default:
|
||||
break;
|
||||
|
||||
Loading…
Reference in New Issue
Block a user