Baxter Demo - Wrong End Effector Poses

[maestrini]

When running the Baxter Demo, the end effector poses are completely off and lead to the cubes ending up at totally wrong positions.
At the bottom of this post you can find screenshots in which you can see the problem (A-D).

Also I have also attached the files i use to generate the plan [1] (which are blindly copied from the demo without any changes), and under the following link [2] you can also find the version of the baxter repo that i am using (which has been cloned from the original baxter repo without any changes).

I basically just fully follow your tutorial [3].
Adjusting q0 leads to different outcomes of the plan execution, so my assumption is, that the mistake may be there.
I do not really know where to begin on debugging this issue, do you maybe have an idea?

I am running the system on Ubuntu 20.04 with ROS Noetic.
When running the system from this pre-compiled docker image [4], everything works fine. Compiling the docker image myself fails at ./configure when trying to find FCL (even though it actually should be installed by the docker image creation process.

[1] baxter-blocks.zip
[2] baxter_common
[3] tmkit.kavrakilab.org/tutorial
[4] tmkit-docker

Click here for viewing the images showing the error

[maestrini]

I have tried the same installation for clean installations of Ubuntu 20, Ubuntu 18 and Ubuntu 16.
All of them have the same issue and fail as described in the original post of this issue thread.

The only way that allowed a correct execution was via the tmkit-docker version, when using the pre-compiled version of the docker image.

Running the pre-compiled image works only on some specific installations, on some similar setups (same OS, both times Ubuntu 20.04) it does not work either.

Trying to compile the docker image manually, fails at fcl, even if fcl is installed from source.

[ndantam]

When running the Baxter Demo, the end effector poses are completely off and lead to the cubes ending up at totally wrong positions.

This behavior looks like what might occur if relative transforms and parent frames are off. Do you see this behavior from following the tutorial? What if you try to run the demo programs? From the tmkit repo: ./demo/run/run-baxter-sussman-linear.sh (and a few others in that same directory).

The only way that allowed a correct execution was via the tmkit-docker version, when using the pre-compiled version of the docker image.

The docker images may not be updated for current FCL versions. The images are not developed by (or used in) my lab, so I don't know the specifics.

[maestrini]

Thank you very much for your reply.

This behavior looks like what might occur if relative transforms and parent frames are off. Do you see this behavior from following the tutorial? What if you try to run the demo programs? From the tmkit repo: ./demo/run/run-baxter-sussman-linear.sh (and a few others in that same directory).

I tried that sh file (and also the ./demo/run/run-baxter-sussman.sh and that works.

This lead to the following discovery:

The system works correctly when running it with a single command. (click here to see command.)

tmsmt package://baxter_description/urdf/baxter.urdf \
  sussman-0.robray \
  allowed-collision.robray \
  tm-blocks.pddl \
  tm-blocks.py \
  -q q0.tmp \
  -g sussman-1.robray  \
  -o baxter-sussman.tmp \
  --gui \
  --write-facts=baxter-sussman.pddl \
  --verbose

But the execution fails (as documented in the original post) if the system is executed with the same files, just as two consecutive calls instead of a single one (which is what your tutorial recommends). (click here to see commands.)

First calculate the plan:

tmsmt package://baxter_description/urdf/baxter.urdf \
  sussman-0.robray \
  allowed-collision.robray \
  tm-blocks.pddl \
  tm-blocks.py \
  -q q0.tmp \
  -g sussman-1.robray  \
  -o baxter-sussman.tmp

Then view the calculated plan:

tmsmt package://baxter_description/urdf/baxter.urdf \
  sussman-0.robray \
  allowed-collision.robray \
  baxter-sussman.tmp \
  --gui

Click here to see the complete bash file I am using to call the framework. Only the last `elif` clause works, the others lead to the described error in execution.

#!/bin/bash

if [[ $1 == init ]]; then
  aarxc --gui package://baxter_description/urdf/baxter.urdf sussman-0.robray
elif [[ $1 == plan ]]; then
  tmsmt package://baxter_description/urdf/baxter.urdf \
      sussman-0.robray \
      allowed-collision.robray \
      tm-blocks.pddl \
      tm-blocks.py \
      -q q0.tmp \
      -g sussman-1.robray  \
      -o baxter-sussman.tmp
elif [[ $1 == view ]]; then
  tmsmt package://baxter_description/urdf/baxter.urdf \
      sussman-0.robray \
      allowed-collision.robray \
      baxter-sussman.tmp \
      --gui
elif [[ $1 == run ]]; then
  tmsmt package://baxter_description/urdf/baxter.urdf \
      sussman-0.robray \
      allowed-collision.robray \
      tm-blocks.pddl \
      tm-blocks.py \
      -q q0.tmp \
      -g sussman-1.robray  \
      -o baxter-sussman.tmp \
      --gui \
      --write-facts=baxter-sussman.pddl \
      --verbose
else
  echo "Parameter options are 'run', 'init', 'plan' and 'view'"
fi

In other words:

Appending --gui to the planning command (Step 4 in the tutorial under section Setup and Invocation) views the plan correctly, but instead loading and viewing the computed plan (Step 5 in the tutorial under section Setup and Invocation) fails.

To me this sounds like an internal bug.

---

The docker images may not be updated for current FCL versions. The images are not developed by (or used in) my lab, so I don't know the specifics.

That is most definitely the case, but never the less, it works (old Amino version with old FCL version in the docker image). And in all my attempts of installing this system I used the newest version of Amino with the newest version of FCL (compiled from source by me), which only works for single command execution, not sequential command execution where planning and execution are split up in two separate calls.

And also, the bug of having different behavior dependent on the way of calling your system does not happen inside the mentioned docker container. The sh file that the docker image (based on Ubuntu 16.04) uses to run the sussman example, also makes use of to consecutive commands, but works just fine. Creating a clean Ubuntu 16.04 installation, which should closely resemble the docker container, also fails executing the consecutive commands. Here you can find a list of packages that have a difference in version between the docker image and a clean Ubuntu 16.04 or are newly installed (marked with a * instead of a second version).

---

My Conclusion:

All of this, to me, sound like there is a bug in Amino which was introduced since 2018-11-20 09:00:48 (the date on which the docker image was released) that only affects execution if running a pre-computed plan.

[ndantam]

(Sorry for the delay in investigation. June turned out to be a busy month.)

I am unable to reproduce your error. Specifically, I tried viewing (with the "view" command in your script) the plan included in the previously attached baxter-blocks.zip. The plan appears to be valid. I also tried first generating the plan and then viewing (with first the "plan" and then the "view" command in your script), which also appears to be valid.

I tested with amino (f8926ae4), tmkit (4fb6fc0), and baxter-common (6c4b0f3) (the current, public master branches). I am using Debian stable (buster) with OMPL, FCL, and Z3 from the distribution repository.

I cannot really comment on the third-party docker images.

Adjusting q0 leads to different outcomes of the plan execution, so my assumption is, that the mistake may be there.
I do not really know where to begin on debugging this issue, do you maybe have an idea?

This file specifies the inital configuration. One related note: collisions detected in the initial configuration are assumed to be allowed, so an invalid initial configuration could lead to invalid plans containing that same collision. (For example, if the initial configuration has the arm in collision with the table, plans may include later collisions between the arm and table).