moveit setJointValueTarget() cannot execute

asked 2020-10-14 22:22:01 -0500

sueee gravatar image

updated 2020-10-15 11:17:46 -0500

Hi, I am using baxter and moveit to control the arm. The problem I met is that I used the moveit motion planning gui in rviz to set a goal pose and execute the plan. Then I echoed the topic /move_group/result to get joint values when reaching the goal. Below is the output:

joint_name: "left_s0"
        position: -0.662817497527
        tolerance_above: 0.0001
        tolerance_below: 0.0001
        weight: 1.0
      - 
        joint_name: "left_s1"
        position: 0.269378851376
        tolerance_above: 0.0001
        tolerance_below: 0.0001
        weight: 1.0
      - 
        joint_name: "left_e0"
        position: -2.87981641053
        tolerance_above: 0.0001
        tolerance_below: 0.0001
        weight: 1.0
      - 
        joint_name: "left_e1"
        position: 0.272509336485
        tolerance_above: 0.0001
        tolerance_below: 0.0001
        weight: 1.0
      - 
        joint_name: "left_w0"
        position: 2.89781588494
        tolerance_above: 0.0001
        tolerance_below: 0.0001
        weight: 1.0
      - 
        joint_name: "left_w1"
        position: 1.55065213543
        tolerance_above: 0.0001
        tolerance_below: 0.0001
        weight: 1.0
      - 
        joint_name: "left_w2"
        position: 0.175511624637
        tolerance_above: 0.0001
        tolerance_below: 0.0001
        weight: 1.0

And I tried to use move_group to control the arm in my node, here is the code:

const robot_state::JointModelGroup* joint_model_group =
      move_group->getCurrentState()->getJointModelGroup(PLANNING_GROUP);
  move_group->setPlannerId("RRTConnectkConfigDefault");
       moveit::core::RobotStatePtr current_state = move_group->getCurrentState();
      // Next get the current set of joint values for the group.
      std::vector<double> joint_group_positions;
      current_state->copyJointGroupPositions(joint_model_group, joint_group_positions);
      ROS_INFO_NAMED("arm_moveit", "Joint Group Positions:");
      std::copy(joint_group_positions.begin(), joint_group_positions.end(), std::ostream_iterator<double>(std::cout, ", "));
      // Now, let's modify one of the joints, plan to the new joint space goal and visualize the plan.
      std::map<std::string, double> joints;
      ROS_INFO_NAMED("arm_moveit", "Available Joint Names:");
      std::copy(move_group->getJointNames().begin(), move_group->getJointNames().end(),
                std::ostream_iterator<std::string>(std::cout, ", "));
      // left_s0, left_s1, left_e0, left_e1, left_w0, left_w1, left_w2,
      joints["left_s0"] = -0.662817497527;  // radians
      joints["left_s1"] = 0.269378851376;
      joints["left_e0"] = -2.87981641053;
      joints["left_e1"] = 0.272509336485;
      joints["left_w0"] = 2.89781588494;
      joints["left_w1"] = 1.55065213543;
      joints["left_w2"] = 0.175511624637;
      move_group->setJointValueTarget(joints);
      // Now, we call the planner to compute the plan
      moveit::planning_interface::MoveGroupInterface::Plan my_plan;
      bool success = (move_group->plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
      if (success)
      {
        // move_group->execute(my_plan);
        ROS_WARN("Visualizing plan 1 (pose goal) SUCCESS");
        move_group->move();
      }
      else
      {
        ROS_WARN("Visualizing plan 1 (pose goal) FAILED");
      }

But i got the output:

[ERROR] [1602731257.704260, 30.593000]: /joint_trajectory_action_server: Exceeded Error Threshold on left_s1: -0.351796114896
[ WARN] [1602731257.705894806, 30.594000000]: Controller /robot/limb/left failed with error PATH_TOLERANCE_VIOLATED: 
[ WARN] [1602731257.705983104, 30.594000000]: Controller handle /robot/limb/left reports status ABORTED
[ INFO] [1602731257.706036219, 30.594000000]: Completed trajectory execution with status ABORTED ...
left_s0, left_s1, left_e0, left_e1, left_w0, left_w1, left_w2, [ INFO] [1602731257.717011699, 30.601000000]: ABORTED: Solution found but controller failed during execution

Anyone could help with this? Is there any other way to do this except for increasing the tolerance to 0.1?

Should I set the whole generated trajectory in the node? Or how can I get a pose goal so I can use move_group->setPoseTarget()?

Thanks!

edit retag flag offensive close merge delete