tesseract-robotics / tesseract

Motion Planning Environment

Tesseract

Platform	CI Status
Linux (Focal)
Windows
Lint (Clang-Format)
Lint (CMake-Format)
Lint (Clang-Tidy)
Lint (CodeCov)

The planning framework (Tesseract) was designed to be light weight, limiting the number of dependencies, mainly only using standard libraries like, eigen, boost, orocos and to the packages below. The core packages are ROS agnostic and have full python support.

Dependencies

Tesseract Setup Wizard and Visualization Tools

Python Installation

A self contained Tesseract Python package is available on PyPI for Windows 10+, Ubuntu 20.04, and Ubuntu 22.04. This package contains Tesseract, Tesseract Planning, and all dependencies in the single package. Packages are available for Python 3.7 - 3.10.

To install on Windows:

python -m pip install tesseract-robotics tesseract-robotics-viewer

To install on Ubuntu 20.04 and Ubuntu 22.04:

sudo apt install python3-pip python3-numpy
# The supplied version of pip on Ubuntu 20.04 is too old for manylinux_2_31, upgrade pip
python3 -m pip install -U pip
python3 -m pip install --user  tesseract_robotics tesseract_robotics_viewer

Tesseract Core Packages

• tesseract – This is the main class that manages the major component Environment, Forward Kinematics, Inverse Kinematics and loading from various data.
• tesseract_command_language – This package contains a generic command language to support motion and process planning similar to industrial teach pendants
• tesseract_collision – This package contains a common interface for collision checking prividing several implementation of a Bullet collision library and FCL collision library. It includes both continuous and discrete collision checking for convex-convex, convex-concave and concave-concave shapes.
• tesseract_common – This package contains common functionality needed by the majority of the packages.
• tesseract_environment – This package contains the Tesseract Environment which provides functionality to add,remove,move and modify links and joint. It also manages adding object to the contact managers and provides the ability.
• tesseract_geometry – This package contains geometry types used by Tesseract including primitive shapes, mesh, convex hull mesh, octomap and signed distance field.
• tesseract_kinematics – This package contains a common interface for Forward and Inverse kinematics for Chains, Trees and Graphs including implementation using KDL and OPW Kinematics.
• tesseract_scene_graph – This package contains the scene graph which is the data structure used to manage the connectivity of objects in the environment. It inherits from boost graph and provides addition functionality for adding,removing and modifying Links and Joints along with search implementation.
• tesseract_support – This package contains support data used for unit tests and examples throughout Tesseract.
• tesseract_visualization – This package contains visualization utilities and libraries.

Related Repositories

• Tesseract Planning
• Tesseract Python
• Tesseract ROS
• Tesseract Documentation

Documentation

• Wiki
• Doxygen
• Benchmark

Evolution

How to create:

• Create Video: gource -1280x720 -seconds-per-day 0.2 --auto-skip-seconds 0.2 --disable-bloom -background d0d3d4 --hide filenames,mouse,progress -o - | ffmpeg -y -r 60 -f image2pipe -vcodec ppm -i - -vcodec libx264 -preset ultrafast -pix_fmt yuv420p -crf 1 -threads 0 -bf 0 gource.mp4
• Create Gif: ffmpeg -i gource.mp4 -r 10 -vf "scale=800:-1,split[s0][s1];[s0]palettegen[p];[s1][p]paletteuse" tesseract_evolution.gif

TODO's

Warning: These packages are under heavy development and are subject to change.

See issue #66

Build Instructions

1. Clone repository into your workspace
2. Clone the repositories in the dependencies.repos file using wstool or some other method (e.g. manually git cloning them)
3. Build the workspace using catkin tools, colcon, or a similar tool

NOTE: For noetic tesseract_ext is not required. Install the following dependencies: libbullet-dev, libbullet-extras-dev and ros-noetic-fcl. Taskflow can be install using the PPA below.

NOTE: To speed up clean build you may want to add tesseract_ext to an extended workspace.

NOTE: Install TaskFlow from ROS-Industrial PPA.

Building with Clang-Tidy Enabled

Must pass the -DTESSERACT_ENABLE_CLANG_TIDY=ON to cmake when building. This is automatically enabled if cmake argument -DTESSERACT_ENABLE_TESTING_ALL=ON is passed.

Building Tesseract Tests

Must pass the -DTESSERACT_ENABLE_TESTING=ON to cmake when wanting to build tests. This is automatically enabled if cmake argument -DTESSERACT_ENABLE_TESTING_ALL=ON is passed.

NOTE: If you are building using catkin tools, use catkin build --force-cmake -DTESSERACT_ENABLE_TESTING=ON.

Running Tesseract Tests

Tesseract packages use ctest because it is ROS agnostic, so to run the test call catkin test --no-deps tesseract_collision tesseract_common tesseract_environment tesseract_geometry tesseract_kinematics tesseract_scene_graph tesseract_urdf tesseract_command_language

Building Tesseract Code Coverage

Must pass the -DTESSERACT_ENABLE_CODE_COVERAGE=ON to cmake when wanting to build code coverage. The code coverage report is located in each individuals build directory inside a ccov/all-merged folder. Open the index.html file to see the packages code coverage report.

NOTE: Must be a clean build when generating a code coverage report. Also must build in debug.

Exclude Code From Code Coverage

• LCOV_EXCL_LINE
  • Lines containing this marker will be excluded.
• LCOV_EXCL_START
  • Marks the beginning of an excluded section. The current line is part of this section.
• LCOV_EXCL_STOP
  • Marks the end of an excluded section. The current line not part of this section.

.. NOTE: You can replace LCOV above with GCOV or GCOVR.

Create Debian Package (Linux) or NuGet Package (Windows)

The following process will generate a Debian or NuGet package leveraging cmake and cpack based on the OS.

The package should be located in the current directory.

cd <workspace directory>
catkin build -DCMAKE_BUILD_TYPE=Release -DTESSERACT_PACKAGE=ON
./src/tesseract/.run-cpack

.. NOTE: You can add a prefix to the package name by setting -DTESSERACT_PACKAGE_PREFIX="ros-noetic-".

Quality Tools Leverage

Tesseract currently leverages Compiler Warnigs, Clang Tidy and Code Coverage. All warnings produced by Compiler and Clang Tidy are treated as errors during CI builds.

• Compiler
  • Wall
  • Wextra
  • Wconversion
  • Wsign-conversion
  • Wno-sign-compare
  • Wnon-virtual-dtor
• Clang Tidy
  • clang-analyzer-*
  • bugprone-*
  • cppcoreguidelines-avoid-goto
  • cppcoreguidelines-c-copy-assignment-signature
  • cppcoreguidelines-interfaces-global-init
  • cppcoreguidelines-narrowing-conversions
  • cppcoreguidelines-no-malloc
  • cppcoreguidelines-slicing
  • cppcoreguidelines-special-member-functions
  • misc-*,-misc-non-private-member-variables-in-classes
  • modernize-*,-modernize-use-trailing-return-type,-modernize-use-nodiscard
  • performance-*
  • readability-avoid-const-params-in-decls
  • readability-container-size-empty
  • readability-delete-null-pointer
  • readability-deleted-default
  • readability-else-after-return
  • readability-function-size
  • readability-identifier-naming
  • readability-inconsistent-declaration-parameter-name
  • readability-misleading-indentation
  • readability-misplaced-array-index
  • readability-non-const-parameter
  • readability-redundant-*
  • readability-simplify-*
  • readability-static-*
  • readability-string-compare
  • readability-uniqueptr-delete-release
  • readability-rary-objects

Documentation and Tutorials

Documentation and tutorials for Tesseract are located at https://github.com/tesseract-robotics/tesseract_docs.git

Debugging Windows Build

• Search Directories CI
  • dir /s /b c:\opt\ros\noetic*assimp*
• Location of ROS Windows Builds
  • https://ros-win.visualstudio.com/ros-win/_build
  • https://ros-win.visualstudio.com/ros-win/_build/results?buildId=8711&view=artifacts&type=publishedArtifacts

tesseract's Issues

ROS 2 Support

dependencies.rosinstall

How to run dependencies.rosinstall? thanks

Collision geometry flattened when loading multibody .DAE from URDF

I have a URDF that contains a .DAE file with multiple geometry elements. When I open the file in a text editor it has a structure like this (trimmed down significantly, it's quite bulky):

<?xml version="1.0" encoding="utf-8"?>
<COLLADA xmlns="http://www.collada.org/2005/11/COLLADASchema" version="1.4.1">
  <asset>
...
  </asset>
  <library_geometries>
    <geometry id="body1" name="body1">
      <mesh>
        ...
      </mesh>
    </geometry>
    <geometry id="body2" name="body2">
      <mesh>
        ...
      </mesh>
    </geometry>
  </library_geometries>
...
</COLLADA>

I would like this file to be parsed such that every geometry element is loaded as a separate collision body. Based on conversations with @Levi-Armstrong this should be possible. However, I noticed that during collision checking Tesseract was treating the geometry as if it were a convex hull, and when I stepped into the URDF parser in my debugger I saw that the link associated with this object was assigned just one mesh that contained the vertices and triangles for all the bodies in the .DAE.

I think the culprit is line 208 in tesseract_scene_graph::convert:

https://github.com/ros-industrial-consortium/tesseract/blob/6598b801a4c3ac8d113f4aa716bda2bd023af73a/tesseract/tesseract_scene_graph/include/tesseract_scene_graph/parser/urdf_parser.h#L198-L213

This passes True to the flatten parameter of tesseract_scene_graph::createMeshFromPath, which ultimately causes Assimp::Importer to be called with the aiProcess_OptimizeMeshes flag, which flattens all the bodies in the .DAE into a single mesh.

When I switched it to False the URDF parser assigned the link the correct number of collision meshes, and my collision checker works in the expected way (i.e. only reporting collisions when actually intersecting a mesh, instead of when intersecting the link's convex hull). The documentation for that function says that the flatten parameter shouldn't be used for collision meshes, so I think urdf_parser.h may need to be fixed.

Scene Builder

After talking with @Levi-Armstrong , I'd like to remove the working assumption that there is one "core" URDF file that generates a scene and everything else is just "attached".

I propose that we make our own domain specific language around workcell modeling (that could closely follow URDFs example) that offers the following features:

• The ability to construct the scene entirely through code
• The ability to parse a URDF into a scene "assembly"
• The ability to compose, mutate, and delete links and assemblies to form desired workspaces.
• Perhaps the ability to specify a "solver" for a particular sub-graph of the scene

Our modeling might allow for parallel kinematic chains - the user would need to provide a solver for these components.

A good use case to build toward would be a first-class representation of "tool changing" - the frames and geometry of the attached tool would join a chain.

There's a lot of stuff to figure out here, but I think it would be worth it.

Food for thought.

tesseract_urdf parser fails if multiple materials with the same name exist

xacro can generate URDFs that contain repeated material definitions. I've included a representative snippet below.

...
<link name="wrist_1_link">
    <visual>
      <geometry>
        <mesh filename="package://myrobot_support/meshes/ur10e/visual/wrist1.dae"/>
      </geometry>
      <material name="LightGrey">
        <color rgba="0.7 0.7 0.7 1.0"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://myrobot_support/meshes/ur10e/collision/wrist1.stl"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="1.96"/>
      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
      <inertia ixx="0.0051082479567" ixy="0.0" ixz="0.0" iyy="0.0051082479567" iyz="0.0" izz="0.0055125"/>
    </inertial>
  </link>
  <joint name="wrist_2_joint" type="revolute">
    <parent link="wrist_1_link"/>
    <child link="wrist_2_link"/>
    <origin rpy="0.0 0.0 0.0" xyz="0.0 0.135 0.0"/>
    <axis xyz="0 0 1"/>
    <limit effort="54.0" lower="-6.28318530718" upper="6.28318530718" velocity="6.28"/>
    <dynamics damping="0.0" friction="0.0"/>
  </joint>
  <link name="wrist_2_link">
    <visual>
      <geometry>
        <mesh filename="package://myrobot_support/meshes/ur10e/visual/wrist2.dae"/>
      </geometry>
      <material name="LightGrey">
        <color rgba="0.7 0.7 0.7 1.0"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://myrobot_support/meshes/ur10e/collision/wrist2.stl"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="1.96"/>
      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
      <inertia ixx="0.0051082479567" ixy="0.0" ixz="0.0" iyy="0.0051082479567" iyz="0.0" izz="0.0055125"/>
    </inertial>
  </link>
  <joint name="wrist_3_joint" type="revolute">
    <parent link="wrist_2_link"/>
    <child link="wrist_3_link"/>
    <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.12"/>
    <axis xyz="0 1 0"/>
    <limit effort="54.0" lower="-6.28318530718" upper="6.28318530718" velocity="6.28"/>
    <dynamics damping="0.0" friction="0.0"/>
  </joint>
  <link name="wrist_3_link">
    <visual>
      <geometry>
        <mesh filename="package://myrobot_support/meshes/ur10e/visual/wrist3.dae"/>
      </geometry>
      <material name="LightGrey">
        <color rgba="0.7 0.7 0.7 1.0"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://myrobot_support/meshes/ur10e/collision/wrist3.stl"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.202"/>
      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
      <inertia ixx="0.000526462289415" ixy="0.0" ixz="0.0" iyy="0.000526462289415" iyz="0.0" izz="0.000568125"/>
    </inertial>
  </link>
...

The stock ROS URDF parser is apparently able to handle this without error, but the new tesseract_urdf parser throws the error below. @Levi-Armstrong try running the Descartes test executable I sent you a few days ago.

Error:   Error parsing urdf 'link' element for urdf 'ur10e'!
           Error parsing link 'visual' element for link 'shoulder_link'!
             Error parsing visual 'material' element!
               Multiple materials with the same name exist!
         at line 167 in /home/joe/workspaces/descartes_unit_test_ws/src/tesseract/tesseract/tesseract_urdf/src/urdf_parser.cpp

I think reasonable behavior for backwards compatibility would be using the first material encountered if duplicates are loaded.

Consolidate descartes samplers

There may be a way to consolidate the three Descartes samplers into a single class.

Add JointTrajectoryPoint structure inside JointTrajectory to replace TrajArray.

Impact of KinematicFilter assert

Hi,
I am creating a scene based on the tesseract_collision package in which I load robot links (with mask id BodyType::ROBOT_LINK) and environment objects (with mask id BodyType::ROBOT_ATTACHED).

When compiling in debug mode I frequently receive the following assert:
https://github.com/ros-industrial-consortium/tesseract/blob/8379d427cda2e413216031b9142335aa75a0a71e/tesseract_collision/include/tesseract_collision/bullet/bullet_utils.h#L420-L421
(asserts are removed in Release mode)

I'd like to understand this better to see where I am going wrong - @Levi-Armstrong do you have any insights on the origin of/need for this check here, and how it is for instance set for trajopt_ros?

Thank you very much for the insights,
Wolfgang

Switch to GTest imported targets?

Currently, the code uses GTEST_BOTH_LIBRARIES. From a cursory look, I believe we should instead switch to using GTest::GTest and GTest::GTestMain, because GTest has a transient dependency on Thread

https://cmake.org/cmake/help/v3.15/module/FindGTest.html

I also noticed that the test code in some other repos (opw_kinematics and descartes_ikfast at least) seems to contain typos, where the imported target is called gtest::GTest, which triggers the inclusion of the external project instead of using the system version.

How to add multiple "robots" to the same Environment?

I've been experimenting a bit more with Tesseract / TrajOpt and it seems to give good results 👍

However I would like to do the following:

• Create an Environment containing my robot via Tesseract::init(urdf, srdf, locator)
• Add collision shapes and other objects independently

So far I've been constructing them by hand (create Link, create Joint, then call addLink), but I would ideally load them from a URDF file. However a quick look at the documentation tells me that:

• parseURDF either returns a scene_graph or appends to a scene_graph
• Environment only has a const getter for its scene graph

I believe that I could get away with any of the following:

1. Merging my URDFs using xacro
2. Create a scene_graph, parse the various URDFs into it, and finally use Tesseract::init
3. By hand traverse each loaded model and add their Link / Joints to the scene
4. Each time a collision shape is to be added, update a stored scene_graph and call again Environment::init

Options 1. and 2. are suboptimal because they don't allow for online modification of the environment but 3. means writing quite a lot of code and 4. feels like a big hack.

Ideally I would like to reach something akin to Environment::addRobot(parseURDF(urdf)) that would allow me to add "robots" (they are static for now) to the environment.

Uninitialized reads reported in Valgrind

Error Description

When running Valgrind on the this unit test I get the following conditional jump on not initialized values messages valgrind_output. The code snippet in question deals with reading in the robot links from the URDF and constructing out of them the bullet collision objects. For the first constructed object, this results in an uninitialized read at this point.

System Description

Description: Ubuntu 18.04.2 LTS
Release: 18.04
Codename: bionic

Linux jp-XPS-15-9550 4.15.0-51-generic
x86_64 x86_64 x86_64 GNU/Linux

ROS_DISTRO=melodic
ROS_ETC_DIR=/opt/ros/melodic/etc/ros
ROS_PACKAGE_PATH=/opt/ros/melodic/share
ROS_PYTHON_VERSION=2
ROS_VERSION=1
ROS_ROOT=/opt/ros/melodic/share/ros
ROS_MASTER_URI=http://localhost:11311
ROSLISP_PACKAGE_DIRECTORIES=

What's the best way to display the scene in RViz?

Hello,

I was dusting off some old experiments I made. At that time, I was using ROSBasicPlotting::plotScene() to send collision updates to RViz. What's the preferred way nowadays?

• I see that the examples use sendRvizChanges under the hood, that makes a service call, but there's a bit of boilerplate associated with revision tracking and so on.
• I also see that there is an EnvironmentMonitor, is it usable? Would it allow for similar functionality?

Segfault When Manipulator's Base Link is the URDF Root Frame

When a planning group has the URDF root frame as its base link (e.g. world), the KDL Environment implementations segfault after accessing a null pointer when parsing the URDF for a robot description.

You can re-produce this error by changing the Puzzle Piece SRDF manipulator group's base link to world.

Thread 1 "trajopt_example" received signal SIGSEGV, Segmentation fault.
0x00007ffff74f4c0c in std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >::_Alloc_hider::_Alloc_hider (__a=..., __dat=<optimized out>, this=<optimized out>) at /usr/include/c++/5/bits/basic_string.h:109
109             : allocator_type(__a), _M_p(__dat) { }
(gdb) bt
#0  0x00007ffff74f4c0c in std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >::_Alloc_hider::_Alloc_hider (__a=..., __dat=<optimized out>, this=<optimized out>) at /usr/include/c++/5/bits/basic_string.h:109
#1  std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >::basic_string (__str=<error reading variable: Cannot access memory at address 0x8>, this=0x7fffffffba30) at /usr/include/c++/5/bits/basic_string.h:399
#2  tesseract::tesseract_ros::KDLChainKin::init (this=this@entry=0x761c50, model=..., base_link="world", tip_link="part", name="manipulator") at /home/jmeyer/ros/tess_ws/src/tesseract/tesseract_ros/src/kdl/kdl_chain_kin.cpp:354
#3  0x00007ffff7504011 in tesseract::tesseract_ros::KDLEnv::addManipulator (this=0x6d3dc0, base_link="world", tip_link="part", manipulator_name="manipulator") at /home/jmeyer/ros/tess_ws/src/tesseract/tesseract_ros/src/kdl/kdl_env.cpp:412
#4  0x00007ffff7501ac3 in tesseract::tesseract_ros::KDLEnv::init (this=<optimized out>, urdf_model=..., srdf_model=...) at /home/jmeyer/ros/tess_ws/src/tesseract/tesseract_ros/src/kdl/kdl_env.cpp:129
#5  0x000000000041cc29 in main (argc=1, argv=0x7fffffffcb98) at /home/jmeyer/ros/tess_ws/src/trajopt_ros/trajopt_examples/src/puzzle_piece_plan.cpp:211
(gdb) 

add python interface?

Clean up contact monitor

The contact monitor could use a refactor. See some comments on #162.

1. It should be refactored into a class so that we can have multiple contact monitors at the same time. This should let us get rid of most if not all of the global variables there.
2. We should work to separate some of the processes more completely. For example, I would think contactTest should go in a different thread from visualization. Then that could be running for example on the GPU. Along those lines, computeCollisionReportThread currently blocks callbackJointState. I would think the joint state callback should be really quick and then when the collision checking needs them it would very quickly lock, copy the latest, and unlock the joint states.

descartes_tesseract package not found

The descartes_tesseract package was removed from trajopt_ros descartes_light but is not in the current master branch of tesseract as expected.

stray NodeHandle instance in kdl_env.cpp

In the implementation of KDLEnv, at line 67, there is a stray NodeHandle instance.

Is there a specific reason you are forcing KDLEnv to be used with a roscore? It looks like the class could be useful (and used) even without a roscore running.

kdl_env seems to incorrectly load the ACM from SRDF

In the implementation of KDLEnv, at line 150, SRDF's ACM is loaded in a way such that disabled collisions in the SRDF (only) are added as allowed in KDLEnv (basically inverting the ACM).

for (const auto& pair : srdf_model_->getDisabledCollisionPairs())
{
  allowed_collision_matrix_->addAllowedCollision(pair.link1_, pair.link2_, pair.reason_);
}

Tesseract Development TODO List

• Continuous collision checking between two moving objects
• Upgrade data structure to support graphs
• Upgrade to support ROS2 #43
  • Make all core package pure cmake package
  • Create ROS2 meta package containing same package in tesseract_ros meta package
• Upgrade to use Bullet3
• Add tesseract process planning
• Update Clang format in CI from 5.0 to 8.0 #93
• Unify syntax for defining shared pointer types
• Rename BasicPlanner class to Planner
• Add sub graph functionality to scene graph
• Add sub graph solver functionality to tesseract_environment. The idea behind this to mimic actual hardware in the environment.
• Planner: Add reference frame to cartesian waypoint #125

Add support for continuous collision checking between two moving objects

Currently detecting collision between two moving objects does work but need to work out the math to calculate the contact time and nearest points relative to each links coordinate system.

Refactor Planner Configuration and Hybrid Planners

The concept of the hybrid planner is to create a trajectory with a "global" planner (OMPL, STOMP, Descartes) and give that trajectory as a seed to the TrajOpt "local" planner. It seems unnecessary to create hybrid planners for each type of global planner. The hybrid planner should really be templated based on the type of global planner you want to use:

template<typename T>
class HybridPlanner : public MotionPlanner
{
public:
...
  bool configure()
  {
    // How should this method work to configure both planners generically?
  }

  bool solve(...) override
  {
    ...
    // Call global planner
    global_planner_.solve(global_planner_result);

    // Seed the local planner with the global planner result
    ...

    // Call local planner
    trajopt_planner_.solve(final_result);
    ...
  }
...
private:
  T global_planner_;
  TrajOptMotionPlanner trajopt_planner_;
}

This is a nice idea, but the current configuration of the planners does not allow this for a few reasons:

• There is not a generic configure method in the planner base class
  • Even if there were, there is not a single set of parameters that could configure all of the different types of planners (unless we used a JSON object, XMLRPC value, or boost::any)
• There is not generic planner configuration base class with which to configure the planners.
  • Even if there were, we would have to cast it to the correct derived type which does not seem like an ideal practice

We should revisit the concept of planner configuration and come up with a better solution that simplifies the interface for configuring the various types of planners in a generic way

readme lists tesseract_rviz twice

The current version of the readme lists the tesseract_rviz package twice: once as the last package, and once some lines before that.

I would've fixed this myself, but I don't know which one you'd like to keep @Levi-Armstrong.

FreeCAD integration

FreeCAD has a (seemingly very limited) robot workbench module that can be used to set waypoints and specify paths.

https://www.freecadweb.org/wiki/Robot_Workbench

Having a tool like this integrated with Tesseract would provide capabilities similar to commercial robot planning software.

RVIZ crashes when trying to add tesseract_trajectory or when trying to load robotmodel with 'base_link'

I get the following error when trying to load trajectory or robot model

terminate called after throwing an instance of 'Ogre::ItemIdentityException'
what(): OGRE EXCEPTION(4:ItemIdentityException): An object of type 'Entity' with name 'Robot Link4' already exists. in SceneManager::createMovableObject at /build/ogre-1.9-mqY1wq/ogre-1.9-1.9.0+dfsg1/OgreMain/src/OgreSceneManager.cpp (line 6832)
Aborted (core dumped)

Kindly please help me how to overcome this error and why do I get this?

"Failed to calculate jacobian" when using KDLJointKin

it looks to me like KDLJointKin does not implement successfully addAttachedLink (parent_link_name is never stored) and thus also calcJacobian (if we request the Jacobian of an attached link it will fail instead of computing the Jacobian of its parent link and then transforming it into the attached link).

The problem became apparent when using TrajOpt with a manipulator with an attached object.

checkJoints Returns True Unconditionally

The KDL impl of the checkJoints function returns true as long as the size of the joint vector passed in is equal to the expected size. It will print warnings but not return false.

Is this intended behavior?

CartVelJacCalculator Fails Assertions

I attempted to use the CartVelCntInfo constraint.

#0  0x00007ffff4117428 in __GI_raise (sig=sig@entry=6) at ../sysdeps/unix/sysv/linux/raise.c:54
#1  0x00007ffff411902a in __GI_abort () at abort.c:89
#2  0x00007ffff410fbd7 in __assert_fail_base (fmt=<optimized out>, assertion=assertion@entry=0x7ffff7084478 "matrix.rows() == jacobian.rows()", 
    file=file@entry=0x7ffff7084418 "/home/jmeyer/ros/tess_ws/src/tesseract/tesseract_ros/include/tesseract_ros/kdl/kdl_utils.h", line=line@entry=93, 
    function=function@entry=0x7ffff708a100 <tesseract::tesseract_ros::KDLToEigen(KDL::Jacobian const&, std::vector<int, std::allocator<int> > const&, Eigen::Ref<Eigen::Matrix<double, -1, -1, 0, -1, -1>, 0, Eigen::OuterStride<-1> >)::__PRETTY_FUNCTION__> "void tesseract::tesseract_ros::KDLToEigen(const KDL::Jacobian&, const std::vector<int>&, Eigen::Ref<Eigen::Matrix<double, -1, -1> >)") at assert.c:92
#3  0x00007ffff410fc82 in __GI___assert_fail (assertion=0x7ffff7084478 "matrix.rows() == jacobian.rows()", 
    file=0x7ffff7084418 "/home/jmeyer/ros/tess_ws/src/tesseract/tesseract_ros/include/tesseract_ros/kdl/kdl_utils.h", line=93, 
    function=0x7ffff708a100 <tesseract::tesseract_ros::KDLToEigen(KDL::Jacobian const&, std::vector<int, std::allocator<int> > const&, Eigen::Ref<Eigen::Matrix<double, -1, -1, 0, -1, -1>, 0, Eigen::OuterStride<-1> >)::__PRETTY_FUNCTION__> "void tesseract::tesseract_ros::KDLToEigen(const KDL::Jacobian&, const std::vector<int>&, Eigen::Ref<Eigen::Matrix<double, -1, -1> >)") at assert.c:101
#4  0x00007ffff7047036 in tesseract::tesseract_ros::KDLToEigen (jacobian=..., q_nrs=std::vector of length 14, capacity 14 = {...}, matrix=...)
    at /home/jmeyer/ros/tess_ws/src/tesseract/tesseract_ros/include/tesseract_ros/kdl/kdl_utils.h:93
#5  0x00007ffff70448e2 in tesseract::tesseract_ros::KDLJointKin::calcJacobian (this=0x875950, jacobian=..., change_base=..., joint_angles=..., 
    link_name="grinder_frame", state=...) at /home/jmeyer/ros/tess_ws/src/tesseract/tesseract_ros/src/kdl/kdl_joint_kin.cpp:152
#6  0x00007ffff7ad372c in trajopt::CartVelJacCalculator::operator() (this=0xe1d9e0, dof_vals=...)
    at /home/jmeyer/ros/tess_ws/src/trajopt_ros/trajopt/src/kinematic_terms.cpp:134
#7  0x00007ffff765266e in sco::MatrixOfVector::call (this=0xe1d9e0, x=...)
    at /home/jmeyer/ros/tess_ws/src/trajopt_ros/trajopt_sco/include/trajopt_sco/num_diff.hpp:46
#8  0x00007ffff76510e9 in sco::ConstraintFromFunc::convex (this=0xe1dcd0, xin=std::vector of length 6118, capacity 6118 = {...}, model=0xbdc8e0)
    at /home/jmeyer/ros/tess_ws/src/trajopt_ros/trajopt_sco/src/modeling_utils.cpp:215
#9  0x00007ffff76476ba in sco::convexifyConstraints (cnts=std::vector of length 873, capacity 873 = {...}, 
    x=std::vector of length 6118, capacity 6118 = {...}, model=0xbdc8e0) at /home/jmeyer/ros/tess_ws/src/trajopt_ros/trajopt_sco/src/optimizers.cpp:67
#10 0x00007ffff76496ed in sco::BasicTrustRegionSQP::optimize (this=0x7fffffffc400)
    at /home/jmeyer/ros/tess_ws/src/trajopt_ros/trajopt_sco/src/optimizers.cpp:314
#11 0x00000000005348c2 in main (argc=1, argv=0x7fffffffcbb8) at /home/jmeyer/ros/tess_ws/src/trajopt_ros/trajopt_examples/src/two_robots.cpp:276

LinearMath/btConvexHullComputer.h: No such file or directory

This compile error pops up occasionally. I just ran into it using tesseract_rosutils/utils.h.

Return vector of succeeded/failed waypoints in PlannerResponse

This is important for descartes especially where we want to remove failed vertices and try again.

BasicEnv Base Class Has No Virtual Destructor

See title. Prevents the proper destruction of objects when manipulating environments through a pointer to the base class.

PCL build issue in tesseract_motion_planning

The issue has been fixed in 1.9.1 PointCloudLibrary/pcl#2626. This is only an issue if you are stuck pre-1.8.1 for some reason (ie using the version that comes with Kinetic)

When POLICY CMP0074 is true and PCL_TO_FIND_COMPONENTS is false then the cmake_policy(POP) does not get called.

The solution is to do what they did in the PR. Go into PCLConfig.cmake and change

if(NOT PCL_TO_FIND_COMPONENTS)
  return()
endif()

to

if(NOT PCL_TO_FIND_COMPONENTS)
  if(POLICY CMP0074)
    cmake_policy(POP)
  endif()
  return()
endif()

Return file contents instead of filename for tesseract_scene_graph::ResourceLocatorFn

Currently tesseract_scene_graph::ResourceLocatorFn returns the filename to a resource instead of the resource contents. This is inflexible since the resource may be stored in a zip file or retrieved from a URL. The locator function should return the file contents as a byte array. Assimp can load in the raw bytes using ReadFileFromMemory so there shouldn't be a compatibility issue.

Leverage BehaviorTree.CPP with Boost or Cereal for process planners.

I think it would be interesting to leverage BehaviorTree.CPP for create a complex process planner using Boost or Cereal for serializing data structures for passing data around in the BehaviorTree.

https://uscilab.github.io/cereal/serialization_archives.html
https://github.com/BehaviorTree

OMPL planner doesn't provide a trajectory

OMPL planner doesn't seem to provide a trajectory, but only the start and end waypoints even if it states that a solution has been found.
I tried it using RRTConnect and PRM with the same configuration in the unit test.

Am I making a mistake?

Duplicates in the list of collisions

Working with collision_monitoring from commit 683f684, it looks like every collision entry is mirrored in the /tesseract_monitoring_contacts/contact_results. Is that desirable? I think once would be enough.

constacts: 
  - 
    distance: 0.0769603028893
    type_id: [0, 0]
    link_names: [link_l, link_r]
.
.
.
    distance: 0.0769603028893
    type_id: [0, 0]
    link_names: [link_r, link_l]

CTest Verbosity

Was #136 supposed to print the results of CTest to the terminal? If so, it hasn't worked for me on my local machine (Ubuntu 16.04, ROS Kinetic) and it doesn't seem to have worked on the Kinetic CI machine either (see latest build of #126 which has been rebased onto master)

.dae mesh collision geometry loaded from URDF is incorrectly oriented

I get different collision results with geometry defined by .dae meshes when using versions of Tesseract before (bef6578) and after (d6397ad) the addition of the new tesseract_urdf parser. The URDF looks the same as before when visualized in Rviz, so I think this is because of a difference in how urdfdom and tesseract_urdf parse the files.

I can demonstrate this by defining the same geometry both as primitive shapes in the URDF and as a multibody .dae object. Assuming that a collision-free path through the waypoints exists, planning will succeed for the primitive-based geometry but may fail for the .dae-based geometry.

My initial guess based on the nature of the failures is that .dae geometry is being rotated by 90 degrees about each object's local Z-axis, though I don't have sufficiently detailed results to conclusively show this.

I made a repo with some test cases. You should be able to replicate this by checking out both versions of Tesseract and running rosrun descartes_system_tests collision_body_tests for each. All tests should succeed for bef6578 and the DescartesMultithreadUnitNearMultibodyDAE test should fail for d6397ad.

SDF Loader

I have started investigating loading SDF robot definition files and have found that the standard sdformat library that comes with Xenial and Bionic is very primitive and does not provide functionality beyond @Levi-Armstrong 's URDF parser. My suggestion is that we modify the URDF parser to be able to parse both URDF and SDF files since there is a lot of overlap. I will talk to RPI about assigning a student to this task. @Levi-Armstrong do you agree with this approach?

Master Branch needs to be Clang Formatted

CI is not currently working

Windows build

I have patched tesseract and its dependencies to build and work with Windows. I am not sure if everything is working correctly because the current master branches are failing unit tests on Linux. The patches can be found here:

https://github.com/johnwason/trajopt_ros/tree/windows

https://github.com/johnwason/tesseract/tree/windows

https://github.com/johnwason/descartes_light/tree/windows

https://github.com/johnwason/opw_kinematics/tree/windows

The following modifications were required:

• Windows requires DLL export tags in the header files and does not like to use STL with shared libraries. For these reasons, static libraries should be used on Windows.
• M_PI is not defined by MSVC. Use EIGEN_PI instead.
• The default Gurobi solver doesn't work on Windows. qpOASIS seems to work, but OSQP is giving a "P is not upper-triangular" error.
• Use STL or Boost instead of system functions to allow cross-platform builds
• For some reason jsoncpp is not in the ROS choco repository for Windows. Using c:\opt\vcpkg to install seems to work.
• Patch ros/catkin#1034 to catkin is required
• GCC specific compiler options need to be fenced with if(NOT MSVC) in cmake files

I will leave it to the main developers to determine the best way to merge in these changes.

Nonexistant variable tesseract_support_INCLUDE_DIRS

Problem

tesseract_support-config.cmake.in defines the variable tesseract_support_INCLUDE_DIRS, which points to the package's include directory. However, tesseract_support doesn't have an include directory, which causes errors when downstream packages try to use it:

Starting >>> tesseract_geometry
--- stderr: tesseract_geometry                                                             
CMake Error at /home/joe/workspaces/tesseract_ws/src/install/tesseract_support/lib/cmake/tesseract_support/tesseract_support-config.cmake:11 (message):
  File or directory
  /home/joe/workspaces/tesseract_ws/src/install/tesseract_support/include
  referenced by variable tesseract_support_INCLUDE_DIRS does not exist !
Call Stack (most recent call first):
  /home/joe/workspaces/tesseract_ws/src/install/tesseract_support/lib/cmake/tesseract_support/tesseract_support-config.cmake:18 (set_and_check)
  examples/CMakeLists.txt:1 (find_package)


---
Failed   <<< tesseract_geometry	[ Exited with code 1 ]
Aborted  <<< fcl                                            

Solution

Remove the line set_and_check(@PROJECT_NAME@_INCLUDE_DIRS "${PACKAGE_PREFIX_DIR}/include") from tesseract_support-config.cmake.in

A PR is on the way!

Failed to update rviz environment.

I got the following error running car_seat_example in tesseract_ros_examples package:
[ INFO] [1566358488.704137330]: Failed to update rviz environment

I also played with the examples and tried to modify the environment. I always get Failed to update rviz environment error in sendRvizChanges() after removing links or joints, by calling removeLink() or removeJoint(), from the environment.

We also have some observations which may help you debuging.

1. The sendRvizChanges() failed when calling removeLink() or removeJoint(). Using add/move link is fine.
2. If we set the rviz parameter to false in the launch file, then no error prompted and the program can successfully finish.

I am using Ubuntu 18.04 with ROS Melodic and Gurobi optimizer.

Could anyone help me to solve this error? Thanks.

KDLEnv can cause warnings in pluginlib

When using KDLEnv it often happens to get warning messages like the following

[ WARN] [1538691530.287577408]: SEVERE WARNING!!! Attempting to unload library while objects created by this loader exist in the heap! You should delete your objects before attempting to unload the library or destroying the ClassLoader. The library will NOT be unloaded.

Make gtest package a submodule so CI clang format will pass

Potential release - and issue with bullet3_ros with other packages

Hi guys!
Thank you very much for the great work in putting together this framework! I am exploring tesseract_collision and really like the abstraction :-).

However, using the package leads to conflicts in our catkin workspace - particular due to the following line:
https://github.com/ros-industrial-consortium/tesseract/blob/4c6d86028bbc08c39606acd5c1b1ba79d69874ce/tesseract_ext/bullet3_ros/CMakeLists.txt#L15
which breaks some of our buildspace logic (and wouldn't work e.g. on the buildfarm).

As thus my questions/request:

• Are there additional bug fixes in upstream bullet that are not yet in the system packages libbullet-dev? [I guess 'yes']
• Is there a way to get bullet3_ros or tesseract as a whole released upstream via the ROS buildfarm? I know there is likely little appetite for a newer version of a system-provided library (and have personally gone through that process a few times - with fcl_catkin and pybind11_catkin). If there is clear benefit and no downside in library conflicts, it may be worth trying.
• E.g., the fcl_catkin we release upstream (as of today) contains @Levi-Armstrong's Convex constructor fix. However, we did not package libccd (and probably should).
• If not, can the logic in bullet3_ros be updated not to create the folder on a devel-only workspace?

Thank you very much,
Wolfgang

Thread Safety Part #2

I'm not sure that all of the threading issues are gone. I absolutely do not discount that the error could be in my code (or likely is), but I wanted to post this notice.

While running some tests with a modified version of Descartes, I noticed that the plans I got back were inconsistent. About 1 in 5 plans returned a slightly different result. Disabling threading seems to fix this. Comparing the plans, it does appear that the collision checker is returning a different number of states every once in a while.

Furthermore, on much rarer occassions I get segfaults. After trying my program a hundred times in GDB, I finally caught this back-trace. Does this ring any bells? The threadIndex variable mentioned on the line it died at makes me suspicious...

0x00007fffdd79f57f in CProfileManager::Start_Profile (name=0x7fffdd564b88 "btCompoundCompoundLeafCallback::Process") at /home/jon/ros/trajopt_ws/src/tess_stuff/tesseract/tesseract_ext/bullet3_ros/bullet3/src/LinearMath/btQuickprof.cpp:555
555		if (name != gCurrentNodes[threadIndex]->Get_Name()) {
(gdb) bt
#0  0x00007fffdd79f57f in CProfileManager::Start_Profile (name=0x7fffdd564b88 "btCompoundCompoundLeafCallback::Process") at /home/jon/ros/trajopt_ws/src/tess_stuff/tesseract/tesseract_ext/bullet3_ros/bullet3/src/LinearMath/btQuickprof.cpp:555
#1  0x00007fffdd4dd251 in btCompoundCompoundLeafCallback::Process (this=this@entry=0x7fff61cf50f0, leaf0=leaf0@entry=0x9e8dd0, leaf1=leaf1@entry=0xa048f0)
    at /home/jon/ros/trajopt_ws/src/tess_stuff/tesseract/tesseract_ext/bullet3_ros/bullet3/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp:130
#2  0x00007fffdd4dd0e0 in MycollideTT (distanceThreshold=0, callback=0x7fff61cf50f0, xform=<synthetic pointer>, root1=<optimized out>, root0=<optimized out>)
    at /home/jon/ros/trajopt_ws/src/tess_stuff/tesseract/tesseract_ext/bullet3_ros/bullet3/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp:296
#3  btCompoundCompoundCollisionAlgorithm::processCollision (this=0xb1c7900, body0Wrap=0x7fff61cf6a10, body1Wrap=0x7fff61cf6a40, dispatchInfo=..., resultOut=0x7fff61cf6a70)
    at /home/jon/ros/trajopt_ws/src/tess_stuff/tesseract/tesseract_ext/bullet3_ros/bullet3/src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp:371
#4  0x00007fffdd9c834d in tesseract::tesseract_bullet::TesseractCollisionPairCallback::processOverlap (this=0x7fff61cf6b90, pair=...)
    at /home/jon/ros/trajopt_ws/src/tess_stuff/tesseract/tesseract_collision/include/tesseract_collision/bullet/bullet_utils.h:681
#5  0x00007fffdd4ab3d0 in btHashedOverlappingPairCache::processAllOverlappingPairs (this=0xb28bf00, callback=0x7fff61cf6b90, dispatcher=0xb286c50)
    at /home/jon/ros/trajopt_ws/src/tess_stuff/tesseract/tesseract_ext/bullet3_ros/bullet3/src/BulletCollision/BroadphaseCollision/btOverlappingPairCache.cpp:386
#6  0x00007fffdd9cd838 in tesseract::tesseract_bullet::BulletDiscreteBVHManager::contactTest (this=0xb1c6850, collisions=...) at /home/jon/ros/trajopt_ws/src/tess_stuff/tesseract/tesseract_collision/src/bullet/bullet_discrete_managers.cpp:491
#7  0x00007ffff2d864d4 in descartes_light::TesseractCollision::validate (this=0xb1c60a0, pos=0x7fff380106e0, size=<optimized out>) at /home/jon/ros/trajopt_ws/src/tess_stuff/descartes_light/src/descartes_light/collision_checker.cpp:38

Thread Safety

It is a point of open investigation as to whether or not the default bullet collision library provides a thread safe interface in the same way that MoveIt's wrapper around FCL does.

CartVelTermInfo() parameters adjustment to acheive trapezoidal Cartesian Velocity

Time of start is generated by trajopt but when the trajectory is given to IPTP it gives new time of start and the velocity is profile is as shown in the below figure:

And I am able to define constraint on CartVelTermInfo() as shown below but not for each waypoint

std::shared_ptr cv(new CartVelTermInfo);
cv->term_type = TT_COST;
cv->first_step = 0;
cv->last_step = pci.basic_info.n_steps-1;
cv->max_displacement = 0.05;
cv->link = "tool0";

Kindly please help me to understand how to play with the parameters to acheive trapezoidal velocity profile or approximated velocity profile.

Eigen Alignment Issues

TrajOpt and Tesseract have been plagued by Eigen alignment issues for a while (see tesseract-robotics/trajopt#27). However it did not seem to be widespread and was limited to only a handful of applications and only on certain people's computers. In working on #117 it suddenly got a lot worse. This was presumably because we switched to using make_shared instead of new when creating pointers, so suddenly none of the EIGEN_MAKE_ALIGNED_OPERATOR_NEW macros worked. Regardless, a class that contains an aligned class must also be aligned, and aligned Eigen types are used in some pretty fundamental classes (e.g Link). Requiring a user to know that they must use new instead of make_shared seems unacceptable.

It turns out that for Eigen 3.2+ this is only an issue on 32 bit machines or when compiling on 64 bit machines with AVX instructions. Codebases like Drake have therefore fixed it by simply disabling AVX instructions in the cmake.

Apparently there is a fix in a newer version of Eigen (could you provide a link? @Levi-Armstrong ). However it is unknown if that will be backported to the system version for still currently common systems like 16.04.

The current plan is to follow Drake's example and disable AVX instructions for most packages in Tesseract.