Grasp pose orientation issue - Moveit Task Constructor

[RyanTrudeau]

Hello. I am following along with the pick place demo: https://moveit.picknik.ai/main/doc/tutorials/pick_and_place_with_moveit_task_constructor/pick_and_place_with_moveit_task_constructor.html only i am using my custom robot. I plan to make my robot execute similar steps to that of the panda robot in the mtc pick place demo. Though I am having issues with the ik planning.

In my mtc script, i have the stages from the current robot position, to the gripper closing onto the object. Here I am using a flat object (which would represent a piece of paper in real life). What i get errors, is from the grasp pose ik stage. When i see the failed solutions in the stage in rviz, my end effector group, which is the gripper aligns with the object, but is 180 rotated in the z axis from this image:

So obviously, the ik will fail since that orientation of the gripper is impossible. I have been experimenting with the approach and grasps stage in the script but it doesnt fix the orientation issue of the gripper. What can i fix within the script? Do i have to adjust my urdf?

Here is the mtc script i am using:
#include <rclcpp/rclcpp.hpp>
#include <moveit/planning_scene/planning_scene.hpp>
#include <moveit/planning_scene_interface/planning_scene_interface.hpp>
#include <moveit/task_constructor/task.h>
#include <moveit/task_constructor/solvers.h>
#include <moveit/task_constructor/stages.h>
#if __has_include(<tf2_geometry_msgs/tf2_geometry_msgs.hpp>)
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#else
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#endif
#if __has_include(<tf2_eigen/tf2_eigen.hpp>)
#include <tf2_eigen/tf2_eigen.hpp>
#else
#include <tf2_eigen/tf2_eigen.h>
#endif

static const rclcpp::Logger LOGGER = rclcpp::get_logger("folding_demo");
namespace mtc = moveit::task_constructor;

class MTCTaskNode
{
public:
MTCTaskNode(const rclcpp::NodeOptions& options);

rclcpp::node_interfaces::NodeBaseInterface::SharedPtr getNodeBaseInterface();

void doTask();

void setupPlanningScene();

private:
// Compose an MTC task from a series of stages.
mtc::Task createTask();
mtc::Task task_;
rclcpp::Node::SharedPtr node_;
};

MTCTaskNode::MTCTaskNode(const rclcpp::NodeOptions& options)
: node_{ std::make_sharedrclcpp::Node("folding_node", options) }
{
}

rclcpp::node_interfaces::NodeBaseInterface::SharedPtr MTCTaskNode::getNodeBaseInterface()
{
return node_->get_node_base_interface();
}

void MTCTaskNode::setupPlanningScene()
{
moveit_msgs::msg::CollisionObject object;
object.id = "object";
object.header.frame_id = "world";
object.primitives.resize(1);
object.primitives[0].type = shape_msgs::msg::SolidPrimitive::BOX;
object.primitives[0].dimensions = { 0.095, 0.095, 0.0001 };

geometry_msgs::msg::Pose pose;
pose.position.x = -0.2;
pose.position.y = 0.05;
pose.position.z = 0.2;
pose.orientation.w = 1.0;
object.pose = pose;

moveit::planning_interface::PlanningSceneInterface psi;
psi.applyCollisionObject(object);
}

void MTCTaskNode::doTask()
{
task_ = createTask();

try
{
task_.init();
}
catch (mtc::InitStageException& e)
{
RCLCPP_ERROR_STREAM(LOGGER, e);
return;
}

if (!task_.plan(5))
{
RCLCPP_ERROR_STREAM(LOGGER, "Task planning failed");
return;
}
task_.introspection().publishSolution(*task_.solutions().front());

auto result = task_.execute(*task_.solutions().front());
if (result.val != moveit_msgs::msg::MoveItErrorCodes::SUCCESS)
{
RCLCPP_ERROR_STREAM(LOGGER, "Task execution failed");
return;
}

return;
}

mtc::Task MTCTaskNode::createTask()
{
mtc::Task task;
task.stages()->setName("demo task");
task.loadRobotModel(node_);

const auto& arm_group_name = "arm";
const auto& hand_group_name = "gripper";
const auto& hand_frame = "link6";

// Set task properties
task.setProperty("group", arm_group_name);
task.setProperty("eef", hand_group_name);
task.setProperty("ik_frame", hand_frame);

// Disable warnings for this line, as it's a variable that's set but not used in this example
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-but-set-variable"
mtc::Stage* current_state_ptr = nullptr; // Forward current_state on to grasp pose generator
#pragma GCC diagnostic pop

auto stage_state_current = std::make_uniquemtc::stages::CurrentState("current"); // Current or home frame
current_state_ptr = stage_state_current.get(); // Use a pointer to lend to IK
task.add(std::move(stage_state_current));

auto sampling_planner = std::make_sharedmtc::solvers::PipelinePlanner(node_); // most complex for curves
auto interpolation_planner = std::make_sharedmtc::solvers::JointInterpolationPlanner(); // Use for gripper

auto cartesian_planner = std::make_sharedmtc::solvers::CartesianPath(); // Moves in a straight line
cartesian_planner->setMaxVelocityScalingFactor(1.0);
cartesian_planner->setMaxAccelerationScalingFactor(1.0);
cartesian_planner->setStepSize(.01);

auto stage_open_hand =
std::make_uniquemtc::stages::MoveTo("open hand", interpolation_planner);
stage_open_hand->setGroup(hand_group_name);
stage_open_hand->setGoal("open"); // Make sure there is open position pose in gripper already predefined
task.add(std::move(stage_open_hand));

// will have to integrate object track here at some point
auto stage_pick_paper = std::make_uniquemtc::stages::Connect(
"pick_paper", mtc::stages::Connect::GroupPlannerVector{{arm_group_name, sampling_planner}});
stage_pick_paper->setTimeout(5.0);
stage_pick_paper->properties().configureInitFrom(mtc::Stage::PARENT);
task.add(std::move(stage_pick_paper));

mtc::Stage* attach_object_stage = nullptr; // creates pointer towards stage object

{
auto grasp = std::make_uniquemtc::SerialContainer("pick paper");
task.properties().exposeTo(grasp->properties(), { "eef", "group", "ik_frame" });
grasp->properties().configureInitFrom(mtc::Stage::PARENT,
{ "eef", "group", "ik_frame" });
{
// Approach pose
auto stage = std::make_uniquemtc::stages::MoveRelative("approach paper", cartesian_planner);
stage->properties().set("marker_ns", "approach_object");
stage->properties().set("link", hand_frame);
stage->properties().configureInitFrom(mtc::Stage::PARENT, { "group" });
stage->setMinMaxDistance(0.1, 0.15);

  // Set hand forward direction
  geometry_msgs::msg::Vector3Stamped vec;
  vec.header.frame_id = hand_frame;
  vec.vector.z = 1.0;
  stage->setDirection(vec);
  grasp->insert(std::move(stage));
}

{
  // Sample grasp pose
  auto stage = std::make_unique<mtc::stages::GenerateGraspPose>("generate grasp pose");
  stage->properties().configureInitFrom(mtc::Stage::PARENT);
  stage->properties().set("marker_ns", "grasp_pose");
  stage->setPreGraspPose("open");
  stage->setObject("object");
  stage->setAngleDelta(M_PI/12);
  stage->setMonitoredStage(current_state_ptr);  // Hook into current state

  Eigen::Isometry3d grasp_frame_transform;
  Eigen::Quaterniond q = Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d::UnitX()) *
                 Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d::UnitY()) *
                 Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d::UnitZ());
  grasp_frame_transform.linear() = q.matrix();
  grasp_frame_transform.translation().z() = 0.1;


  //Eigen::Isometry3d grasp_frame_transform = Eigen::Isometry3d::Identity();
  //grasp_frame_transform.translation().z() = 0.1;


  // Compute IK
  auto wrapper =
    std::make_unique<mtc::stages::ComputeIK>("grasp pose IK", std::move(stage));
  wrapper->setMaxIKSolutions(8);
  wrapper->setMinSolutionDistance(1.0);
  wrapper->setIKFrame(grasp_frame_transform, hand_frame);
  wrapper->properties().configureInitFrom(mtc::Stage::PARENT, { "eef", "group" });
  wrapper->properties().configureInitFrom(mtc::Stage::INTERFACE, { "target_pose" });
  grasp->insert(std::move(wrapper));

}

{
  // Enable collision on the object
  auto stage =
  std::make_unique<mtc::stages::ModifyPlanningScene>("allow collision (hand,object)");
  stage->allowCollisions("object",
                    task.getRobotModel()
                        ->getJointModelGroup(hand_group_name)
                        ->getLinkModelNamesWithCollisionGeometry(),
                    true);
  grasp->insert(std::move(stage));
}

{ 
  // Closes hand on object
  auto stage = std::make_unique<mtc::stages::MoveTo>("close hand", interpolation_planner);
  stage->setGroup(hand_group_name);
  stage->setGoal("close");
  grasp->insert(std::move(stage));
}

{
  // Attaches object
  auto stage = std::make_unique<mtc::stages::ModifyPlanningScene>("attach object");
  stage->attachObject("object", hand_frame);
  attach_object_stage = stage.get();
  grasp->insert(std::move(stage));
}

task.add(std::move(grasp));

}
return task;
}

int main(int argc, char** argv)
{
rclcpp::init(argc, argv);

rclcpp::NodeOptions options;
options.automatically_declare_parameters_from_overrides(true);

auto mtc_task_node = std::make_shared(options);
rclcpp::executors::MultiThreadedExecutor executor;

auto spin_thread = std::make_uniquestd::thread(&executor, &mtc_task_node {
executor.add_node(mtc_task_node->getNodeBaseInterface());
executor.spin();
executor.remove_node(mtc_task_node->getNodeBaseInterface());
});

mtc_task_node->setupPlanningScene();
mtc_task_node->doTask();

spin_thread->join();
rclcpp::shutdown();
return 0;
}

And here is the terminal output as well:

If there is another issue you see, please let me know.

[rhaschke]

GenerateGraspPose generates target poses by applying rotations (0° - 360°) about the given rotation axis, starting from the object's original coordinate frame.
The IK solver attempts to match those poses with the given IK frame (a transformation relative to the end-effector link).

Note: If your robot is not at least 6-DoF, MoveIt's IK solvers usually fail.