mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
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:
parent
168ff6f3f4
commit
29ecec7403
@ -93,7 +93,7 @@ void GeneratePlacePose::compute() {
|
|||||||
const moveit::core::AttachedBody* object
|
const moveit::core::AttachedBody* object
|
||||||
= robot_state.getAttachedBody(props.get<std::string>("object"));
|
= robot_state.getAttachedBody(props.get<std::string>("object"));
|
||||||
// current object_pose w.r.t. planning frame
|
// 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");
|
const geometry_msgs::PoseStamped& pose_msg = props.get<geometry_msgs::PoseStamped>("pose");
|
||||||
Eigen::Isometry3d target_pose;
|
Eigen::Isometry3d target_pose;
|
||||||
@ -105,11 +105,25 @@ void GeneratePlacePose::compute() {
|
|||||||
Eigen::Isometry3d ik_frame;
|
Eigen::Isometry3d ik_frame;
|
||||||
tf::poseMsgToEigen(ik_frame_msg.pose, ik_frame);
|
tf::poseMsgToEigen(ik_frame_msg.pose, ik_frame);
|
||||||
ik_frame = robot_state.getGlobalLinkTransform(ik_frame_msg.header.frame_id) * 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;
|
||||||
|
|
||||||
|
// 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);
|
||||||
|
|
||||||
// target ik_frame's pose w.r.t. planning frame
|
// target ik_frame's pose w.r.t. planning frame
|
||||||
geometry_msgs::PoseStamped target_pose_msg;
|
geometry_msgs::PoseStamped target_pose_msg;
|
||||||
target_pose_msg.header.frame_id = scene->getPlanningFrame();
|
target_pose_msg.header.frame_id = scene->getPlanningFrame();
|
||||||
tf::poseEigenToMsg(target_pose * object_pose.inverse() * ik_frame, target_pose_msg.pose);
|
tf::poseEigenToMsg(object * object_to_ik, target_pose_msg.pose);
|
||||||
|
|
||||||
InterfaceState state(scene);
|
InterfaceState state(scene);
|
||||||
forwardProperties(*s.end(), state); // forward properties from inner solutions
|
forwardProperties(*s.end(), state); // forward properties from inner solutions
|
||||||
@ -121,5 +135,29 @@ void GeneratePlacePose::compute() {
|
|||||||
|
|
||||||
spawn(std::move(state), std::move(trajectory));
|
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);
|
||||||
|
}
|
||||||
|
|
||||||
} } }
|
} } }
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user