GenerateGraspPose: Expose rotation_axis as property (#535)

This commit is contained in:
Captain Yoshi 2024-03-06 05:02:44 -05:00 committed by GitHub
parent 5720b83dce
commit 911bc67c4d
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
2 changed files with 5 additions and 2 deletions

View File

@ -55,6 +55,7 @@ public:
void setEndEffector(const std::string& eef) { setProperty("eef", eef); }
void setObject(const std::string& object) { setProperty("object", object); }
void setAngleDelta(double delta) { setProperty("angle_delta", delta); }
void setRotationAxis(const Eigen::Vector3d& axis) { setProperty("rotation_axis", axis); }
void setPreGraspPose(const std::string& pregrasp) { properties().set("pregrasp", pregrasp); }
void setPreGraspPose(const moveit_msgs::RobotState& pregrasp) { properties().set("pregrasp", pregrasp); }

View File

@ -54,6 +54,7 @@ GenerateGraspPose::GenerateGraspPose(const std::string& name) : GeneratePose(nam
p.declare<std::string>("eef", "name of end-effector");
p.declare<std::string>("object");
p.declare<double>("angle_delta", 0.1, "angular steps (rad)");
p.declare<Eigen::Vector3d>("rotation_axis", Eigen::Vector3d::UnitZ(), "rotate object pose about given axis");
p.declare<boost::any>("pregrasp", "pregrasp posture");
p.declare<boost::any>("grasp", "grasp posture");
@ -160,11 +161,12 @@ void GenerateGraspPose::compute() {
geometry_msgs::PoseStamped target_pose_msg;
target_pose_msg.header.frame_id = props.get<std::string>("object");
Eigen::Vector3d rotation_axis = props.get<Eigen::Vector3d>("rotation_axis");
double current_angle = 0.0;
while (current_angle < 2. * M_PI && current_angle > -2. * M_PI) {
// rotate object pose about z-axis
Eigen::Isometry3d target_pose(Eigen::AngleAxisd(current_angle, Eigen::Vector3d::UnitZ()));
// rotate object pose about axis
Eigen::Isometry3d target_pose(Eigen::AngleAxisd(current_angle, rotation_axis));
current_angle += props.get<double>("angle_delta");
InterfaceState state(scene);