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 image PeteBlackerThe3rd  ( 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 image MarijnStam  ( 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 image PeteBlackerThe3rd  ( 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 image MarijnStam  ( 2020-01-13 09:22:36 -0600 )edit

Hi @MarijnStam ,it seems that I have the same issue. How did you finally slove this problem?

Heho gravatar image Heho  ( 2020-10-09 21:09:54 -0600 )edit
1

Hello @Heho

I took the approach which I shortly explained in my previous comment. Note that these solutions were somewhat desperate as I had a few days to finish my school project deadline at that time so I cannot surely say that these approaches are good or even correct. But I did get the arm moving in the end.

You can find the sourcecode for the working version here: https://github.com/MarijnStam/arm_con... Other notable files for the MoveIt configuration can also be found in this repository.

I hope this helps!

MarijnStam gravatar image MarijnStam  ( 2020-10-10 03:36:20 -0600 )edit

It works! Thank a lot!

Heho gravatar image Heho  ( 2020-10-10 23:18:46 -0600 )edit