robossembler/runtime
7
0
Fork 0
Code Issues 4 Pull requests Projects Releases Packages Wiki Activity Actions

improving the code and adding some comments

Browse source
This commit is contained in:
Ilya Uraev 2023-04-22 15:27:51 +03:00
parent 84798d94f8
commit f0f8d6ca5c
1 changed files with 11 additions and 8 deletions
Download patch file Download diff file Expand all files Collapse all files

19
rbs_skill_servers/src/pick_place_pose_loader.cpp
View file

@ -67,26 +67,29 @@ GetGraspPlacePoseServer::collect_pose(
Eigen::Vector3d posedir;
Eigen::Affine3d pose_ = pose;
tf2::fromMsg(direction, posedir);
std::cout << "\nPoseDir: =" << posedir << std::endl;
Eigen::Matrix3d matIx = posedir * posedir.transpose();
Eigen::Matrix3d matIZ = Eigen::Vector3d::UnitZ() * Eigen::Vector3d::UnitZ().transpose();
Eigen::Matrix3d Ixy = Eigen::Matrix3d::Zero();//posedir * posedir.transpose();
Ixy.diagonal() = posedir;
Eigen::Matrix3d Iz = Eigen::Matrix3d::Zero();
Iz.diagonal() = Eigen::Vector3d::UnitZ();
if (posedir.cwiseEqual(Eigen::Vector3d::UnitX()).all()) // IF Direction == [1,0,0]
{
std::cout << "\n TBD" << std::endl;
}
else if (posedir.cwiseEqual(Eigen::Vector3d::UnitY()).all()) // IF Direction == [0,1,0]
{
pose_.pretranslate(-(matIx * scale_vec));
// Gp -- grasp point frame
pose_.pretranslate(-(Ixy * scale_vec)); // Gp-y
pose_v_.push_back(tf2::toMsg(pose_));
pose_.pretranslate(matIZ * scale_vec);
pose_.pretranslate(Iz * scale_vec); // Gp-y + z
pose_v_.push_back(tf2::toMsg(pose_));
pose_.pretranslate(matIx * scale_vec);
pose_.pretranslate(Ixy * scale_vec); // Gp+z
pose_v_.push_back(tf2::toMsg(pose_));
}
else if (posedir.cwiseEqual(Eigen::Vector3d::UnitZ()).all()) // IF Direction == [0,0,1]
{
std::cout << "\npreplace = \n" << pose_.pretranslate(matIx * scale_vec).translation() << std::endl;
pose_v_.push_back(tf2::toMsg(pose_));
pose_.pretranslate(Ixy * scale_vec); // Choose Pre Grasp == Post Grasp
pose_v_.push_back(tf2::toMsg(pose_)); // So calculate only Pre Grasp
}
else
{