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
|
||||
= 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);
|
||||
}
|
||||
|
||||
} } }
|
||||
|
||||
Loading…
Reference in New Issue
Block a user