mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
GenerateGraspPose: Expose rotation_axis as property (#535)
This commit is contained in:
parent
5720b83dce
commit
911bc67c4d
@ -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); }
|
||||
|
||||
@ -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);
|
||||
|
||||
Loading…
Reference in New Issue
Block a user