JointWaypoints and StateWaypoints as planning result

[kaiConnected]

Hello tesseract community!

I am currently working on my first ever tesseract project and am getting some confusing results. I am planning a path between two points. When I do this using the abb_irb2400 urdf I am getting a trajectory consisting of StateWaypoints. But when I use a custom urdf (containing an UR5) the planned trajectory consists of JointWaypoints.
So my question is what could be the reason for this behavior, why is the type of waypoint changing?

In the end I actually do not really care whether or not it is a StateWaypoint, as long as I can extract cartesian coordiantes from it, which I so far have only managed to do for the StateWaypoints. In case you can't help me with my question above I would also be glad if you could tell me if there is a solution for that.

The code I am using (mostly copied from the tesseract_python example):

def path_planning(wp1:CartesianWaypoint = None,wp2:CartesianWaypoint = None):

    OMPL_DEFAULT_NAMESPACE = "OMPLMotionPlannerTask"
    TRAJOPT_DEFAULT_NAMESPACE = "TrajOptMotionPlannerTask"

    task_composer_filename = os.environ["TESSERACT_TASK_COMPOSER_CONFIG_FILE"]

    # Initialize the resource locator and environment
    locator = GeneralResourceLocator()
    urdf_xml_path = FilesystemPath(locator.locateResource("/root/ws/src/tesseract_planning/resources/urdf/workStation.urdf").getFilePath())
    srdf_xml_path =  FilesystemPath(locator.locateResource("/root/ws/src/tesseract_planning/resources/urdf/workStation.srdf").getFilePath())

    t_env = Environment()

    # locator_fn must be kept alive by maintaining a reference
    assert t_env.init(urdf_xml_path, srdf_xml_path, locator)
    # Fill in the manipulator information. This is used to find the kinematic chain for the manipulator. This must
    # match the SRDF, although the exact tcp_frame can differ if a tool is used.
    manip_info = ManipulatorInfo()
    manip_info.tcp_frame = "tool0"
    manip_info.manipulator = "manipulator"
    manip_info.working_frame = "base_link"

    # # Create a viewer and set the environment so the results can be displayed later
    # viewer = TesseractViewer()
    # viewer.update_environment(t_env, [0,0,0])

    # Set the initial state of the robot
    # joint_names = ["joint_%d" % (i+1) for i in range(6)]

    joint_names = ["shoulder_pan_joint","shoulder_lift_joint","elbow_joint","wrist_1_joint","wrist_2_joint","wrist_3_joint"]


    # viewer.update_joint_positions(joint_names, np.array([1,-.2,.01,.3,-.5,1]))
    # # # Start the viewer
    # viewer.start_serve_background()

    # Set the initial state of the robot
    t_env.setState(joint_names, np.ones(6)*0.1)

    # Create the input command program waypoints
    # wp1 = CartesianWaypoint(Isometry3d.Identity() * Translation3d(-0.694,-0.552,1.0155) * Quaterniond(0.0,0,0.0,0))
    # wp2 = CartesianWaypoint(Isometry3d.Identity() * Translation3d(-0.594,-0.652,1.5155) * Quaterniond(0.0,0.0,0.0,0))

    # Create the input command program instructions. Note the use of explicit construction of the CartesianWaypointPoly
    # using the *_wrap_CartesianWaypoint functions. This is required because the Python bindings do not support implicit
    # conversion from the CartesianWaypoint to the CartesianWaypointPoly.
    start_instruction = MoveInstruction(CartesianWaypointPoly_wrap_CartesianWaypoint(wp1), MoveInstructionType_FREESPACE)
    plan_f1 = MoveInstruction(CartesianWaypointPoly_wrap_CartesianWaypoint(wp2), MoveInstructionType_FREESPACE, "DEFAULT")

    # Create the input command program. Note the use of *_wrap_MoveInstruction functions. This is required because the
    # Python bindings do not support implicit conversion from the MoveInstruction to the MoveInstructionPoly.
    program = CompositeInstruction("DEFAULT")
    program.setManipulatorInfo(manip_info)
    program.appendMoveInstruction(MoveInstructionPoly_wrap_MoveInstruction(start_instruction))
    program.appendMoveInstruction(MoveInstructionPoly_wrap_MoveInstruction(plan_f1))

    # Create the task composer plugin factory and load the plugins
    config_path = FilesystemPath(task_composer_filename)
    factory = TaskComposerPluginFactory(config_path)

    # Create the task composer node. In this case the FreespacePipeline is used. Many other are available.
    task = factory.createTaskComposerNode("FreespacePipeline")

    # Get the input and output keys for the task
    input_key = task.getInputKeys()[0]
    output_key = task.getOutputKeys()[0]

    # Create a profile dictionary. Profiles can be customized by adding to this dictionary and setting the profiles
    # in the instructions.
    profiles = ProfileDictionary()

    # Create an AnyPoly containing the program. This explicit step is required because the Python bindings do not
    # support implicit conversion from the CompositeInstruction to the AnyPoly.
    program_anypoly = AnyPoly_wrap_CompositeInstruction(program)

    # Create the task data storage and set the data
    task_data = TaskComposerDataStorage()
    task_data.setData(input_key, program_anypoly)

    # Create the task problem and input
    task_planning_problem = PlanningTaskComposerProblemUPtr.make_unique(t_env, task_data, profiles)
    task_problem = PlanningTaskComposerProblemUPtr_as_TaskComposerProblemUPtr(task_planning_problem)
    task_input = TaskComposerInput(task_problem)

    # Create an executor to run the task
    task_executor = factory.createTaskComposerExecutor("TaskflowExecutor")

    # Run the task and wait for completion
    future = task_executor.run(task.get(), task_input)
    future.wait()
    # Retrieve the output, converting the AnyPoly back to a CompositeInstruction
    results = AnyPoly_as_CompositeInstruction(task_input.data_storage.getData(output_key))

ABB urdf:

<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- |    This document was autogenerated by xacro from irb2400.xacro                  | -->
<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
<!-- =================================================================================== -->
<robot name="abb_irb2400" xmlns:xacro="http://ros.org/wiki/xacro">
  <!-- Conversion were obtained from http://www.e-paint.co.uk/Lab_values.asp
       unless otherwise stated. -->
  <!-- Actual colors -->
  <!-- NCS 2070-Y60R orange, standard robot color 1974 - May 2014 -->
  <!-- RAL 7035 light grey (abb graphite white), standard robot color May 2014 - current -->
  <!-- RAL 9003 signal white (abb white), cleanroom robot color -->
  <!-- RAL 7032 pebble gray, controller -->
  <!-- RAL 7012 basalt gray (abb dark gray) , robot primer -->
  <!-- RAL 7021 black gray (abb black) , robot with dark link color -->
  <!-- Other robot colours availble, but not limited too. -->
  <!-- RAL 3001 signal red -->
  <!-- RAL 3002 carmine red -->
  <!-- RAL 5002 ultramarine blue -->
  <!-- RAL 5015 sky blue -->
  <!-- RAL 6002 leaf green -->
  <!-- RAL 9002 gray white -->
  <!-- RAL 9010 pure white -->
  <!-- approximations -->
  <!-- yellow, collision model -->
  <!-- link list -->
  <link name="base_link">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://tesseract_planning/resources/meshes/abb_irb2400/irb2400/visual/base_link.dae"/>
      </geometry>
      <material name="">
        <color rgba="0.7372549 0.3490196 0.1607843 1"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://tesseract_planning/resources/meshes/abb_irb2400/irb2400/collision/base_link.stl"/>
      </geometry>
      <material name="yellow">
        <color rgba="1 1 0 1"/>
      </material>
    </collision>
  </link>
  <link name="link_1">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://tesseract_planning/resources/meshes/abb_irb2400/irb2400/visual/link_1.dae"/>
      </geometry>
      <material name="">
        <color rgba="0.7372549 0.3490196 0.1607843 1"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://tesseract_planning/resources/meshes/abb_irb2400/irb2400/collision/link_1.stl"/>
      </geometry>
    </collision>
  </link>
  <link name="link_2">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://tesseract_planning/resources/meshes/abb_irb2400/irb2400/visual/link_2.dae"/>
      </geometry>
      <material name="">
        <color rgba="0.7372549 0.3490196 0.1607843 1"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://tesseract_planning/resources/meshes/abb_irb2400/irb2400/collision/link_2_whole.stl"/>
      </geometry>
    </collision>
  </link>
  <link name="link_3">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://tesseract_planning/resources/meshes/abb_irb2400/irb2400/visual/link_3.dae"/>
      </geometry>
      <material name="">
        <color rgba="0.7372549 0.3490196 0.1607843 1"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://tesseract_planning/resources/meshes/abb_irb2400/irb2400/collision/link_3.stl"/>
      </geometry>
    </collision>
  </link>
  <link name="link_4">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://tesseract_planning/resources/meshes/abb_irb2400/irb2400/visual/link_4.dae"/>
      </geometry>
      <material name="">
        <color rgba="0.7372549 0.3490196 0.1607843 1"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://tesseract_planning/resources/meshes/abb_irb2400/irb2400/collision/link_4.stl"/>
      </geometry>
    </collision>
  </link>
  <link name="link_5">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://tesseract_planning/resources/meshes/abb_irb2400/irb2400/visual/link_5.dae"/>
      </geometry>
      <material name="">
        <color rgba="0.7372549 0.3490196 0.1607843 1"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://tesseract_planning/resources/meshes/abb_irb2400/irb2400/collision/link_5.stl"/>
      </geometry>
    </collision>
  </link>
  <link name="link_6">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://tesseract_planning/resources/meshes/abb_irb2400/irb2400/visual/link_6.dae"/>
      </geometry>
      <material name="">
        <color rgba="0.1882353 0.1960784 0.2039216 1"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://tesseract_planning/resources/meshes/abb_irb2400/irb2400/collision/link_6.stl"/>
      </geometry>
    </collision>
  </link>
  <link name="tool0"/>
  <!-- end of link list -->
  <!-- joint list -->
  <joint name="joint_1" type="revolute">
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <parent link="base_link"/>
    <child link="link_1"/>
    <axis xyz="0 0 1"/>
    <limit effort="0" lower="-3.1416" upper="3.1416" velocity="2.618"/>
  </joint>
  <joint name="joint_2" type="revolute">
    <origin rpy="0 0 0" xyz="0.1 0 0.615"/>
    <parent link="link_1"/>
    <child link="link_2"/>
    <axis xyz="0 1 0"/>
    <limit effort="0" lower="-1.7453" upper="1.9199" velocity="2.618"/>
  </joint>
  <joint name="joint_3" type="revolute">
    <origin rpy="0 0 0" xyz="0 0 0.705"/>
    <parent link="link_2"/>
    <child link="link_3"/>
    <axis xyz="0 1 0"/>
    <limit effort="0" lower="-1.0472" upper="1.1345" velocity="2.618"/>
  </joint>
  <joint name="joint_4" type="revolute">
    <origin rpy="0 0 0" xyz="0.258 0 0.135"/>
    <parent link="link_3"/>
    <child link="link_4"/>
    <axis xyz="1 0 0"/>
    <limit effort="0" lower="-3.49" upper="3.49" velocity="6.2832"/>
  </joint>
  <joint name="joint_5" type="revolute">
    <origin rpy="0 0 0" xyz="0.497 0 0"/>
    <parent link="link_4"/>
    <child link="link_5"/>
    <axis xyz="0 1 0"/>
    <limit effort="0" lower="-2.0944" upper="2.0944" velocity="6.2832"/>
  </joint>
  <joint name="joint_6" type="revolute">
    <origin rpy="0 0 0" xyz="0.085 0 0"/>
    <parent link="link_5"/>
    <child link="link_6"/>
    <axis xyz="1 0 0"/>
    <limit effort="0" lower="-6.9813" upper="6.9813" velocity="7.854"/>
  </joint>
  <joint name="joint_6-tool0" type="fixed">
    <parent link="link_6"/>
    <child link="tool0"/>
    <origin rpy="0 1.57079632679 0" xyz="0 0 0"/>
  </joint>
  <!-- end of joint list -->
  <!-- ROS base_link to ABB World Coordinates transform -->
  <link name="base"/>
  <joint name="base_link-base" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <parent link="base_link"/>
    <child link="base"/>
  </joint>
</robot>

ABB srdf:

<?xml version="1.0" ?>
<!--This does not replace URDF, and is not an extension of URDF.
    This is a format for representing semantic information about the robot structure.
    A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined
-->
<robot name="abb_irb2400">
    <!--GROUPS: Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
    <!--LINKS: When a link is specified, the parent joint of that link (if it exists) is automatically included-->
    <!--JOINTS: When a joint is specified, the child link of that joint (which will always exist) is automatically included-->
    <!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
    <!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
    <group name="manipulator">
        <chain base_link="base_link" tip_link="tool0" />
    </group>
    <!--GROUP STATES: Purpose: Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
    <group_state name="all-zeros" group="manipulator">
        <joint name="joint_1" value="0" />
        <joint name="joint_2" value="0" />
        <joint name="joint_3" value="0" />
        <joint name="joint_4" value="0" />
        <joint name="joint_5" value="0" />
        <joint name="joint_6" value="0" />
    </group_state>

    <!--GROUP TOOL CENTER POINTS -->
    <group_tcps group="manipulator">
      <tcp name="laser" xyz="1 .1 1" rpy="0 1.57 0" />
      <tcp name="welder" xyz=".1 1 .2" wxyz="1 0 0 0" />
    </group_tcps>

    <!--Groups kinematic plugins -->
    <kinematics_plugin_config filename="package://tesseract_planning/resources/urdf/abb_irb2400_plugins.yaml"/>

    <!--Contact Managers plugins -->
    <contact_managers_plugin_config filename="package://tesseract_planning/resources/urdf/contact_manager_plugins.yaml"/>

    <!--VIRTUAL JOINT: Purpose: this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)-->
    <virtual_joint name="FixedBase" type="fixed" parent_frame="world" child_link="base_link" />

    <!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
    <disable_collisions link1="base_link" link2="link_1" reason="Adjacent" />
    <disable_collisions link1="base_link" link2="link_2" reason="Never" />
    <disable_collisions link1="base_link" link2="link_3" reason="Never" />
    <disable_collisions link1="link_1" link2="link_2" reason="Adjacent" />
    <disable_collisions link1="link_1" link2="link_3" reason="Never" />
    <disable_collisions link1="link_2" link2="link_3" reason="Adjacent" />
    <disable_collisions link1="link_2" link2="link_4" reason="Never" />
    <disable_collisions link1="link_2" link2="link_5" reason="Never" />
    <disable_collisions link1="link_2" link2="link_6" reason="Never" />
    <disable_collisions link1="link_3" link2="link_4" reason="Adjacent" />
    <disable_collisions link1="link_3" link2="link_5" reason="Never" />
    <disable_collisions link1="link_3" link2="link_6" reason="Never" />
    <disable_collisions link1="link_4" link2="link_5" reason="Adjacent" />
    <disable_collisions link1="link_4" link2="link_6" reason="Default" />
    <disable_collisions link1="link_5" link2="link_6" reason="Adjacent" />
</robot>

Custom urdf:

<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- |    This document was autogenerated by xacro from ur5.urdf.xacro                 | -->
<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
<!-- =================================================================================== -->
<!-- 

# ===================================== COPYRIGHT ===================================== #
#                                                                                       #
#  IFRA (Intelligent Flexible Robotics and Assembly) Group, CRANFIELD UNIVERSITY        #
#  Created on behalf of the IFRA Group at Cranfield University, United Kingdom          #
#  E-mail: [email protected]                                                         #
#                                                                                       #
#  Licensed under the Apache-2.0 License.                                               #
#  You may not use this file except in compliance with the License.                     #
#  You may obtain a copy of the License at: http://www.apache.org/licenses/LICENSE-2.0  #
#                                                                                       #
#  Unless required by applicable law or agreed to in writing, software distributed      #
#  under the License is distributed on an "as-is" basis, without warranties or          #
#  conditions of any kind, either express or implied. See the License for the specific  #
#  language governing permissions and limitations under the License.                    #
#                                                                                       #
#  IFRA Group - Cranfield University                                                    #
#  AUTHORS: Mikel Bueno Viso - [email protected]                         #
#           Seemal Asif      - [email protected]                                   #
#           Phil Webb        - [email protected]                                 #
#                                                                                       #
#  Date: July, 2022.                                                                    #
#                                                                                       #
# ===================================== COPYRIGHT ===================================== #

# ===================================== COPYRIGHT ===================================== #
#                                                                                       #
#  Some of the information used to build the ROS2 UR5-Robot .urdf files in this ROS2    #
#  repository has been taken from the following file://resources:                              #
#       - Universal Robots ROS2 Description:                                            #
#         https://github.com/UniversalRobots/Universal_Robots_ROS2_Description          #
#       - Universal Robots ROS2 Gazebo Simulation:                                      #
#         https://github.com/UniversalRobots/Universal_Robots_ROS2_Gazebo_Simulation    #
#                                                                                       #
#  AUTHORS:                                                                             #
#       - ROS-Industrial - https://rosindustrial.org                                    #
#       - Universal Robots - https://github.com/UniversalRobots                         #
#       - Nadia Hammoudeh Garcia, Kelsey Hawkins, G.A. vd. Hoorn, Lovro Ivanov, Mathias #
#         Ludtke, Wim Meeussen, Felix Messmer, Miguel Prada Sarasola, Denis Stogl,      #
#         Andy Zelenak, Marvin Grosse Besselmann.                                       #
#       - Maintainers: Felix Exner ([email protected]), Tristan Schnell ([email protected]),    #
#         Lovro Ivanov ([email protected]), Denis Stogl ([email protected]).  #
#                                                                                       #
#  Copyright (c) 2021, Universal Robots A/S                                             #
#  (ORIGINAL LICENSE) -> Licensed under the BSD 3-Clause License.                       #
#                                                                                       #
#  CHANGES MADE:                                                                        #
#  Robot information/specifications have been taken, and the code has been modified and #
#  re-organised in accordance with the ros2_RobotSimulation standards.                  #
#                                                                                       #
# ===================================== COPYRIGHT ===================================== #

# ======= CITE OUR WORK ======= #
# You can cite our work with the following statement:
# IFRA (2022) ROS2.0 ROBOT SIMULATION. URL: https://github.com/IFRA-Cranfield/ros2_RobotSimulation.

-->
<robot name="ur5">
  <!--link name="eef_tool">
      <visual>
        <geometry>
          <mesh filename="file://$(find ur5_ros2_gazebo)/meshes/end_effector/ATI.stl" scale="0.001 0.001 0.001"/>
        </geometry>
      </visual>
      <collision>
        <geometry>
          
          <mesh filename="file://$(find ur5_ros2_gazebo)/meshes/end_effector/ATI.stl" scale="0.001 0.001 0.001"/>
        </geometry>
      </collision>
    <inertial>
      <mass value="${eef_mass}" />
      <origin xyz="${-0.5 * eef_length} 0.0 0.0" rpy="0 ${pi/2} 0" />
      <inertia ixx="${0.0833333 * eef_mass * (3 * eef_radius * eef_radius + eef_length * eef_length)}" ixy="0.0" ixz="0.0"
        iyy="${0.0833333 * eef_mass * (3 * eef_radius * eef_radius + eef_length * eef_length)}" iyz="0.0"
        izz="${0.5 * eef_mass * eef_radius * eef_radius}" />
    </inertial>
    </link-->
  <link name="ur10_adaptor">
    <visual>
      <geometry>
        <mesh filename="file:///root/ws/src/tesseract_planning/resources/meshes/workstation/visual/UR_10_adaptor.dae" scale="0.001 0.001 0.001"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="file:///root/ws/src/tesseract_planning/resources/meshes/workstation/collision/UR_10_adaptor.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.65918"/>
      <origin rpy="0 0 0" xyz="0.0 0.0 0.4984"/>
      <inertia ixx="0.002454919" ixy="0.0" ixz="0.0" iyy="0.0024549196" iyz="0.0" izz="0.00044587949"/>
    </inertial>
  </link>
  <link name="ur10_adaptor2">
    <visual>
      <origin rpy="0 0 0" xyz="-0.1 0.0 0"/>
      <geometry>
        <mesh filename="file:///root/ws/src/tesseract_planning/resources/meshes/workstation/visual/UR_10_adaptor2.dae" scale="0.001 0.001 0.001"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="-0.1 0.0 0"/>
      <geometry>
        <mesh filename="file:///root/ws/src/tesseract_planning/resources/meshes/workstation/collision/UR_10_adaptor2.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="2.08325"/>
      <origin rpy="0 0 0" xyz="0.1 0.0415 0.02766"/>
      <inertia ixx="0.00706490805" ixy="0.00864546876" ixz="0.00576209782" iyy="0.02748654784" iyz="0.0023912706" izz="0.03000135577"/>
    </inertial>
  </link>
  <link name="ATI">
    <visual>
      <origin rpy="0 0 0" xyz="-0.041275 -0.21622 -0.041275"/>
      <geometry>
        <mesh filename="file:///root/ws/src/tesseract_planning/resources/meshes/workstation/visual/ATI_9150_RCE_230_E.dae" scale="0.001 0.001 0.001"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="-0.041275 -0.21622 -0.041275"/>
      <geometry>
        <mesh filename="file:///root/ws/src/tesseract_planning/resources/meshes/workstation/collision/ATI_9150_RCE_230_E.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="1.637468"/>
      <origin rpy="0 0 0" xyz="0.0 -0.04318 -0.035306"/>
      <inertia ixx="0.009007449" ixy="-0.000002926" ixz="-0.000002926" iyy="0.003268785" iyz="0.002513775" izz="0.007020425"/>
    </inertial>
  </link>
  <link name="burr">
    <visual>
      <geometry>
        <mesh filename="file:///root/ws/src/tesseract_planning/resources/meshes/workstation/visual/9150_RC_B_24033.dae" scale="0.001 0.001 0.001"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="file:///root/ws/src/tesseract_planning/resources/meshes/workstation/collision/9150_RC_B_24033.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.00553"/>
      <origin rpy="0 0 0" xyz="0.0 0.0 0.01111"/>
      <inertia ixx="0.00000092483" ixy="-0.0" ixz="-0.0" iyy="0.00000092483" iyz="0.0" izz="0.00000002788"/>
    </inertial>
  </link>
  <joint name="eef_adaptor_joint" type="fixed">
    <parent link="ur10_adaptor"/>
    <child link="ur10_adaptor2"/>
    <origin rpy="0 0 0" xyz="0.0 -0.0415 0.105"/>
  </joint>
  <joint name="ur10_adaptor2_ATI_joint" type="fixed">
    <parent link="ur10_adaptor2"/>
    <child link="ATI"/>
    <origin rpy="-0.785398 0 1.5707963267948966" xyz="0.0665 0.04054 0.0325"/>
    <!--origin xyz="0.065 0.04054 0.0" rpy="0 0.55766 0.0" /-->
  </joint>
  <joint name="ATI_burr_joint" type="fixed">
    <parent link="ATI"/>
    <child link="burr"/>
    <origin rpy="1.5707963267948966 0 0" xyz="0.0 -0.21622 0.0"/>
  </joint>
  <link name="table__link">
    <visual>
      <origin rpy="0 0 0" xyz="-0.6 -0.4 0"/>
      <geometry>
        <!-- The path to the visual meshes in the package. -->
        <mesh filename="file:///root/ws/src/tesseract_planning/resources/meshes/workstation/visual/Table.dae" scale="0.001 0.001 0.001"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="-0.6 -0.4 0"/>
      <geometry>
        <!-- The path to the collision meshes in the package. -->
        <mesh filename="file:///root/ws/src/tesseract_planning/resources/meshes/workstation/collision/Table.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="2280.960"/>
      <origin rpy="0 0 0" xyz="0.6 0.4 0.44"/>
      <inertia ixx="1075.396608000" ixy="547.430400000" ixz="602.173440000" iyy="1683.652608000" iyz="401.448960000" izz="1581.465600000"/>
    </inertial>
  </link>
  <gazebo reference="table__link">
    <material>Gazebo/Red</material>
  </gazebo>
  <link name="world"/>
  <joint name="workstation_world_joint" type="fixed">
    <parent link="world"/>
    <child link="table__link"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
  </joint>
  <link name="rotary_table">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0.0"/>
      <geometry>
        <mesh filename="file:///root/ws/src/tesseract_planning/resources/meshes/workstation/visual/Rotary_table.dae" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="Cyan">
        <color rgba="0 1.0 1.0 1.0"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0.0"/>
      <geometry>
        <mesh filename="file:///root/ws/src/tesseract_planning/resources/meshes/workstation/collision/Rotary_table.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="3.528591"/>
      <origin xyz="0.012984 -0.023475 0.110085"/>
      <inertia ixx="0.052238831649" ixy="-0.002725646505" ixz="0.005081081687" iyy="0.046727412047" iyz="-0.009271627212" izz="0.012390121881"/>
    </inertial>
  </link>
  <joint name="rotary_table_joint" type="fixed">
    <parent link="table__link"/>
    <child link="rotary_table"/>
    <origin xyz="0 0 0.79"/>
  </joint>
  <link name="gripper_adaptor1">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0.0"/>
      <geometry>
        <mesh filename="file:///root/ws/src/tesseract_planning/resources/meshes/workstation/visual/Gripper_adaptor1.dae" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="Cyan">
        <color rgba="0 1.0 1.0 1.0"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0.0"/>
      <geometry>
        <mesh filename="file:///root/ws/src/tesseract_planning/resources/meshes/workstation/collision/Gripper_adaptor1.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.17649"/>
      <origin xyz="0.0 0.0 0.010"/>
      <inertia ixx="0.00014747601" ixy="0.0" ixz="0.00" iyy="0.00014747601" iyz="0.0" izz="0.00024788677"/>
    </inertial>
  </link>
  <link name="Gripper_head_80D_assy">
    <visual>
      <origin rpy="0 0 0" xyz="-0.0915 -0.152 0.00"/>
      <geometry>
        <mesh filename="file:///root/ws/src/tesseract_planning/resources/meshes/workstation/visual/Gripper_head_80D_assy.dae" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="Cyan">
        <color rgba="0 1.0 1.0 1.0"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="-0.0915 -0.152 0.000"/>
      <geometry>
        <mesh filename="file:///root/ws/src/tesseract_planning/resources/meshes/workstation/collision/Gripper_head_80D_assy.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.7"/>
      <!-- 0.6383 -->
      <origin xyz="0.035 -0.03944 0.007"/>
      <inertia ixx="0.00173006549" ixy="-0.00000937696" ixz="-0.00000055057" iyy="0.00087925780" iyz="-0.00000163034" izz="0.00173813172"/>
    </inertial>
  </link>
  <joint name="rotary_table_adaptor_joint" type="fixed">
    <parent link="rotary_table"/>
    <child link="gripper_adaptor1"/>
    <origin xyz="0 0 0.135"/>
  </joint>
  <joint name="gripper_joint" type="fixed">
    <parent link="gripper_adaptor1"/>
    <child link="Gripper_head_80D_assy"/>
    <origin xyz="0 0 -0.132"/>
  </joint>
  <link name="workpiece">
    <visual>
      <origin rpy="0 0 0" xyz="-0.0025 0 -0.0025"/>
      <geometry>
        <mesh filename="file:///root/ws/src/tesseract_planning/resources/meshes/workstation/visual/Car_part.dae" scale="0.001 0.001 0.001"/>
        <!--size: x:0.9  y:0.65  z:1.08 -->
      </geometry>
      <material name="Cyan">
        <color rgba="0 1.0 1.0 1.0"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="-0.0025 0 -0.0025"/>
      <geometry>
        <mesh filename="file:///root/ws/src/tesseract_planning/resources/meshes/workstation/collision/Car_part.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.456333"/>
      <origin xyz="-0.002021 -0.028628 -0.001532"/>
      <inertia ixx="0.001626993890" ixy="0.000051063177" ixz="-0.000025245846" iyy="0.000367839908" iyz="-0.000051815686" izz="0.001582860453"/>
    </inertial>
  </link>
  <joint name="gripper_workspiece_joint" type="fixed">
    <parent link="Gripper_head_80D_assy"/>
    <child link="workpiece"/>
    <origin rpy="1.5707963267948966 0 0" xyz="0 0 0.225"/>
  </joint>
  <link name="camera_assy">
    <!--Mass and inertial values for HFSB5-4040-800.  -->
    <visual>
      <origin rpy="0 0 0" xyz="-0.098 0.398 -0.0875"/>
      <geometry>
        <mesh filename="file:///root/ws/src/tesseract_planning/resources/meshes/workstation/visual/Camera_assy.dae" scale="0.001 0.001 0.001"/>
        <!--size: x:0.9  y:0.65  z:1.08 -->
      </geometry>
      <material name="Cyan">
        <color rgba="0 1.0 1.0 1.0"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="-0.00 0 -0.00"/>
      <geometry>
        <mesh filename="file:///root/ws/src/tesseract_planning/resources/meshes/workstation/collision/Camera_assy.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="1.038"/>
      <origin xyz="0.0 0.0 -0.4"/>
      <inertia ixx="0.22177798699" ixy="0.00" ixz="-0.00" iyy="0.22177798699" iyz="0.00" izz="0.00038507830"/>
    </inertial>
  </link>
  <joint name="table_camera_assy_joint" type="fixed">
    <parent link="table__link"/>
    <child link="camera_assy"/>
    <origin rpy="0 0 -1.5707963267948966" xyz="-0.57 0.0 0.85"/>
    <!-- -0.6 0 0.8-->
  </joint>
  <!-- Load end effector file: -->
  <!--xacro:include filename="$(find ur_description)/urdf/end_effector.urdf.xacro" /-->
  <!-- Load and initialise ur5 INITIAL POSITIONS FILE:  
    <xacro:arg name="initial_positions_file" default="$(find ur5_ros2_gazebo/config/initial_positions.yaml"/>
    <xacro:property name="initial_positions_file" default="$(arg initial_positions_file)"/>  -->
  <!-- IF ADD TO ur5_robot: initial_positions="${load_yaml(initial_positions_file)}" -->
  <!-- ***** PARAMETER VALUES ***** -->
  <!-- All parameter values have been taken from the following .yaml files, located in the ~/ur5_ros2_gazebo/config folder:
          - default_kinematics.yaml
          - joint_limits.yaml
          - physical_parameters.yaml
          - visual_parameters.yaml 
          -->
  <transmission name="shoulder_pan_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="shoulder_pan_joint">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="shoulder_pan_motor">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="shoulder_lift_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="shoulder_lift_joint">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="shoulder_lift_motor">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="elbow_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="elbow_joint">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="elbow_motor">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="wrist_1_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="wrist_1_joint">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="wrist_1_motor">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="wrist_2_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="wrist_2_joint">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="wrist_2_motor">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="wrist_3_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="wrist_3_joint">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="wrist_3_motor">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <gazebo>
    <plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
      <robot_sim_type>gazebo_ros2_control/GazeboSystem</robot_sim_type>
      <parameters>~/FAL/e4ffal1508-high-level-control/simulator/src/ros2_RobotSimulation/UniversalRobots/UR5/ur5_ros2_gazebo/config/ur5_controller.yaml</parameters>
    </plugin>
  </gazebo>
  <ros2_control name="GazeboSystem" type="system">
    <hardware>
      <plugin>gazebo_ros2_control/GazeboSystem</plugin>
    </hardware>
    <!--
      <hardware>
        <xacro:if value="${use_fake_hardware}">
          <plugin>fake_components/GenericSystem</plugin>
          <param name="fake_sensor_commands">${fake_sensor_commands}</param>
          <param name="state_following_offset">0.0</param>
        </xacro:if>
        <xacro:unless value="${use_fake_hardware}">
          <plugin>ur_robot_driver/URPositionHardwareInterface</plugin>
          <param name="robot_ip">${robot_ip}</param>
          <param name="script_filename">${script_filename}</param>
          <param name="output_recipe_filename">${output_recipe_filename}</param>
          <param name="input_recipe_filename">${input_recipe_filename}</param>
          <param name="headless_mode">0</param>
          <param name="reverse_port">50001</param>
          <param name="script_sender_port">50002</param>
          <param name="tf_prefix">"${tf_prefix}"</param>
          <param name="non_blocking_read">0</param>
          <param name="servoj_gain">2000</param>
          <param name="servoj_lookahead_time">0.03</param>
          <param name="use_tool_communication">0</param>
          <param name="kinematics/hash">${hash_kinematics}</param>
          <param name="tool_voltage">0</param>
          <param name="tool_parity">0</param>
          <param name="tool_baud_rate">115200</param>
          <param name="tool_stop_bits">1</param>
          <param name="tool_rx_idle_chars">1.5</param>
          <param name="tool_tx_idle_chars">3.5</param>
          <param name="tool_device_name">/tmp/ttyUR</param>
          <param name="tool_tcp_port">54321</param>
        </xacro:unless>
      </hardware> -->
    <joint name="shoulder_pan_joint">
      <command_interface name="position">
        <param name="min">-6.283185307179586</param>
        <param name="max">6.283185307179586</param>
      </command_interface>
      <command_interface name="velocity">
        <param name="min">-3.15</param>
        <param name="max">3.15</param>
      </command_interface>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
      <!-- <param name="initial_position">${initial_positions['shoulder_pan_joint']}</param>   initial position for the FakeSystem -->
    </joint>
    <joint name="shoulder_lift_joint">
      <command_interface name="position">
        <param name="min">-6.283185307179586</param>
        <param name="max">6.283185307179586</param>
      </command_interface>
      <command_interface name="velocity">
        <param name="min">-3.15</param>
        <param name="max">3.15</param>
      </command_interface>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
      <!-- <param name="initial_position">${initial_positions['shoulder_lift_joint']}</param>   initial position for the FakeSystem -->
    </joint>
    <joint name="elbow_joint">
      <command_interface name="position">
        <param name="min">-3.141592653589793</param>
        <param name="max">3.141592653589793</param>
      </command_interface>
      <command_interface name="velocity">
        <param name="min">-3.15</param>
        <param name="max">3.15</param>
      </command_interface>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
      <!-- <param name="initial_position">${initial_positions['elbow_joint']}</param>   initial position for the FakeSystem -->
    </joint>
    <joint name="wrist_1_joint">
      <command_interface name="position">
        <param name="min">-6.283185307179586</param>
        <param name="max">6.283185307179586</param>
      </command_interface>
      <command_interface name="velocity">
        <param name="min">-3.2</param>
        <param name="max">3.2</param>
      </command_interface>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
      <!-- <param name="initial_position">${initial_positions['wrist_1_joint']}</param>   initial position for the FakeSystem -->
    </joint>
    <joint name="wrist_2_joint">
      <command_interface name="position">
        <param name="min">-6.283185307179586</param>
        <param name="max">6.283185307179586</param>
      </command_interface>
      <command_interface name="velocity">
        <param name="min">-3.2</param>
        <param name="max">3.2</param>
      </command_interface>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
      <!-- <param name="initial_position">${initial_positions['wrist_2_joint']}</param>   initial position for the FakeSystem -->
    </joint>
    <joint name="wrist_3_joint">
      <command_interface name="position">
        <param name="min">-6.283185307179586</param>
        <param name="max">6.283185307179586</param>
      </command_interface>
      <command_interface name="velocity">
        <param name="min">-3.2</param>
        <param name="max">3.2</param>
      </command_interface>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
      <!-- <param name="initial_position">${initial_positions['wrist_3_joint']}</param>   initial position for the FakeSystem -->
    </joint>
    <!--
      <sensor name="tcp_fts_sensor">
        <state_interface name="force.x"/>
        <state_interface name="force.y"/>
        <state_interface name="force.z"/>
        <state_interface name="torque.x"/>
        <state_interface name="torque.y"/>
        <state_interface name="torque.z"/>
      </sensor> -->
  </ros2_control>
  <gazebo reference="shoulder_link">
    <selfCollide>false</selfCollide>
  </gazebo>
  <gazebo reference="upper_arm_link">
    <selfCollide>false</selfCollide>
  </gazebo>
  <gazebo reference="forearm_link">
    <selfCollide>false</selfCollide>
  </gazebo>
  <gazebo reference="wrist_1_link">
    <selfCollide>false</selfCollide>
  </gazebo>
  <gazebo reference="wrist_3_link">
    <selfCollide>false</selfCollide>
  </gazebo>
  <gazebo reference="wrist_2_link">
    <selfCollide>false</selfCollide>
  </gazebo>
  <gazebo reference="ee_link">
    <selfCollide>false</selfCollide>
  </gazebo>
  <gazebo reference="table__link">
    <material>Gazebo/Grey</material>
    <selfCollide>false</selfCollide>
  </gazebo>
  <gazebo reference="workpiece">
    <material>Gazebo/SkyBlue</material>
    <selfCollide>false</selfCollide>
  </gazebo>
  <gazebo reference="ur10_adaptor">
    <material>Gazebo/Grey</material>
    <selfCollide>false</selfCollide>
  </gazebo>
  <gazebo reference="ur10_adaptor2">
    <material>Gazebo/Grey</material>
    <selfCollide>false</selfCollide>
  </gazebo>
  <gazebo reference="ATI">
    <material>Gazebo/Grey</material>
    <selfCollide>false</selfCollide>
  </gazebo>
  <gazebo reference="burr">
    <material>Gazebo/Grey</material>
    <selfCollide>false</selfCollide>
  </gazebo>
  <!-- ***** ur5 ROBOT .urdf SERIAL CHAIN *****-->
  <!-- Links: -->
  <link name="base_link"/>
  <link name="base_link_inertia">
    <visual>
      <origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
      <geometry>
        <mesh filename="file:///root/ws/src/tesseract_planning/resources/meshes/workstation/visual/base.dae"/>
      </geometry>
      <material name="LightGrey">
        <color rgba="0.7 0.7 0.7 1.0"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
      <geometry>
        <mesh filename="file:///root/ws/src/tesseract_planning/resources/meshes/workstation/collision/base.stl"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="4.0"/>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <inertia ixx="0.00443333156" ixy="0.0" ixz="0.0" iyy="0.00443333156" iyz="0.0" izz="0.0072"/>
    </inertial>
  </link>
  <link name="shoulder_link">
    <visual>
      <origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
      <geometry>
        <mesh filename="file:///root/ws/src/tesseract_planning/resources/meshes/workstation/visual/shoulder.dae"/>
      </geometry>
      <material name="LightGrey">
        <color rgba="0.7 0.7 0.7 1.0"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
      <geometry>
        <mesh filename="file:///root/ws/src/tesseract_planning/resources/meshes/workstation/collision/shoulder.stl"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="3.7"/>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <inertia ixx="0.014972352344389999" ixy="0.0" ixz="0.0" iyy="0.014972352344389999" iyz="0.0" izz="0.01040625"/>
    </inertial>
  </link>
  <link name="upper_arm_link">
    <visual>
      <origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.13585"/>
      <geometry>
        <mesh filename="file:///root/ws/src/tesseract_planning/resources/meshes/workstation/visual/upperarm.dae"/>
      </geometry>
      <material name="LightGrey">
        <color rgba="0.7 0.7 0.7 1.0"/>
      </material>
    </visual>
    <collision>
      <origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.13585"/>
      <geometry>
        <mesh filename="file:///root/ws/src/tesseract_planning/resources/meshes/workstation/collision/upperarm.stl"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="8.393"/>
      <origin rpy="0 1.5707963267948966 0" xyz="-0.2125 0.0 0.136"/>
      <inertia ixx="0.1338857818623325" ixy="0.0" ixz="0.0" iyy="0.1338857818623325" iyz="0.0" izz="0.0151074"/>
    </inertial>
  </link>
  <link name="forearm_link">
    <visual>
      <origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.0165"/>
      <geometry>
        <mesh filename="file:///root/ws/src/tesseract_planning/resources/meshes/workstation/visual/forearm.dae"/>
      </geometry>
      <material name="LightGrey">
        <color rgba="0.7 0.7 0.7 1.0"/>
      </material>
    </visual>
    <collision>
      <origin rpy="1.5707963267948966 0 -1.5707963267948966" xyz="0 0 0.0165"/>
      <geometry>
        <mesh filename="file:///root/ws/src/tesseract_planning/resources/meshes/workstation/collision/forearm.stl"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="2.275"/>
      <origin rpy="0 1.5707963267948966 0" xyz="-0.196125 0.0 0.0165"/>
      <inertia ixx="0.03121679102890359" ixy="0.0" ixz="0.0" iyy="0.03121679102890359" iyz="0.0" izz="0.004095"/>
    </inertial>
  </link>
  <link name="wrist_1_link">
    <visual>
      <origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.093"/>
      <geometry>
        <mesh filename="file:///root/ws/src/tesseract_planning/resources/meshes/workstation/visual/wrist1.dae"/>
      </geometry>
      <material name="LightGrey">
        <color rgba="0.7 0.7 0.7 1.0"/>
      </material>
    </visual>
    <collision>
      <origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.093"/>
      <geometry>
        <mesh filename="file:///root/ws/src/tesseract_planning/resources/meshes/workstation/collision/wrist1.stl"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="1.219"/>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <inertia ixx="0.0020138887777775" ixy="0.0" ixz="0.0" iyy="0.0020138887777775" iyz="0.0" izz="0.0021942"/>
    </inertial>
  </link>
  <link name="wrist_2_link">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 -0.095"/>
      <geometry>
        <mesh filename="file:///root/ws/src/tesseract_planning/resources/meshes/workstation/visual/wrist2.dae"/>
      </geometry>
      <material name="LightGrey">
        <color rgba="0.7 0.7 0.7 1"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 -0.095"/>
      <geometry>
        <mesh filename="file:///root/ws/src/tesseract_planning/resources/meshes/workstation/collision/wrist2.stl"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="1.219"/>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <inertia ixx="0.0020138887777775" ixy="0.0" ixz="0.0" iyy="0.0020138887777775" iyz="0.0" izz="0.0021942"/>
    </inertial>
  </link>
  <link name="wrist_3_link">
    <visual>
      <origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.0818"/>
      <geometry>
        <mesh filename="file:///root/ws/src/tesseract_planning/resources/meshes/workstation/visual/wrist3.dae"/>
      </geometry>
      <material name="LightGrey">
        <color rgba="0.7 0.7 0.7 1.0"/>
      </material>
    </visual>
    <collision>
      <origin rpy="1.5707963267948966 0 0" xyz="0 0 -0.0818"/>
      <geometry>
        <mesh filename="file:///root/ws/src/tesseract_planning/resources/meshes/workstation/collision/wrist3.stl"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.1879"/>
      <origin rpy="0 0 0" xyz="0.0 0.0 -0.01525"/>
      <inertia ixx="8.062472608343e-05" ixy="0.0" ixz="0.0" iyy="8.062472608343e-05" iyz="0.0" izz="0.0001321171875"/>
    </inertial>
  </link>
  <!-- Joints: -->
  <joint name="base_link-base_link_inertia" type="fixed">
    <parent link="base_link"/>
    <child link="base_link_inertia"/>
    <!-- 'base_link' is REP-103 aligned (so X+ forward), while the internal
           frames of the robot/controller have X+ pointing backwards.
           Use the joint between 'base_link' and 'base_link_inertia' (a dummy
           link/frame) to introduce the necessary rotation over Z (of pi rad).
      -->
    <origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
  </joint>
  <joint name="shoulder_pan_joint" type="revolute">
    <parent link="base_link_inertia"/>
    <child link="shoulder_link"/>
    <origin rpy="0 0 0" xyz="0 0 0.089159"/>
    <axis xyz="0 0 1"/>
    <limit effort="150.0" lower="-6.2832" upper="6.2832" velocity="3.1416"/>
    <dynamics damping="0" friction="0"/>
  </joint>
  <joint name="shoulder_lift_joint" type="revolute">
    <parent link="shoulder_link"/>
    <child link="upper_arm_link"/>
    <origin rpy="1.570796327 0 0" xyz="0 0 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="150.0" lower="-6.2832" upper="6.2832" velocity="3.1416"/>
    <dynamics damping="0" friction="0"/>
  </joint>
  <joint name="elbow_joint" type="revolute">
    <parent link="upper_arm_link"/>
    <child link="forearm_link"/>
    <origin rpy="0 0 0" xyz="-0.425 0 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="150.0" lower="-3.1416" upper="3.1416" velocity="3.1416"/>
    <dynamics damping="0" friction="0"/>
  </joint>
  <joint name="wrist_1_joint" type="revolute">
    <parent link="forearm_link"/>
    <child link="wrist_1_link"/>
    <origin rpy="0 0 0" xyz="-0.39225 0 0.10915"/>
    <axis xyz="0 0 1"/>
    <limit effort="28.0" lower="-6.2832" upper="6.2832" velocity="3.1416"/>
    <dynamics damping="0" friction="0"/>
  </joint>
  <joint name="wrist_2_joint" type="revolute">
    <parent link="wrist_1_link"/>
    <child link="wrist_2_link"/>
    <origin rpy="1.570796327 0 0" xyz="0 -0.09465 -1.941303950897609e-11"/>
    <axis xyz="0 0 1"/>
    <limit effort="28.0" lower="-6.2832" upper="6.2832" velocity="3.1416"/>
    <dynamics damping="0" friction="0"/>
  </joint>
  <joint name="wrist_3_joint" type="revolute">
    <parent link="wrist_2_link"/>
    <child link="wrist_3_link"/>
    <origin rpy="1.570796326589793 3.141592653589793 3.141592653589793" xyz="0 0.0823 -1.688001216681175e-11"/>
    <axis xyz="0 0 1"/>
    <limit effort="28.0" lower="-6.2832" upper="6.2832" velocity="3.1416"/>
    <dynamics damping="0" friction="0"/>
  </joint>
  <!-- ROS-Industrial 'base' frame - base_link to UR 'Base' Coordinates transform -->
  <link name="base"/>
  <joint name="base_link-base_fixed_joint" type="fixed">
    <!-- Note the rotation over Z of pi radians - as base_link is REP-103
           aligned (i.e., has X+ forward, Y+ left and Z+ up), this is needed
           to correctly align 'base' with the 'Base' coordinate system of
           the UR controller.
      -->
    <origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
    <parent link="base_link"/>
    <child link="base"/>
  </joint>
  <!-- ROS-Industrial 'flange' frame - attachment point for EEF models -->
  <link name="flange"/>
  <joint name="wrist_3-flange" type="fixed">
    <parent link="wrist_3_link"/>
    <child link="flange"/>
    <origin rpy="0 -1.5707963267948966 -1.5707963267948966" xyz="0 0 0"/>
  </joint>
  <!-- End Effector link joint with the flange. Link described in the end_effector.urdf.xacro file -->
  <joint name="eef_flange_joint" type="fixed">
    <parent link="flange"/>
    <child link="ur10_adaptor"/>
    <origin rpy="0.0 1.5707963267948966 0.0" xyz="0.0 0.0 0.0"/>
  </joint>
  <!-- ROS-Industrial 'tool0' frame - all-zeros tool frame -->
  <!-- default toolframe - X+ left, Y+ up, Z+ front -->
  <link name="tool0"/>
  <joint name="burr-tool0" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 0.02223"/>
    <parent link="burr"/>
    <child link="tool0"/>
  </joint>
  <joint name="ur_joint" type="fixed">
    <parent link="table__link"/>
    <child link="base_link"/>
    <origin rpy="0.0 0.0 0.0" xyz="0.51 0.0 0.88"/>
  </joint>
</robot>

Custom srdf:

<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- |    This document was autogenerated by xacro from ur.srdf.xacro                  | -->
<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
<!-- =================================================================================== -->
<robot name="ur5">
  <!--GROUPS - Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
  <!--LINKS - When a link is specified, the parent joint of that link (if it exists) is automatically included-->
  <!--JOINTS - When a joint is specified, the child link of that joint (which will always exist) is automatically included-->
  <!--CHAINS - When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
  <!--SUBGROUPS - Groups can also be formed by referencing to already defined group names-->
  <group name="manipulator">
    <chain base_link="base_link" tip_link="tool0"/>
  </group>
  <!--GROUP STATES - Purpose - Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
  <group_state group="manipulator" name="home">
    <joint name="elbow_joint" value="0"/>
    <joint name="shoulder_lift_joint" value="-1.5707"/>
    <joint name="shoulder_pan_joint" value="0"/>
    <joint name="wrist_1_joint" value="0"/>
    <joint name="wrist_2_joint" value="0"/>
    <joint name="wrist_3_joint" value="0"/>
  </group_state>
  <group_state group="manipulator" name="up">
    <joint name="elbow_joint" value="0"/>
    <joint name="shoulder_lift_joint" value="-1.5707"/>
    <joint name="shoulder_pan_joint" value="0"/>
    <joint name="wrist_1_joint" value="-1.5707"/>
    <joint name="wrist_2_joint" value="0"/>
    <joint name="wrist_3_joint" value="0"/>
  </group_state>
  <group_state group="manipulator" name="test_configuration">
    <joint name="elbow_joint" value="1.4"/>
    <joint name="shoulder_lift_joint" value="-1.62"/>
    <joint name="shoulder_pan_joint" value="1.54"/>
    <joint name="wrist_1_joint" value="-1.2"/>
    <joint name="wrist_2_joint" value="-1.6"/>
    <joint name="wrist_3_joint" value="-0.11"/>
  </group_state>
  <!--END EFFECTOR - Purpose - Represent information about an end effector.-->
  <group_tcps group="manipulator">
    <tcp name="tool0" xyz="0 0 0.2" wxyz="1 0 0 0"/>
  </group_tcps>
    <!--Groups kinematic plugins -->
  <kinematics_plugin_config filename="package://tesseract_planning/resources/urdf/abb_irb2400_plugins.yaml"/>
    <!--Contact Managers plugins -->
  <contact_managers_plugin_config filename="package://tesseract_planning/resources/urdf/contact_manager_plugins.yaml"/>

  <!--VIRTUAL JOINT - Purpose - this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)-->
  <virtual_joint child_link="base_link" name="fixed_base" parent_frame="world" type="fixed"/>
  <!--DISABLE COLLISIONS - By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
  <disable_collisions link1="base_link" link2="base_link_inertia" reason="Adjacent"/>
  <disable_collisions link1="base_link_inertia" link2="shoulder_link" reason="Adjacent"/>

  <!--Added-->
  <disable_collisions link1="base_link_inertia" link2="upper_arm_link" reason="Never"/>
  <disable_collisions link1="ATI" link2="ur10_adaptor2" reason="Never"/>
  <disable_collisions link1="ur10_adaptor2" link2="ur10_adaptor" reason="Never"/>
  <disable_collisions link1="wrist_3_link" link2="ur10_adaptor" reason="Never"/>
  <!--Added end-->
  <disable_collisions link1="tool0" link2="wrist_1_link" reason="Never"/>
  <disable_collisions link1="tool0" link2="wrist_2_link" reason="Never"/>
  <disable_collisions link1="tool0" link2="wrist_3_link" reason="Adjacent"/>
  <disable_collisions link1="forearm_link" link2="upper_arm_link" reason="Adjacent"/>
  <disable_collisions link1="forearm_link" link2="wrist_1_link" reason="Adjacent"/>
  <disable_collisions link1="shoulder_link" link2="upper_arm_link" reason="Adjacent"/>
  <disable_collisions link1="wrist_1_link" link2="wrist_2_link" reason="Adjacent"/>
  <disable_collisions link1="wrist_1_link" link2="wrist_3_link" reason="Never"/>
  <disable_collisions link1="wrist_2_link" link2="wrist_3_link" reason="Adjacent"/>
</robot>

Thank you for reading this!
Best regards,
Kai

[marip8]

task_composer_filename = os.environ["TESSERACT_TASK_COMPOSER_CONFIG_FILE"]

Can you post the contents of your task composer config file? There are a few places where you can tell the planners to "format_result_as_input" (which makes them return original types of waypoints (i.e. JointWaypoint and CartesianWaypoint) rather than StateWaypoints), namely in a motion planner task here via its configuration.

Changing your URDF shouldn't have any affect on the output of the motion planners. There may also be a bug in the Python bindings that's not handling the format_result_as_input correctly

[kaiConnected]

Thanks for the quick reply!
I am using the composer config that can be found in "tesseract_planning/tesseract_task_composer/config/task_composer_plugins_no_trajopt_ifopt.yaml". It has following content:

task_composer_plugins:
  search_paths:
    - /usr/local/lib
  search_libraries:
    - tesseract_task_composer_factories
    - tesseract_task_composer_planning_factories
    - tesseract_task_composer_taskflow_factories
  executors:
    default: TaskflowExecutor
    plugins:
      TaskflowExecutor:
        class: TaskflowTaskComposerExecutorFactory
        config:
          threads: 5
  tasks:
    plugins:
      DescartesFTask:
        class: PipelineTaskFactory
        config:
          conditional: true
          inputs: [input_data]
          outputs: [output_data]
          nodes:
            DoneTask:
              class: DoneTaskFactory
              config:
                conditional: false
            ErrorTask:
              class: ErrorTaskFactory
              config:
                conditional: false
            MinLengthTask:
              class: MinLengthTaskFactory
              config:
                conditional: true
                inputs: [input_data]
                outputs: [output_data]
            DescartesMotionPlannerTask:
              class: DescartesFMotionPlannerTaskFactory
              config:
                conditional: true
                inputs: [output_data]
                outputs: [output_data]
                format_result_as_input: false
            DiscreteContactCheckTask:
              class: DiscreteContactCheckTaskFactory
              config:
                conditional: true
                inputs: [output_data]
            IterativeSplineParameterizationTask:
              class: IterativeSplineParameterizationTaskFactory
              config:
                conditional: true
                inputs: [output_data]
                outputs: [output_data]
          edges:
            - source: MinLengthTask
              destinations: [ErrorTask, DescartesMotionPlannerTask]
            - source: DescartesMotionPlannerTask
              destinations: [ErrorTask, DiscreteContactCheckTask]
            - source: DiscreteContactCheckTask
              destinations: [ErrorTask, IterativeSplineParameterizationTask]
            - source: IterativeSplineParameterizationTask
              destinations: [ErrorTask, DoneTask]
          terminals: [ErrorTask, DoneTask]
      DescartesFPipeline:
        class: PipelineTaskFactory
        config:
          conditional: true
          inputs: [input_data]
          outputs: [output_data]
          nodes:
            DoneTask:
              class: DoneTaskFactory
              config:
                conditional: false
            AbortTask:
              class: AbortTaskFactory
              config:
                conditional: false
            MotionPlanningTask:
              task: DescartesFTask
              config:
                conditional: true
          edges:
            - source: MotionPlanningTask
              destinations: [AbortTask, DoneTask]
          terminals: [AbortTask, DoneTask]
      DescartesDTask:
        class: PipelineTaskFactory
        config:
          conditional: true
          inputs: [input_data]
          outputs: [output_data]
          nodes:
            DoneTask:
              class: DoneTaskFactory
              config:
                conditional: false
            ErrorTask:
              class: ErrorTaskFactory
              config:
                conditional: false
            MinLengthTask:
              class: MinLengthTaskFactory
              config:
                conditional: true
                inputs: [input_data]
                outputs: [output_data]
            DescartesMotionPlannerTask:
              class: DescartesDMotionPlannerTaskFactory
              config:
                conditional: true
                inputs: [output_data]
                outputs: [output_data]
                format_result_as_input: false
            DiscreteContactCheckTask:
              class: DiscreteContactCheckTaskFactory
              config:
                conditional: true
                inputs: [output_data]
            IterativeSplineParameterizationTask:
              class: IterativeSplineParameterizationTaskFactory
              config:
                conditional: true
                inputs: [output_data]
                outputs: [output_data]
          edges:
            - source: MinLengthTask
              destinations: [ErrorTask, DescartesMotionPlannerTask]
            - source: DescartesMotionPlannerTask
              destinations: [ErrorTask, DiscreteContactCheckTask]
            - source: DiscreteContactCheckTask
              destinations: [ErrorTask, IterativeSplineParameterizationTask]
            - source: IterativeSplineParameterizationTask
              destinations: [ErrorTask, DoneTask]
          terminals: [ErrorTask, DoneTask]
      DescartesDPipeline:
        class: PipelineTaskFactory
        config:
          conditional: true
          inputs: [input_data]
          outputs: [output_data]
          nodes:
            DoneTask:
              class: DoneTaskFactory
              config:
                conditional: false
            AbortTask:
              class: AbortTaskFactory
              config:
                conditional: false
            MotionPlanningTask:
              task: DescartesDTask
              config:
                conditional: true
          edges:
            - source: MotionPlanningTask
              destinations: [AbortTask, DoneTask]
          terminals: [AbortTask, DoneTask]
      DescartesFNPCTask:
        class: PipelineTaskFactory
        config:
          conditional: true
          inputs: [input_data]
          outputs: [output_data]
          nodes:
            DoneTask:
              class: DoneTaskFactory
              config:
                conditional: false
            ErrorTask:
              class: ErrorTaskFactory
              config:
                conditional: false
            MinLengthTask:
              class: MinLengthTaskFactory
              config:
                conditional: true
                inputs: [input_data]
                outputs: [output_data]
            DescartesMotionPlannerTask:
              class: DescartesFMotionPlannerTaskFactory
              config:
                conditional: true
                inputs: [output_data]
                outputs: [output_data]
                format_result_as_input: false
            IterativeSplineParameterizationTask:
              class: IterativeSplineParameterizationTaskFactory
              config:
                conditional: true
                inputs: [output_data]
                outputs: [output_data]
          edges:
            - source: MinLengthTask
              destinations: [ErrorTask, DescartesMotionPlannerTask]
            - source: DescartesMotionPlannerTask
              destinations: [ErrorTask, IterativeSplineParameterizationTask]
            - source: IterativeSplineParameterizationTask
              destinations: [ErrorTask, DoneTask]
          terminals: [ErrorTask, DoneTask]
      DescartesFNPCPipeline:
        class: PipelineTaskFactory
        config:
          conditional: true
          inputs: [input_data]
          outputs: [output_data]
          nodes:
            DoneTask:
              class: DoneTaskFactory
              config:
                conditional: false
            AbortTask:
              class: AbortTaskFactory
              config:
                conditional: false
            MotionPlanningTask:
              task: DescartesFNPCTask
              config:
                conditional: true
          edges:
            - source: MotionPlanningTask
              destinations: [AbortTask, DoneTask]
          terminals: [AbortTask, DoneTask]
      DescartesDNPCTask:
        class: PipelineTaskFactory
        config:
          conditional: true
          inputs: [input_data]
          outputs: [output_data]
          nodes:
            DoneTask:
              class: DoneTaskFactory
              config:
                conditional: false
            ErrorTask:
              class: ErrorTaskFactory
              config:
                conditional: false
            MinLengthTask:
              class: MinLengthTaskFactory
              config:
                conditional: true
                inputs: [input_data]
                outputs: [output_data]
            DescartesMotionPlannerTask:
              class: DescartesDMotionPlannerTaskFactory
              config:
                conditional: true
                inputs: [output_data]
                outputs: [output_data]
                format_result_as_input: false
            IterativeSplineParameterizationTask:
              class: IterativeSplineParameterizationTaskFactory
              config:
                conditional: true
                inputs: [output_data]
                outputs: [output_data]
          edges:
            - source: MinLengthTask
              destinations: [ErrorTask, DescartesMotionPlannerTask]
            - source: DescartesMotionPlannerTask
              destinations: [ErrorTask, IterativeSplineParameterizationTask]
            - source: IterativeSplineParameterizationTask
              destinations: [ErrorTask, DoneTask]
          terminals: [ErrorTask, DoneTask]
      DescartesDNPCPipeline:
        class: PipelineTaskFactory
        config:
          conditional: true
          inputs: [input_data]
          outputs: [output_data]
          nodes:
            DoneTask:
              class: DoneTaskFactory
              config:
                conditional: false
            AbortTask:
              class: AbortTaskFactory
              config:
                conditional: false
            MotionPlanningTask:
              task: DescartesDNPCTask
              config:
                conditional: true
          edges:
            - source: MotionPlanningTask
              destinations: [AbortTask, DoneTask]
          terminals: [AbortTask, DoneTask]
      OMPLTask:
        class: PipelineTaskFactory
        config:
          conditional: true
          inputs: [input_data]
          outputs: [output_data]
          nodes:
            DoneTask:
              class: DoneTaskFactory
              config:
                conditional: false
            ErrorTask:
              class: ErrorTaskFactory
              config:
                conditional: false
            MinLengthTask:
              class: MinLengthTaskFactory
              config:
                conditional: true
                inputs: [input_data]
                outputs: [output_data]
            OMPLMotionPlannerTask:
              class: OMPLMotionPlannerTaskFactory
              config:
                conditional: true
                inputs: [output_data]
                outputs: [output_data]
                format_result_as_input: false
            DiscreteContactCheckTask:
              class: DiscreteContactCheckTaskFactory
              config:
                conditional: true
                inputs: [output_data]
            IterativeSplineParameterizationTask:
              class: IterativeSplineParameterizationTaskFactory
              config:
                conditional: true
                inputs: [output_data]
                outputs: [output_data]
          edges:
            - source: MinLengthTask
              destinations: [ErrorTask, OMPLMotionPlannerTask]
            - source: OMPLMotionPlannerTask
              destinations: [ErrorTask, DiscreteContactCheckTask]
            - source: DiscreteContactCheckTask
              destinations: [ErrorTask, IterativeSplineParameterizationTask]
            - source: IterativeSplineParameterizationTask
              destinations: [ErrorTask, DoneTask]
          terminals: [ErrorTask, DoneTask]
      OMPLPipeline:
        class: PipelineTaskFactory
        config:
          conditional: true
          inputs: [input_data]
          outputs: [output_data]
          nodes:
            DoneTask:
              class: DoneTaskFactory
              config:
                conditional: false
            AbortTask:
              class: AbortTaskFactory
              config:
                conditional: false
            MotionPlanningTask:
              task: OMPLTask
              config:
                conditional: true
          edges:
            - source: MotionPlanningTask
              destinations: [AbortTask, DoneTask]
          terminals: [AbortTask, DoneTask]
      TrajOptTask:
        class: PipelineTaskFactory
        config:
          conditional: true
          inputs: [input_data]
          outputs: [output_data]
          nodes:
            DoneTask:
              class: DoneTaskFactory
              config:
                conditional: false
            ErrorTask:
              class: ErrorTaskFactory
              config:
                conditional: false
            MinLengthTask:
              class: MinLengthTaskFactory
              config:
                conditional: true
                inputs: [input_data]
                outputs: [output_data]
            TrajOptMotionPlannerTask:
              class: TrajOptMotionPlannerTaskFactory
              config:
                conditional: true
                inputs: [output_data]
                outputs: [output_data]
                format_result_as_input: false
            DiscreteContactCheckTask:
              class: DiscreteContactCheckTaskFactory
              config:
                conditional: true
                inputs: [output_data]
            IterativeSplineParameterizationTask:
              class: IterativeSplineParameterizationTaskFactory
              config:
                conditional: true
                inputs: [output_data]
                outputs: [output_data]
          edges:
            - source: MinLengthTask
              destinations: [ErrorTask, TrajOptMotionPlannerTask]
            - source: TrajOptMotionPlannerTask
              destinations: [ErrorTask, DiscreteContactCheckTask]
            - source: DiscreteContactCheckTask
              destinations: [ErrorTask, IterativeSplineParameterizationTask]
            - source: IterativeSplineParameterizationTask
              destinations: [ErrorTask, DoneTask]
          terminals: [ErrorTask, DoneTask]
      TrajOptPipeline:
        class: PipelineTaskFactory
        config:
          conditional: true
          inputs: [input_data]
          outputs: [output_data]
          nodes:
            DoneTask:
              class: DoneTaskFactory
              config:
                conditional: false
            AbortTask:
              class: AbortTaskFactory
              config:
                conditional: false
            MotionPlanningTask:
              task: TrajOptTask
              config:
                conditional: true
          edges:
            - source: MotionPlanningTask
              destinations: [AbortTask, DoneTask]
          terminals: [AbortTask, DoneTask]
      CartesianTask:
        class: PipelineTaskFactory
        config:
          conditional: true
          inputs: [input_data]
          outputs: [output_data]
          nodes:
            DoneTask:
              class: DoneTaskFactory
              config:
                conditional: false
            ErrorTask:
              class: ErrorTaskFactory
              config:
                conditional: false
            MinLengthTask:
              class: MinLengthTaskFactory
              config:
                conditional: true
                inputs: [input_data]
                outputs: [output_data]
            DescartesMotionPlannerTask:
              class: DescartesFMotionPlannerTaskFactory
              config:
                conditional: true
                inputs: [output_data]
                outputs: [output_data]
                format_result_as_input: true
            TrajOptMotionPlannerTask:
              class: TrajOptMotionPlannerTaskFactory
              config:
                conditional: true
                inputs: [output_data]
                outputs: [output_data]
                format_result_as_input: false
            DiscreteContactCheckTask:
              class: DiscreteContactCheckTaskFactory
              config:
                conditional: true
                inputs: [output_data]
            IterativeSplineParameterizationTask:
              class: IterativeSplineParameterizationTaskFactory
              config:
                conditional: true
                inputs: [output_data]
                outputs: [output_data]
          edges:
            - source: MinLengthTask
              destinations: [ErrorTask, DescartesMotionPlannerTask]
            - source: DescartesMotionPlannerTask
              destinations: [ErrorTask, TrajOptMotionPlannerTask]
            - source: TrajOptMotionPlannerTask
              destinations: [ErrorTask, DiscreteContactCheckTask]
            - source: DiscreteContactCheckTask
              destinations: [ErrorTask, IterativeSplineParameterizationTask]
            - source: IterativeSplineParameterizationTask
              destinations: [ErrorTask, DoneTask]
          terminals: [ErrorTask, DoneTask]
      CartesianPipeline:
        class: PipelineTaskFactory
        config:
          conditional: true
          inputs: [input_data]
          outputs: [output_data]
          nodes:
            DoneTask:
              class: DoneTaskFactory
              config:
                conditional: false
            AbortTask:
              class: AbortTaskFactory
              config:
                conditional: false
            MotionPlanningTask:
              task: CartesianTask
              config:
                conditional: true
          edges:
            - source: MotionPlanningTask
              destinations: [AbortTask, DoneTask]
          terminals: [AbortTask, DoneTask]
      FreespaceTask:
        class: PipelineTaskFactory
        config:
          conditional: true
          inputs: [input_data]
          outputs: [output_data]
          nodes:
            DoneTask:
              class: DoneTaskFactory
              config:
                conditional: false
            ErrorTask:
              class: ErrorTaskFactory
              config:
                conditional: false
            MinLengthTask:
              class: MinLengthTaskFactory
              config:
                conditional: true
                inputs: [input_data]
                outputs: [output_data]
            OMPLMotionPlannerTask:
              class: OMPLMotionPlannerTaskFactory
              config:
                conditional: true
                inputs: [output_data]
                outputs: [output_data]
                format_result_as_input: true
            TrajOptMotionPlannerTask:
              class: TrajOptMotionPlannerTaskFactory
              config:
                conditional: true
                inputs: [output_data]
                outputs: [output_data]
                format_result_as_input: false
            DiscreteContactCheckTask:
              class: DiscreteContactCheckTaskFactory
              config:
                conditional: true
                inputs: [output_data]
            IterativeSplineParameterizationTask:
              class: IterativeSplineParameterizationTaskFactory
              config:
                conditional: true
                inputs: [output_data]
                outputs: [output_data]
          edges:
            - source: MinLengthTask
              destinations: [ErrorTask, OMPLMotionPlannerTask]
            - source: OMPLMotionPlannerTask
              destinations: [ErrorTask, TrajOptMotionPlannerTask]
            - source: TrajOptMotionPlannerTask
              destinations: [ErrorTask, DiscreteContactCheckTask]
            - source: DiscreteContactCheckTask
              destinations: [ErrorTask, IterativeSplineParameterizationTask]
            - source: IterativeSplineParameterizationTask
              destinations: [ErrorTask, DoneTask]
          terminals: [ErrorTask, DoneTask]
      FreespacePipeline:
        class: PipelineTaskFactory
        config:
          conditional: true
          inputs: [input_data]
          outputs: [output_data]
          nodes:
            DoneTask:
              class: DoneTaskFactory
              config:
                conditional: false
            AbortTask:
              class: AbortTaskFactory
              config:
                conditional: false
            MotionPlanningTask:
              task: FreespaceTask
              config:
                conditional: true
          edges:
            - source: MotionPlanningTask
              destinations: [AbortTask, DoneTask]
          terminals: [AbortTask, DoneTask]
      RasterFtTask:
        class: PipelineTaskFactory
        config:
          conditional: true
          inputs: [input_data]
          outputs: [output_data]
          nodes:
            DoneTask:
              class: DoneTaskFactory
              config:
                conditional: false
            ErrorTask:
              class: ErrorTaskFactory
              config:
                conditional: false
            SimpleMotionPlannerTask:
              class: SimpleMotionPlannerTaskFactory
              config:
                conditional: true
                inputs: [input_data]
                outputs: [output_data]
                format_result_as_input: true
            RasterMotionTask:
              class: RasterMotionTaskFactory
              config:
                conditional: true
                inputs: [output_data]
                outputs: [output_data]
                freespace:
                  task: FreespacePipeline
                  config:
                    input_remapping:
                      input_data: output_data
                    input_indexing: [output_data]
                    output_indexing: [output_data]
                raster:
                  task: CartesianPipeline
                  config:
                    input_remapping:
                      input_data: output_data
                    input_indexing: [output_data]
                    output_indexing: [output_data]
                transition:
                  task: FreespacePipeline
                  config:
                    input_remapping:
                      input_data: output_data
                    input_indexing: [output_data]
                    output_indexing: [output_data]
          edges:
            - source: SimpleMotionPlannerTask
              destinations: [ErrorTask, RasterMotionTask]
            - source: RasterMotionTask
              destinations: [ErrorTask, DoneTask]
          terminals: [ErrorTask, DoneTask]
      RasterFtPipeline:
        class: PipelineTaskFactory
        config:
          conditional: true
          inputs: [input_data]
          outputs: [output_data]
          nodes:
            DoneTask:
              class: DoneTaskFactory
              config:
                conditional: false
            AbortTask:
              class: AbortTaskFactory
              config:
                conditional: false
            MotionPlanningTask:
              task: RasterFtTask
              config:
                conditional: true
          edges:
            - source: MotionPlanningTask
              destinations: [AbortTask, DoneTask]
          terminals: [AbortTask, DoneTask]
      RasterCtTask:
        class: PipelineTaskFactory
        config:
          conditional: true
          inputs: [input_data]
          outputs: [output_data]
          nodes:
            DoneTask:
              class: DoneTaskFactory
              config:
                conditional: false
            ErrorTask:
              class: ErrorTaskFactory
              config:
                conditional: false
            SimpleMotionPlannerTask:
              class: SimpleMotionPlannerTaskFactory
              config:
                conditional: true
                inputs: [input_data]
                outputs: [output_data]
                format_result_as_input: true
            RasterMotionTask:
              class: RasterMotionTaskFactory
              config:
                conditional: true
                inputs: [output_data]
                outputs: [output_data]
                freespace:
                  task: FreespacePipeline
                  config:
                    input_remapping:
                      input_data: output_data
                    input_indexing: [output_data]
                    output_indexing: [output_data]
                raster:
                  task: CartesianPipeline
                  config:
                    input_remapping:
                      input_data: output_data
                    input_indexing: [output_data]
                    output_indexing: [output_data]
                transition:
                  task: CartesianPipeline
                  config:
                    input_remapping:
                      input_data: output_data
                    input_indexing: [output_data]
                    output_indexing: [output_data]
          edges:
            - source: SimpleMotionPlannerTask
              destinations: [ErrorTask, RasterMotionTask]
            - source: RasterMotionTask
              destinations: [ErrorTask, DoneTask]
          terminals: [ErrorTask, DoneTask]
      RasterCtPipeline:
        class: PipelineTaskFactory
        config:
          conditional: true
          inputs: [input_data]
          outputs: [output_data]
          nodes:
            DoneTask:
              class: DoneTaskFactory
              config:
                conditional: false
            AbortTask:
              class: AbortTaskFactory
              config:
                conditional: false
            MotionPlanningTask:
              task: RasterCtTask
              config:
                conditional: true
          edges:
            - source: MotionPlanningTask
              destinations: [AbortTask, DoneTask]
          terminals: [AbortTask, DoneTask]
      RasterFtOnlyTask:
        class: PipelineTaskFactory
        config:
          conditional: true
          inputs: [input_data]
          outputs: [output_data]
          nodes:
            DoneTask:
              class: DoneTaskFactory
              config:
                conditional: false
            ErrorTask:
              class: ErrorTaskFactory
              config:
                conditional: false
            SimpleMotionPlannerTask:
              class: SimpleMotionPlannerTaskFactory
              config:
                conditional: true
                inputs: [input_data]
                outputs: [output_data]
                format_result_as_input: true
            RasterMotionTask:
              class: RasterOnlyMotionTaskFactory
              config:
                conditional: true
                inputs: [output_data]
                outputs: [output_data]
                raster:
                  task: CartesianPipeline
                  config:
                    input_remapping:
                      input_data: output_data
                    input_indexing: [output_data]
                    output_indexing: [output_data]
                transition:
                  task: FreespacePipeline
                  config:
                    input_remapping:
                      input_data: output_data
                    input_indexing: [output_data]
                    output_indexing: [output_data]
          edges:
            - source: SimpleMotionPlannerTask
              destinations: [ErrorTask, RasterMotionTask]
            - source: RasterMotionTask
              destinations: [ErrorTask, DoneTask]
          terminals: [ErrorTask, DoneTask]
      RasterFtOnlyPipeline:
        class: PipelineTaskFactory
        config:
          conditional: true
          inputs: [input_data]
          outputs: [output_data]
          nodes:
            DoneTask:
              class: DoneTaskFactory
              config:
                conditional: false
            AbortTask:
              class: AbortTaskFactory
              config:
                conditional: false
            MotionPlanningTask:
              task: RasterFtOnlyTask
              config:
                conditional: true
          edges:
            - source: MotionPlanningTask
              destinations: [AbortTask, DoneTask]
          terminals: [AbortTask, DoneTask]
      RasterCtOnlyTask:
        class: PipelineTaskFactory
        config:
          conditional: true
          inputs: [input_data]
          outputs: [output_data]
          nodes:
            DoneTask:
              class: DoneTaskFactory
              config:
                conditional: false
            ErrorTask:
              class: ErrorTaskFactory
              config:
                conditional: false
            SimpleMotionPlannerTask:
              class: SimpleMotionPlannerTaskFactory
              config:
                conditional: true
                inputs: [input_data]
                outputs: [output_data]
                format_result_as_input: true
            RasterMotionTask:
              class: RasterOnlyMotionTaskFactory
              config:
                conditional: true
                inputs: [output_data]
                outputs: [output_data]
                raster:
                  task: CartesianPipeline
                  config:
                    input_remapping:
                      input_data: output_data
                    input_indexing: [output_data]
                    output_indexing: [output_data]
                transition:
                  task: CartesianPipeline
                  config:
                    input_remapping:
                      input_data: output_data
                    input_indexing: [output_data]
                    output_indexing: [output_data]
          edges:
            - source: SimpleMotionPlannerTask
              destinations: [ErrorTask, RasterMotionTask]
            - source: RasterMotionTask
              destinations: [ErrorTask, DoneTask]
          terminals: [ErrorTask, DoneTask]
      RasterCtOnlyPipeline:
        class: PipelineTaskFactory
        config:
          conditional: true
          inputs: [input_data]
          outputs: [output_data]
          nodes:
            DoneTask:
              class: DoneTaskFactory
              config:
                conditional: false
            AbortTask:
              class: AbortTaskFactory
              config:
                conditional: false
            MotionPlanningTask:
              task: RasterCtOnlyTask
              config:
                conditional: true
          edges:
            - source: MotionPlanningTask
              destinations: [AbortTask, DoneTask]
          terminals: [AbortTask, DoneTask]
      RasterFtGlobalTask:
        class: PipelineTaskFactory
        config:
          conditional: true
          inputs: [input_data]
          outputs: [output_data]
          nodes:
            DoneTask:
              class: DoneTaskFactory
              config:
                conditional: false
            ErrorTask:
              class: ErrorTaskFactory
              config:
                conditional: false
            SimpleMotionPlannerTask:
              class: SimpleMotionPlannerTaskFactory
              config:
                conditional: true
                inputs: [input_data]
                outputs: [output_data]
                format_result_as_input: true
            DescartesMotionPlannerTask:
              class: DescartesFMotionPlannerTaskFactory
              config:
                conditional: true
                inputs: [output_data]
                outputs: [output_data]
                format_result_as_input: true
            RasterMotionTask:
              class: RasterMotionTaskFactory
              config:
                conditional: true
                inputs: [output_data]
                outputs: [output_data]
                freespace:
                  task: FreespacePipeline
                  config:
                    input_remapping:
                      input_data: output_data
                    input_indexing: [output_data]
                    output_indexing: [output_data]
                raster:
                  task: CartesianPipeline
                  config:
                    input_remapping:
                      input_data: output_data
                    input_indexing: [output_data]
                    output_indexing: [output_data]
                transition:
                  task: FreespacePipeline
                  config:
                    input_remapping:
                      input_data: output_data
                    input_indexing: [output_data]
                    output_indexing: [output_data]
          edges:
            - source: SimpleMotionPlannerTask
              destinations: [ErrorTask, DescartesMotionPlannerTask]
            - source: DescartesMotionPlannerTask
              destinations: [ErrorTask, RasterMotionTask]
            - source: RasterMotionTask
              destinations: [ErrorTask, DoneTask]
          terminals: [ErrorTask, DoneTask]
      RasterFtGlobalPipeline:
        class: PipelineTaskFactory
        config:
          conditional: true
          inputs: [input_data]
          outputs: [output_data]
          nodes:
            DoneTask:
              class: DoneTaskFactory
              config:
                conditional: false
            AbortTask:
              class: AbortTaskFactory
              config:
                conditional: false
            MotionPlanningTask:
              task: RasterFtGlobalTask
              config:
                conditional: true
          edges:
            - source: MotionPlanningTask
              destinations: [AbortTask, DoneTask]
          terminals: [AbortTask, DoneTask]
      RasterCtGlobalTask:
        class: PipelineTaskFactory
        config:
          conditional: true
          inputs: [input_data]
          outputs: [output_data]
          nodes:
            DoneTask:
              class: DoneTaskFactory
              config:
                conditional: false
            ErrorTask:
              class: ErrorTaskFactory
              config:
                conditional: false
            SimpleMotionPlannerTask:
              class: SimpleMotionPlannerTaskFactory
              config:
                conditional: true
                inputs: [input_data]
                outputs: [output_data]
                format_result_as_input: true
            DescartesMotionPlannerTask:
              class: DescartesFMotionPlannerTaskFactory
              config:
                conditional: true
                inputs: [output_data]
                outputs: [output_data]
                format_result_as_input: true
            RasterMotionTask:
              class: RasterMotionTaskFactory
              config:
                conditional: true
                inputs: [output_data]
                outputs: [output_data]
                freespace:
                  task: FreespacePipeline
                  config:
                    input_remapping:
                      input_data: output_data
                    input_indexing: [output_data]
                    output_indexing: [output_data]
                raster:
                  task: CartesianPipeline
                  config:
                    input_remapping:
                      input_data: output_data
                    input_indexing: [output_data]
                    output_indexing: [output_data]
                transition:
                  task: CartesianPipeline
                  config:
                    input_remapping:
                      input_data: output_data
                    input_indexing: [output_data]
                    output_indexing: [output_data]
          edges:
            - source: SimpleMotionPlannerTask
              destinations: [ErrorTask, DescartesMotionPlannerTask]
            - source: DescartesMotionPlannerTask
              destinations: [ErrorTask, RasterMotionTask]
            - source: RasterMotionTask
              destinations: [ErrorTask, DoneTask]
          terminals: [ErrorTask, DoneTask]
      RasterCtGlobalPipeline:
        class: PipelineTaskFactory
        config:
          conditional: true
          inputs: [input_data]
          outputs: [output_data]
          nodes:
            DoneTask:
              class: DoneTaskFactory
              config:
                conditional: false
            AbortTask:
              class: AbortTaskFactory
              config:
                conditional: false
            MotionPlanningTask:
              task: RasterCtGlobalTask
              config:
                conditional: true
          edges:
            - source: MotionPlanningTask
              destinations: [AbortTask, DoneTask]
          terminals: [AbortTask, DoneTask]
      RasterFtOnlyGlobalTask:
        class: PipelineTaskFactory
        config:
          conditional: true
          inputs: [input_data]
          outputs: [output_data]
          nodes:
            DoneTask:
              class: DoneTaskFactory
              config:
                conditional: false
            ErrorTask:
              class: ErrorTaskFactory
              config:
                conditional: false
            SimpleMotionPlannerTask:
              class: SimpleMotionPlannerTaskFactory
              config:
                conditional: true
                inputs: [input_data]
                outputs: [output_data]
                format_result_as_input: true
            DescartesMotionPlannerTask:
              class: DescartesFMotionPlannerTaskFactory
              config:
                conditional: true
                inputs: [output_data]
                outputs: [output_data]
                format_result_as_input: true
            RasterMotionTask:
              class: RasterOnlyMotionTaskFactory
              config:
                conditional: true
                inputs: [output_data]
                outputs: [output_data]
                raster:
                  task: CartesianPipeline
                  config:
                    input_remapping:
                      input_data: output_data
                    input_indexing: [output_data]
                    output_indexing: [output_data]
                transition:
                  task: FreespacePipeline
                  config:
                    input_remapping:
                      input_data: output_data
                    input_indexing: [output_data]
                    output_indexing: [output_data]
          edges:
            - source: SimpleMotionPlannerTask
              destinations: [ErrorTask, DescartesMotionPlannerTask]
            - source: DescartesMotionPlannerTask
              destinations: [ErrorTask, RasterMotionTask]
            - source: RasterMotionTask
              destinations: [ErrorTask, DoneTask]
          terminals: [ErrorTask, DoneTask]
      RasterFtOnlyGlobalPipeline:
        class: PipelineTaskFactory
        config:
          conditional: true
          inputs: [input_data]
          outputs: [output_data]
          nodes:
            DoneTask:
              class: DoneTaskFactory
              config:
                conditional: false
            AbortTask:
              class: AbortTaskFactory
              config:
                conditional: false
            MotionPlanningTask:
              task: RasterFtOnlyGlobalTask
              config:
                conditional: true
          edges:
            - source: MotionPlanningTask
              destinations: [AbortTask, DoneTask]
          terminals: [AbortTask, DoneTask]
      RasterCtOnlyGlobalTask:
        class: PipelineTaskFactory
        config:
          conditional: true
          inputs: [input_data]
          outputs: [output_data]
          nodes:
            DoneTask:
              class: DoneTaskFactory
              config:
                conditional: false
            ErrorTask:
              class: ErrorTaskFactory
              config:
                conditional: false
            SimpleMotionPlannerTask:
              class: SimpleMotionPlannerTaskFactory
              config:
                conditional: true
                inputs: [input_data]
                outputs: [output_data]
                format_result_as_input: true
            DescartesMotionPlannerTask:
              class: DescartesFMotionPlannerTaskFactory
              config:
                conditional: true
                inputs: [output_data]
                outputs: [output_data]
                format_result_as_input: true
            RasterMotionTask:
              class: RasterOnlyMotionTaskFactory
              config:
                conditional: true
                inputs: [output_data]
                outputs: [output_data]
                raster:
                  task: CartesianPipeline
                  config:
                    input_remapping:
                      input_data: output_data
                    input_indexing: [output_data]
                    output_indexing: [output_data]
                transition:
                  task: CartesianPipeline
                  config:
                    input_remapping:
                      input_data: output_data
                    input_indexing: [output_data]
                    output_indexing: [output_data]
          edges:
            - source: SimpleMotionPlannerTask
              destinations: [ErrorTask, DescartesMotionPlannerTask]
            - source: DescartesMotionPlannerTask
              destinations: [ErrorTask, RasterMotionTask]
            - source: RasterMotionTask
              destinations: [ErrorTask, DoneTask]
          terminals: [ErrorTask, DoneTask]
      RasterCtOnlyGlobalPipeline:
        class: PipelineTaskFactory
        config:
          conditional: true
          inputs: [input_data]
          outputs: [output_data]
          nodes:
            DoneTask:
              class: DoneTaskFactory
              config:
                conditional: false
            AbortTask:
              class: AbortTaskFactory
              config:
                conditional: false
            MotionPlanningTask:
              task: RasterCtOnlyGlobalTask
              config:
                conditional: true
          edges:
            - source: MotionPlanningTask
              destinations: [AbortTask, DoneTask]
          terminals: [AbortTask, DoneTask]

But to be honest I am not sure which one of them I am using.

[kaiConnected]

I tried adding the "format_result_as_input" statement, but sadly that did not have any visible effect.

[Levi-Armstrong]

You are using the FreespacePipeline task = factory.createTaskComposerNode("FreespacePipeline")

[Levi-Armstrong]

Looking at your code you are not checking if it was successful so I assume it is failing and you are just getting the output of the simple planner which would be joint waypoints.

[kaiConnected]

Thanks for the hint but sadly that does not seem to be the problem. I just checked, the behavior you described is true for the abb urdf, there it gives me JointWaypoints when no solution is found and StateWaypoints when it was successful. But for the custom urdf it gives me JointWaypoints even in those cases that succeed.

[Levi-Armstrong]

The urdf should have no impact on the behavior of the task_composer pipeline in this regard, so I am not sure. Have you tried using the c++ version?

[kaiConnected]

I have not yet tried the c++ version.
While I still don't know what the error in my code was, I found a completely different solution that works for me.

Thank you for your time.

[Levi-Armstrong]

What did you change?