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.错误

评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值