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();
|
auto& p = properties();
|
||||||
p.declare<std::string>("object");
|
p.declare<std::string>("object");
|
||||||
p.declare<geometry_msgs::PoseStamped>("ik_frame");
|
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) {
|
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
|
// 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,
|
auto spawner = [&s, &scene, &object_to_ik, this](const Eigen::Isometry3d& nominal, uint z_flips,
|
||||||
uint z_rotations = 10) {
|
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
|
// flip about object's x-axis
|
||||||
Eigen::Isometry3d object = nominal * Eigen::AngleAxisd(flip * M_PI, Eigen::Vector3d::UnitX());
|
Eigen::Isometry3d object = nominal * Eigen::AngleAxisd(flip * M_PI, Eigen::Vector3d::UnitX());
|
||||||
for (uint i = 0; i < z_rotations; ++i) {
|
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) {
|
if (object->getShapes().size() == 1) {
|
||||||
switch (object->getShapes()[0]->type) {
|
switch (object->getShapes()[0]->type) {
|
||||||
case shapes::CYLINDER:
|
case shapes::CYLINDER:
|
||||||
spawner(target_pose, 2);
|
spawner(target_pose, z_flips);
|
||||||
return;
|
return;
|
||||||
|
|
||||||
case shapes::BOX: { // consider 180/90 degree rotations about z axis
|
case shapes::BOX: { // consider 180/90 degree rotations about z axis
|
||||||
const double* dims = static_cast<const shapes::Box&>(*object->getShapes()[0]).size;
|
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;
|
return;
|
||||||
}
|
}
|
||||||
case shapes::SPHERE: // keep original orientation and rotate about world's z
|
case shapes::SPHERE: // keep original orientation and rotate about world's z
|
||||||
target_pose.linear() = orig_object_pose.linear();
|
target_pose.linear() = orig_object_pose.linear();
|
||||||
spawner(target_pose, 1);
|
spawner(target_pose, z_flips);
|
||||||
return;
|
return;
|
||||||
default:
|
default:
|
||||||
break;
|
break;
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user