Reach study shows incorrect IK solutions

[vicltz]

I am trying to do a reach study of the TIAGo robot, you can find the URDF here.. It uses a gripper as end effector , you can find the urdf of the gripper here.

However I struggle to make it work, and I am not sure if it is because of misconfiguration or some bug with the package.

At the end of the analysis, some interactive markers are broken and lead to impossible pose of the robot.

These are the groups defined in the srdf file of the robot:

<group name="arm">
        <joint name="arm_1_joint" />
        <joint name="arm_2_joint" />
        <joint name="arm_3_joint" />
        <joint name="arm_4_joint" />
        <joint name="arm_5_joint" />
        <joint name="arm_6_joint" />
        <joint name="arm_7_joint" />
        <joint name="arm_tool_joint" />
    </group>

    <group name="arm_torso">
        <joint name="torso_lift_joint" />
        <joint name="arm_1_joint" />
        <joint name="arm_2_joint" />
        <joint name="arm_3_joint" />
        <joint name="arm_4_joint" />
        <joint name="arm_5_joint" />
        <joint name="arm_6_joint" />
        <joint name="arm_7_joint" />
        <joint name="arm_tool_joint" />
    </group>

    <group name="gripper">
        <link name="gripper_left_finger_link" />
        <link name="gripper_right_finger_link" />
        <link name="gripper_link" />
        <joint name="gripper_left_finger_joint" />
        <joint name="gripper_right_finger_joint" />
    </group>

    <group name="manipulator">
        <joint name="torso_lift_joint" />
        <joint name="arm_1_joint" />
        <joint name="arm_2_joint" />
        <joint name="arm_3_joint" />
        <joint name="arm_4_joint" />
        <joint name="arm_5_joint" />
        <joint name="arm_6_joint" />
        <joint name="arm_7_joint" />
        <joint name="arm_tool_joint" />
        <link name="tcp" />
    </group>

I added a TCP link to my end effector URDF (gripper urdf) like in the demo:

  <link name="tcp"/>
  <joint name="gripper_base_link_to_tcp" type="fixed">
    <parent link="gripper_base_link"/>
    <child link="tcp"/>
    <origin xyz="0 0 0.26" rpy="0 0 0"/>
  </joint>

Most of the interactive makers work, but a small number of them are just acting weird:

example of working marker :

example of broken marker:

This is the pose of the robot I get when clicking on the green marker on the extreme right.

All the markers at the extreme left (relatively to the robot orientation) seem broken:

Here is my yaml config file:

optimization:
  radius: 0.2
  max_steps: 15
  step_improvement_threshold: 0.01

ik_solver:
  name: MoveItIKSolver
  distance_threshold: 0.0
  planning_group: manipulator
  collision_mesh_filename: package://reach_ros/demo/config/part.ply
  touch_links: []

evaluator:
  name: MultiplicativeEvaluator
  plugins:
    - name: ManipulabilityMoveIt
      planning_group: manipulator
    - name: DistancePenaltyMoveIt
      planning_group: manipulator
      distance_threshold: 0.0
      exponent: 2
      collision_mesh_filename: package://reach_ros/demo/config/part.ply
      touch_links: []

display:
  name: ROSDisplay
  collision_mesh_filename: package://reach_ros/demo/config/part.ply
  kinematic_base_frame: base_footprint
  marker_scale: 0.05

target_pose_generator:
  name: PointCloudTargetPoseGenerator
  pcd_file: package://reach_ros/demo/config/part.pcd
  points_frame: base_footprint
  target_frame: base_footprint

logger:
  name: BoostProgressConsoleLogger

Here is the kinematics config file:

arm_torso:
  kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
  kinematics_solver_search_resolution: 0.005
  kinematics_solver_timeout: 0.005
  kinematics_solver_attempts: 3
arm:
  kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
  kinematics_solver_search_resolution: 0.005
  kinematics_solver_timeout: 0.005
  kinematics_solver_attempts: 3
manipulator:
  kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
  kinematics_solver_search_resolution: 0.005
  kinematics_solver_timeout: 0.005
  kinematics_solver_attempts: 3

One other thing to notice is that the results are highly asymmetrical while the scene and the robot are symmetrical.

The only asymmetrical thing I know is that:

This is the pose of the robot with zero angle on each joint:

But when the robot is launched into RVIZ the pose of the robot is like this (arm in front of him)

I am not sure why this happens and if this is normal ?

Thank you for your help

[vicltz]

I also get really different results each time , even with the same parameters. If it helps to figure out the problem ?

Thank you for your help

[marip8]

I’ll take a look at this soon. A few things you could try in the interim are:

• Change the motion groups in the SRDF from joint groups to chains. I’m not sure how MoveIt/KDL handle joint groups; I’ve only used chains in the past
• Add the max_threads: 1 to the optimization section of your reach study config file. There may be an issue with multi threading that’s causing this

To help address the asymmetry you’re seeing, set the seed_state parameter to a desired IK seed position for your robot

[vicltz]

Thank you for the tips !
I observed no changes after replacing the joint group by a chain group but at least it is a good practice.
Disabling multi threading doesn't seem to fix the solution neither.

I am trying different seed_state but it's really hard to see the effect since the results are really unstable and can give very different reachability with the same parameters and setup (from 0.5% points reached to up to 50% on the same config !).

However, I think that there is a problem with IK before the optimization step. When I get some really low % of reachable points (It should be over 50% on my workpiece) , the results are bad already after IK and before optimization.

[marip8]

I recreated your demo here so I could play around with it. I think the primary issue is with the definition of the motion groups. The reach study assumes that the target poses are provided to the IK solver relative to that solver's kinematic base frame. When you specify a group of joints as a motion group in the SRDF, you can't explicitly define the desired kinematic base frame. There is an implicit configuration "connection" between the target pose generator, IK solver, and display plugins to make it function correctly, which is that they all need to have the same reference frame. I think in your case where you defined a group of joints as your motion group, the kinematic base link of that motion group was not the same frame as was specified to the display plugin, hence the robot looking like it moved to randomly incorrect positions.

Changing the motion group to a chain with its root at base_footprint seems to solve the issue. With my test repo, I was able to generate the results below consistently with correct visualization. Please try it out and let me know if this corrects your issue.

I made a few other changes in that repo as well that differs from your original setup:

• Changed the origin of part.ply and part.pcd to be a more realistic origin for the part
• Added a reach_object link to more easily test out different part locations
  • I got a lot better reach results with the part ~1m away from the robot, which might be further away than your tests
• Updated the target pose generator to the TransformedPointCloudTargetPoseGenerator plugin which can take reach targets relative to a given frame (i.e., reach_object) and transform them into the common kinematic base frame (i.e., base_footprint)
  • This is different than the PointCloudTargetPoseGenerator from the reach repository; that plugin simply provides the point cloud poses, assuming that they are already in the kinematic base frame of the robot
• Updated the other plugin configurations to set the collision_mesh_frame equal to reach_object

[vicltz]

I confirm that your setup gives stable, reproducible results with the gripper. And there is no more broken markers. That's the first time I am able to get this with the gripper. Thank you for this !

With your setup and a script that compile the results from many result files into one final (and remove the unreachable markers) , I was able to get this type of results:

Which is really nice and what I wanted.

However, I was only able to get this result with your "lightweight" setup. My idea was to use the full move it config for TIAGo . This config use xacro shortcuts and has it specific launch file that launches all the planning and controlling stuff for the robot for every custom configuration (TIAGo robot with the accessories you provide (gripper, hand, no end effector, classical base or omni base, etc).

The idea was to add the TCP frame to the currently used xacro (in this case gripper.urdf.xacro) file, add the planning group to the SRDF (in this case tiago_pal-gripper_schunk-ft.srdf) , and run the reach study, and add the group in the kinematic yaml file (in this case, kinematics_kdl.yaml) .

To try this idea I used the following launch file that opens everything for the robot:

<?xml version="1.0" encoding="UTF-8"?>
<launch>
  <arg name="arm"  default="True"/>
  <arg name="end_effector"  default="pal-gripper"/>
  <arg name="ft_sensor"  default="schunk-ft"/>
  <arg name="laser_model"  default="sick-571"/>
  <arg name="camera_model"  default="orbbec-astra"/>
  <arg name="use_moveit_camera"  default="false"/>
  <arg name="wrist_model"  default="wrist-2010"/>
  <arg name="no_safety_eps" default="False"/>
  <arg name="advanced_grasping" default="false"/> <!-- Requires extra software from PAL Robotics-->
  <arg name="base_type" default="pmb2"/>
  <arg name="enable_moveit_camera" default="false"/>
  <arg name="base_type_suffix" value="" unless="$(eval arg('base_type') == 'omni_base')"/>
  <arg name="base_type_suffix" value="_omni" if="$(eval arg('base_type') == 'omni_base')"/>

  <!-- Robot model -->
  <include file="$(find tiago_description)/robots/upload.launch">
    <arg name="arm"  value="$(arg arm)"/>
    <arg name="end_effector"  value="$(arg end_effector)"/>
    <arg name="ft_sensor"  value="$(arg ft_sensor)"/>
    <arg name="laser_model"  value="$(arg laser_model)"/>
    <arg name="camera_model"  value="$(arg camera_model)"/>
    <arg name="wrist_model"  value="$(arg wrist_model)"/>
    <arg name="no_safety_eps"  value="$(arg no_safety_eps)"/>
    <arg name="base_type"  value="$(arg base_type)"/>
  </include>


    <!-- Motion planning -->

    <include file="$(find tiago_moveit_config)/launch/move_group.launch">
      <arg name="arm" value="$(arg arm)"/>
      <arg name="end_effector" value="$(arg end_effector)"/>
      <arg name="ft_sensor" value="$(arg ft_sensor)"/>
      <arg name="laser_model" value="$(arg laser_model)"/>
      <arg name="camera_model" value="$(arg camera_model)"/>
      <arg name="camera" value="$(arg enable_moveit_camera)"/>
      <arg name="base_type" value="$(arg base_type)"/>
      <arg name="capabilities" value="move_group/CapabilityLoader"/>
      <arg name="moveit_sensor_manager" value="advanced_grasping" if="$(arg advanced_grasping)"/>
    </include>    

This would allow to run new reach studies really fast with all configurations of the robot.

I was unfortunately unable to do that , even with the changes you gave me above, it leads to the same issues: crashes, and/or varying results and/or broken markers.

If you see any obvious reason of why it might not work. Here is a list of things I tried to fix these issues:

• Add motion groups with the graphical interface (move it configurator)
• Add manually the manipulator motion group to ompl and stompl planning yaml file
• Use a kinematics.yaml file instead of kinematics_kdl.yaml file
• Keep the gripper but attach the TCP frame to arm_tool_link (before the new branch in the kinematic tree)
• Adding some links in touch_links
• What you told me above

Unfortunately, none of them solved the issue. Since I am a beginner with ROS, maybe you will see an obvious reason of why it's not working.

One other question is: is the name of the file hard coded somewhere ? Like the names robot.launch, reach_study.yaml, reach_study.xacro etc... , do they matter ? I am pretty sure they do not if you call the right launch files in your command but at this point I am a bit confused so I am asking.

One important thing I noticed:

Even with your clean configuration, I am able to get crashes (exit code -9) (no broken markers however, and the results seems to be reproducible which is good).
If I put too much points in the pcd, the study might crash with during either the inverse kinematics step or the optimization.
(crashes can also come later when RVIZ will try to display a lot of markers but this is another topic)

At this point, from what I experienced so far, my hypothesis is that if it's too computationally intense to do the study, it will crash. For some reason I don't know loading the full move it configuration requires more power (or memory space ?) than the lightweight config. To me it makes sense because having a very low amount of markers for the study with the full move it config gives way less crashes and unstable results. It might also explain why I am able to add more markers to the study with the simple config than the heavier one.

However I don't know if what I am saying could possibly be true or if it's completely bullshit and non sense.

[marip8]

My idea was to use the full move it config for TIAGo .

That should be fine. The only requirements for the MoveIt REACH plugins is that they can access the URDF, SRDF, and kinematics/joint configurations from the ROS parameter server (i.e., robot_description, robot_description_semantic, robot_description_kinematics, robot_description_planning). It doesn't matter if those things get loaded by the "lightweight" version or by a launch file from the MoveIt config package

[marip8]

I was unfortunately unable to do that , even with the changes you gave me above, it leads to the same issues: crashes, and/or varying results and/or broken markers.

I would double check that the contents of the relevant files (URDF, SRDF, kinematics.yaml, joint_limits.yaml) are the same in the "lightweight" version as they are in your MoveIt config package.

If you see any obvious reason of why it might not work. Here is a list of things I tried to fix these issues:

• Add motion groups with the graphical interface (move it configurator)

As long as it ultimately populates these groups into the SRDF this should be fine

• Add manually the manipulator motion group to ompl and stompl planning yaml file

The reach study doesn't use any OMPL or STOMP parameters, so this wouldn't change anything

• Use a kinematics.yaml file instead of kinematics_kdl.yaml file

The name of the file doesn't matter. What matters is the ROS parameter that the contents of the file gets loaded to (robot_description_kinematics)

• Keep the gripper but attach the TCP frame to arm_tool_link (before the new branch in the kinematic tree)

If your TCP frame is downstream of the gripper joints, then your kinematic chain (from base_footprint to tcp) will include the gripper joints, which seems unnecessary. I think as long as your TCP frame is fixed to a link beyond the last robot joint but before the first gripper joint, it shouldn't make a difference

• Adding some links in touch_links

This only affects collision detection. touch_links is a list of links in the URDF that the added reachability collision object is allowed to touch (e.g., a sander on a car hood, etc.)

[marip8]

One other question is: is the name of the file hard coded somewhere ? Like the names robot.launch, reach_study.yaml, reach_study.xacro etc... , do they matter ? I am pretty sure they do not if you call the right launch files in your command but at this point I am a bit confused so I am asking.

Short answer

If you simply replace the contents of the files that exist in the demo, you should be able to launch and run the reach study without issue according to the instructions on the README

Long answer

robot.launch is just a convenience launch file to hold all of the things that need to be customized between different reach study configurations. setup.launch points to this launch file and launches a number of other common nodes needed for the reach study (joint state publisher, robot state publisher, Rviz, etc.).

Running setup.launch (and by extension, robot.launch) is not strictly required, provided you launch all of the nodes/parameters required by the REACH ROS plugins:

• ROS parameters
  • robot_description: URDF
  • robot_description_semantic: SRDF
  • robot_description_kinematics: MoveIt kinematics.yaml
  • robot_description_planning: MoveIt joint_limits.yaml
• joint_state_publisher
• robot_state_publisher for TF
• rviz for visualization

The reach study node itself also just needs a YAML file with the configuration of the reach study. This file doesn't need to be named reach_study.yaml, it's location just needs to be provided to the reach study node (and it needs to have the correct contents about the reach study)

[marip8]

Even with your clean configuration, I am able to get crashes (exit code -9) (no broken markers however, and the results seems to be reproducible which is good).

I saw this a lot as well when I was running the reach study with the Tiago. I traced the issue to this line, where there was a segfault in FCL. I tried setting the number of threads to 1, but I still got the same random segfaults, so I don't think it's a multi-threading issue. I'm not entirely sure what's causing this. I'll log an issue and we can look into it more.

If I put too much points in the pcd, the study might crash with during either the inverse kinematics step or the optimization.
(crashes can also come later when RVIZ will try to display a lot of markers but this is another topic)

At this point, from what I experienced so far, my hypothesis is that if it's too computationally intense to do the study, it will crash.

I don't think it has anything to do with launching more nodes alongside the reach study or adding more points since I was able to replicate relatively frequently with the "lightweight" setup

[vicltz]

I have a few other questions after reading your files:

• You kept the arm group in kinematics.yaml file, any reason for that ?
• for the scaling factor in joints_limits.yaml , you used 0.1 , why ?
• You number of disabled collision is very low, how did you managed to generate them ? The moveit configurator gui would give you way more, is that right ?
• Are the passive joints important in the srdf ?

Thanks a lot

[marip8]

You kept the arm group in kinematics.yaml file, any reason for that ?

Not necessarily. You could change the planning_group parameter in the reach_study.yaml file to arm if you wanted to run a reach study with that motion group instead of the full manipulator

for the scaling factor in joints_limits.yaml , you used 0.1 , why ?

That's just what MoveIt autogenerates. Those parameters are not used by the reach study anyway; they're for motion planning in MoveIt

You number of disabled collision is very low, how did you managed to generate them ? The moveit configurator gui would give you way more, is that right ?

I used the MoveIt setup assistant to generate these automatically. There are ~230 allowed collision pairs in that SRDF; were you expecting more? They seem to cover all of the relevant allowed collisions

Are the passive joints important in the srdf ?

I don't actually know how MoveIt consumes this information. My thought was that I would create passive joints for all of the joints I don't plan on solving IK for, but I'm not sure if it makes a difference or not. According to this documentation page the specification of passive joints is mostly for the motion planners. I'm not sure how it affects IK though. In this case it probably doesn't matter because the passive joints I specified are not inside the kinematic chain of the manipulator group