Planning in RViz works but setPoseTarget() fails.

asked 2020-01-11 11:03:45 -0600

MarijnStam gravatar image

updated 2020-01-12 06:49:31 -0600

Hello,

I'm working on making the PhantomX Reactor Arm work with MoveIt (Ubuntu 16.04. ROS Kinetic). I used a pre-made URDF model and used the setup assistant to make a MoveIt config package. Since the PhantomX arm works with the Arbotix-M microcontroller, I use the Arbotix-ROS package to initialize the follow_joint_trajectory topic which MoveIt can talk to. This setup works perfectly in RViz. I can plan to any random valid point and it the arm moves there smoothly. The robot model in RViz also actively responds when I manually move the arm.

However when I try to to move to verified valid coordinates by using the C++ interface, the planner fails.

EDIT

I have found something interesting just now when running rostopic echo on /move_group/goal When I set a planning goal from the RViz interface, I get the following ros message on the topic:

goal: 
  request: 
    workspace_parameters: 
      header: 
        seq: 0
        stamp: 
          secs: 1578832766
          nsecs: 260008148
        frame_id: "/base_footprint"
      min_corner: 
        x: -1.0
        y: -1.0
        z: -1.0
      max_corner: 
        x: 1.0
        y: 1.0
        z: 1.0
    start_state: 
      joint_state: 
        header: 
          seq: 0
          stamp: 
            secs: 0
            nsecs:         0
          frame_id: ''
        name: []
        position: []
        velocity: []
        effort: []
      multi_dof_joint_state: 
        header: 
          seq: 0
          stamp: 
            secs: 0
            nsecs:         0
          frame_id: ''
        joint_names: []
        transforms: []
        twist: []
        wrench: []
      attached_collision_objects: []
      is_diff: True
    goal_constraints: 
      - 
        name: ''
        joint_constraints: 
          - 
            joint_name: "shoulder_yaw_joint"
            position: -2.07496997299
            tolerance_above: 0.0001
            tolerance_below: 0.0001
            weight: 1.0
          - 
            joint_name: "shoulder_pitch_joint"
            position: -0.924272207835
            tolerance_above: 0.0001
            tolerance_below: 0.0001
            weight: 1.0
          - 
            joint_name: "elbow_pitch_joint"
            position: 1.32824846404
            tolerance_above: 0.0001
            tolerance_below: 0.0001
            weight: 1.0
          - 
            joint_name: "wrist_pitch_joint"
            position: 1.75904480582
            tolerance_above: 0.0001
            tolerance_below: 0.0001
            weight: 1.0
          - 
            joint_name: "wrist_roll_joint"
            position: -2.9276301813
            tolerance_above: 0.0001
            tolerance_below: 0.0001
            weight: 1.0
        position_constraints: []
        orientation_constraints: []
        visibility_constraints: []
    path_constraints: 
      name: ''
      joint_constraints: []
      position_constraints: []
      orientation_constraints: []
      visibility_constraints: []
    trajectory_constraints: 
      constraints: []
    planner_id: "RRTConnect"
    group_name: "arm"

When I call a plan from the C++ interface, I receive the following message:

goal: 
  request: 
    workspace_parameters: 
      header: 
        seq: 0
        stamp: 
          secs: 0
          nsecs:         0
        frame_id: ''
      min_corner: 
        x: 0.0
        y: 0.0
        z: 0.0
      max_corner: 
        x: 0.0
        y: 0.0
        z: 0.0
    start_state: 
      joint_state: 
        header: 
          seq: 0
          stamp: 
            secs: 0
            nsecs:         0
          frame_id: ''
        name: []
        position: []
        velocity: []
        effort: []
      multi_dof_joint_state: 
        header: 
          seq: 0
          stamp: 
            secs: 0
            nsecs:         0
          frame_id: ''
        joint_names: []
        transforms: []
        twist: []
        wrench: []
      attached_collision_objects: []
      is_diff: True
    goal_constraints: 
      - 
        name: ''
        joint_constraints: []
        position_constraints: 
          - 
            header: 
              seq: 0
              stamp: 
                secs: 0
                nsecs:         0
              frame_id: "/base_footprint"
            link_name: "gripper_guide_link"
            target_point_offset: 
              x: 0.0
              y: 0.0
              z: 0.0
            constraint_region: 
              primitives: 
                - 
                  type: 2
                  dimensions: [0.0001]
              primitive_poses: 
                - 
                  position: 
                    x: -0.0237
                    y: 0.0897
                    z: 0.3766
                  orientation: 
                    x: 0.0
                    y: 0.0
                    z: 0.0
                    w: 1.0
              meshes: []
              mesh_poses: []
            weight: 1.0
        orientation_constraints: 
          - 
            header: 
              seq: 0
              stamp: 
                secs: 0
                nsecs:         0
              frame_id: "/base_footprint"
            orientation: 
              x: 0.0
              y: 0.0
              z: 0.0
              w: 1.0
            link_name: "gripper_guide_link"
            absolute_x_axis_tolerance: 0.001
            absolute_y_axis_tolerance: 0.001
            absolute_z_axis_tolerance: 0.001
            weight: 1.0
        visibility_constraints: []
    path_constraints: 
      name: ''
      joint_constraints: []
      position_constraints: []
      orientation_constraints: []
      visibility_constraints: []
    trajectory_constraints: 
      constraints: []
    planner_id: ''
    group_name: "arm"

It seems to be empty at most places but I ... (more)

edit retag flag offensive close merge delete

Comments

1

There are a few things I can spot that might help. Firstly the second message from the C++ API doesn't include a planner_id. Also the goal from the RVIZ interface is a joint goal, where the goal is specified in explicit joint angles, whereas the C++ goal is a position goal, where the goal is defined by the pose of the gripper_guide_link link. So the two goals are actually very different, even though they may result in the arm being in the same place at the end!

PeteBlackerThe3rd gravatar imagePeteBlackerThe3rd ( 2020-01-13 04:44:22 -0600 )edit

Thank you for your answer. I was able to solve the planner_id problem by explicitly setting this in the C++ code. As far as the other observation, I just noticed that as well. This makes me think that the IK solver is not able to find a solution for the problem it is given. I suppose this could be because the arm is 5-DOF (tried all the kinematics configs, someone is working on making a FastIK plugin now.). However I've also noted that maybe the transforms are wrong, the gripper guide link which I use as my EEF link seems to be static when I inspect it in the /tf topic. It does not change whenever I move the robot. I used the valid URDF model which has worked with MoveIt through another interface (USB2Dynamixel). When I call setToPose() with another link as EEF though (with a verified well ...(more)

MarijnStam gravatar imageMarijnStam ( 2020-01-13 05:19:05 -0600 )edit
1

It sounds like we're making some progress. Have you tried using the kinematic solver directly from the C++ API to verify that a solution can be found before passing the request to the planner? If you're working with a non-holonomic 5DOF arm then it may well be very tricky setting position goals that the planner will deem possible.

PeteBlackerThe3rd gravatar imagePeteBlackerThe3rd ( 2020-01-13 07:15:38 -0600 )edit

This seems to somewhat work! I now call the IK with setFromIK on the model and the set the returned joint targets via setJointValueTargets(). There are still some problems that arise though, the positions and orientations that I get from RViz don't seem to correspond with where the robot moves. I wanted to define a home position to move to, so I manually set the robot to this, and read out the coordinates from the RobotModel in RViz. I set these coordinates in the C++ file and run the code. IK solves it well and plans it too, but moves to a different position (elbow doesn't move, wrist pitch moves wrongly). I retried this by trying to set the orientation as well as found in RViz, but checking this time, the position and orientation we're completely different. This happened several times, even though RViz sees the arm ...(more)

MarijnStam gravatar imageMarijnStam ( 2020-01-13 09:22:36 -0600 )edit