Moveit 移动机械臂API 调用

在moveit 移动机械臂时，有很方便的调用API方式

1.直接设定规划组提前设置的目标位姿

if(arm_.setNamedTarget("down")==false)

{
    ROS_ERROR("Set forward failed");
    return false;
}
moveit::planning_interface::MoveItErrorCode result = arm_.asyncMove();
    if (bool(result) == true)
    {
      return true;
    }
    else
    {
      ROS_ERROR("Move to target failed (error %d)", result.val);

return false;

2.手动输入设定目标位姿

geometry_msgs::Pose target_pose1;
   target_pose1.position.x = 0.1;
        target_pose1.position.y = 0.2;
        target_pose1.position.z = -0.05;

geometry_msgs::PoseStamped target_pose;
target_pose.header.frame_id = arm_link;
target_pose.pose = target_pose1;

int attempts = 0;

while (attempts < 5)
{
    geometry_msgs::PoseStamped modiff_target;
    modiff_target.header.frame_id = arm_link;
    modiff_target.pose = target_pose1;
     double x = modiff_target.pose.position.x;
     double y = modiff_target.pose.position.y;
     double z = modiff_target.pose.position.z;
     double d = sqrt(x*x + y*y);
     if (d>0.3)
     {
       ROS_ERROR("Target pose out of reach [%f > %f]", d, 0.3);


      }
      // Pitch is 90 (vertical) at 10 cm from the arm base; the farther the target is, the closer to horizontal
      // we point the gripper (0.205 = arm's max reach - vertical pitch distance + ε). Yaw is the direction to
      // the target. We also try some random variations of both to increase the chances of successful planning.
      // Roll is simply ignored, as our arm lacks the proper dof.
      double rp = M_PI_2 - std::asin((d - 0.1)/0.205) + attempts*fRand(-0.05, +0.05);
      double ry = std::atan2(y, x) + attempts*fRand(-0.05, +0.05);
      double rr = 0.0;

      tf::Quaternion q = tf::createQuaternionFromRPY(rr, rp, ry);
      tf::quaternionTFToMsg(q, modiff_target.pose.orientation);

      // Slightly increase z proportionally to pitch to avoid hitting the table with the lower gripper corner
      ROS_DEBUG("z increase: %f + %f", modiff_target.pose.position.z, std::abs(std::cos(rp))/50.0);
      modiff_target.pose.position.z += std::abs(std::cos(rp))/50.0;

//       ROS_DEBUG("Set pose target [%.2f, %.2f, %.2f] [d: %.2f, r: %.2f, p: %.2f, y: %.2f]", x, y, z, d, rr, rp, ry);

        ROS_INFO("Set pose target [%.2f, %.2f, %.2f] [d: %.2f, r: %.2f, p: %.2f, y: %.2f]", x, y, z, d, rr, rp, ry);
      if (arm_.setPoseTarget(modiff_target) == false)
      {
        ROS_ERROR("Set pose target [%.2f, %.2f, %.2f, %.2f] failed",
                  modiff_target.pose.position.x, modiff_target.pose.position.y, modiff_target.pose.position.z,
                  tf::getYaw(modiff_target.pose.orientation));


      }

      moveit::planning_interface::MoveItErrorCode result = arm_.move();
      if(bool(result) == true){
   return true;
      }
      else
   return false;

   attempts++;
}

这里需要注意，必须正确设定目标的角度（orientation）,否则会报 ABORTED: No motion plan found. No execution attempted.错误

Moveit 移动机械臂API 调用

猜你喜欢