GenerateGraspPose: Expose rotation_axis as property (moveit#535)

ubi-agni · Mar 6, 2024 · 911bc67 · 911bc67
1 parent 5720b83
commit 911bc67
Show file tree

Hide file tree

Showing 2 changed files with 5 additions and 2 deletions.
diff --git a/core/include/moveit/task_constructor/stages/generate_grasp_pose.h b/core/include/moveit/task_constructor/stages/generate_grasp_pose.h
@@ -55,6 +55,7 @@ class GenerateGraspPose : public GeneratePose
 	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); }

diff --git a/core/src/stages/generate_grasp_pose.cpp b/core/src/stages/generate_grasp_pose.cpp
@@ -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);