personalrobotics / aikido

Artificial Intelligence for Kinematics, Dynamics, and Optimization

AIKIDO - AI for KIDO

⚠️ Warning: AIKIDO is under heavy development. These instructions are primarily for reference by the developers.

AIKIDO is a C++ library, complete with Python bindings, for solving robotic motion planning and decision making problems. This library is tightly integrated with DART for kinematic/dynamics calculations and OMPL for motion planning. AIKIDO optionally integrates with ROS, through the suite of aikido_ros packages, for execution on real robots.

Building from Source

Dependencies

AIKIDO depends on ROS. You should install ROS by adding the ROS repository to your sources.list as follows. We encourage users to install noetic with at least the following packages:

$ sudo apt install ros-noetic-actionlib ros-noetic-geometry-msgs ros-noetic-interactive-markers ros-noetic-roscpp ros-noetic-std-msgs ros-noetic-tf ros-noetic-trajectory-msgs ros-noetic-visualization-msgs

AIKIDO also depends on CMake, Boost, DART (version 6.8.5 or above), OMPL, yaml-cpp, tinyxml2, pr-control-msgs, libmicrohttpd, and the Python development headers (python-dev on Debian systems). DART and AIKIDO both make heavy use of C++14 and require a modern compiler.

On Ubuntu Focal using CMake

You should install the ROS packages as described above to build all the ROS-dependent AIKIDO components (e.g., aikido-control-ros).

Install the other dependencies:

$ sudo add-apt-repository ppa:dartsim/ppa
$ sudo add-apt-repository ppa:personalrobotics/ppa
$ sudo apt-get update
$ sudo apt-get install cmake build-essential libboost-filesystem-dev libdart6-optimizer-nlopt-dev libdart6-utils-dev libdart6-utils-urdf-dev libmicrohttpd-dev libompl-dev libtinyxml2-dev libyaml-cpp-dev pr-control-msgs

Once the dependencies are installed, you can build and install AIKIDO using CMake:

$ mkdir build
$ cd build
$ cmake ..
$ make  # you may want to build AIKIDO using multi-core by executing `make -j4`
$ sudo make install

AIKIDO includes several optional components that depend on ROS. While we suggest building AIKIDO in a Catkin workspace (see below) to enable the ROS components, it is also possible to build those components in a standalone build. To do so, source the setup.bash file in your Catkin workspace before running the above commands, e.g.:

$ . /path/to/my/workspace/setup.bash

On Ubuntu Focal using Catkin

It is also possible to build AIKIDO as a third-party package inside a Catkin workspace. To do so, clone AIKIDO into your Catkin workspace and use the catkin build command like normal.

If you are using the older catkin_make command, then you must build your workspace with catkin_make_isolated. This may dramatically increase your build time, so we strongly recommend that you use catkin build, which is provided by the catkin_tools package, if possible.

On macOS using CMake

Please install Homebrew as described above, then you can easily install the dependencies as follows:

$ cd <aikido_directory>
$ brew bundle

Once the dependencies are installed, you can build and install AIKIDO using CMake:

$ cd <aikido_directory>
$ mkdir build
$ cd build
$ cmake ..
$ make  # you may want to build AIKIDO using multi-core by executing `make -j4`
$ sudo make install

Code Style

Please follow the AIKIDO style guidelines when making a contribution.

License

AIKIDO is licensed under a BSD license. See LICENSE for more information.

Authors

AIKIDO is developed by the Personal Robotics Lab in the Paul G. Allen School of Computer Science and Engineering at the University of Washington. The library was started by Michael Koval (@mkoval) and Pras Velagapudi (@psigen). It has received major contributions from Shushman Choudhury (@Shushman), Ethan Gordon (@egordon), Brian Hou (@brianhou), Aaron Johnson (@aaronjoh), Jennifer King (@jeking), Gilwoo Lee (@gilwoolee), Jeongseok Lee (@jslee02), and Clint Liddick (@ClintLiddick). We also would like to thank Michael Grey (@mxgrey) and Jeongseok Lee (@jslee02) for making changes to DART to better support AIKIDO.

aikido's Issues

Mechanism for indexing into a CompoundState

It would be helpful to have a standard method of opaquely indexing into a CompoundState to access a specific substate. We currently do this by index, which does not generalize nicely to nested CompoundStateSpaces. This is similar to the SubstateLocation concept in OMPL.

Add VanDerCorput::length() method

Compute total length of sequence; useful for pre-allocating memory or not using iterators.

Implement TSR's project

Implement and test TSR::project(..) method.

OMPL default state sampler

Current implementation of allocDefaultStateSampler will allocate a sampler that returns the same sequence every time, i.e. two calls to the function will generate samplers that produce identical sequences. Check that this fits within OMPL's intended usage of the function.

TSR's isSatisfied compares getValue's norm to some double number.

This may not be the right way to check if a state is in TSR.

Build errors

Here are the errors I got in aikido build. It seems ompl and boost doesn't go well. I have ompl 0.13.0 and boost 1.54.0 installed.

/home/js/dev/pr/aikido/master/aikido/src/util/CatkinResourceRetriever.cpp:3:25: fatal error: ros/package.h: No such file or directory
 #include <ros/package.h>
                         ^
compilation terminated.
make[2]: *** [CMakeFiles/aikido.dir/src/util/CatkinResourceRetriever.cpp.o] Error 1
make[2]: *** Waiting for unfinished jobs....
In file included from /usr/include/boost/smart_ptr/make_shared.hpp:15:0,
                 from /usr/include/boost/make_shared.hpp:15,
                 from /home/js/dev/pr/aikido/master/aikido/src/ompl/plan.cpp:3:
/usr/include/boost/smart_ptr/make_shared_object.hpp: In instantiation of 'typename boost::detail::sp_if_not_array<T>::type boost::make_shared(Arg1&&, Args&& ...) [with T = ompl::geometric::SimpleSetup; Arg1 = const boost::shared_ptr<ompl::base::SpaceInformation>&; Args = {}; typename boost::detail::sp_if_not_array<T>::type = boost::shared_ptr<ompl::geometric::SimpleSetup>]':
/home/js/dev/pr/aikido/master/aikido/src/ompl/plan.cpp:77:59:   required from here
/usr/include/boost/smart_ptr/make_shared_object.hpp:218:5: error: no matching function for call to 'ompl::geometric::SimpleSetup::SimpleSetup(const boost::shared_ptr<ompl::base::SpaceInformation>&)'
     ::new( pv ) T( boost::detail::sp_forward<Arg1>( arg1 ), boost::detail::sp_forward<Args>( args )... );
     ^
/usr/include/boost/smart_ptr/make_shared_object.hpp:218:5: note: candidates are:
In file included from /home/js/dev/pr/aikido/master/aikido/src/ompl/plan.cpp:9:0:
/usr/include/ompl/geometric/SimpleSetup.h:70:13: note: ompl::geometric::SimpleSetup::SimpleSetup(const StateSpacePtr&)
             SimpleSetup(const base::StateSpacePtr &space);
             ^
/usr/include/ompl/geometric/SimpleSetup.h:70:13: note:   no known conversion for argument 1 from 'const boost::shared_ptr<ompl::base::SpaceInformation>' to 'const StateSpacePtr& {aka const boost::shared_ptr<ompl::base::StateSpace>&}'
/usr/include/ompl/geometric/SimpleSetup.h:64:15: note: ompl::geometric::SimpleSetup::SimpleSetup(const ompl::geometric::SimpleSetup&)
         class SimpleSetup
               ^
/usr/include/ompl/geometric/SimpleSetup.h:64:15: note:   no known conversion for argument 1 from 'const boost::shared_ptr<ompl::base::SpaceInformation>' to 'const ompl::geometric::SimpleSetup&'
make[2]: *** [CMakeFiles/aikido.dir/src/ompl/plan.cpp.o] Error 1
make[1]: *** [CMakeFiles/aikido.dir/all] Error 2
make: *** [all] Error 2
14:19:27: The process "/usr/bin/make" exited with code 2.
Error while building/deploying project aikido (kit: Desktop)
When executing step "Make"

Add stream operator to StateHandle

@psigen suggested overloading operator << on StateHandle to call the StateSpace::print function in #94.

Retry logic for IkSampleableConstraint->sample() when sampling a seed fails.

In current implementation of IkSampleableConstraint->sample(), when sampling from either mSeedSampler or mPoseSampler fails it will continue retrying mMaxIteration has been reached. However, these samplers are deterministic, and once they cannot sample, it's likely that all samples are exhausted and thus retrying will not produce any new sample. One possible solution is to wrap these with FiniteCyclicSampelableConstraint to cycle multiple times.

Handle memory leak when States are copied.

Current implementation risks memory leak when constructors copy States and throw error. We currently avoid this by freeing states before the constructor throws an error. It'd be nice to create an RAII helper class to encapsulate this (@mkoval's suggestion.)

Migrate from travis-ci.com to travis-ci.org

Now that Aikido is public we should migrate from travis-ci.com to travis-ci.org. This was a bit of a painful process for or_fcl, so I am holding off until we are done with the upcoming merges.

RnBoxConstraint's getValueAndJacobian calls getValue twice.

This should be either removed to use the default implementation or changed to call getValue once.

SnapPlanner uses a hard-coded resolution

It's currently implemented using this:

aikido::util::VanDerCorput vdc{1, true, 0.02};  // TODO junk resolution

TSR uses finite differencing for getJacobian.

Change this to analytic derivative.

Set up Travis builds

As mentioned by @mkoval in #12, we should set up Travis builds for automated testing. A few questions:

• Should we build standalone, catkin, or both? (I prefer standalone, but maybe both since we have aikido_rviz, etc.)
• Which version(s) of kido should we test against?
• Can we use Travis's container-based infrastructure? (https://docs.travis-ci.com/user/workers/container-based-infrastructure/)
  • Are the dependencies we need available on their whitelist (assuming we build kido from source)?
  • Alternative: docker images (https://hub.docker.com/_/ros/)?

Inconsistent naming: Sampleable/SO2Sampleable/SO2UniformSampler

Currently, the sampling interface is called Sampleable.
We define a header called SO2UniformSampler.
Inside it is a class called SO2Sampleable.

We need to standardize on these things. I posit the following naming convention:

• virtual and abstract classes adopt the adjective name: Sampleable
• concrete classes adopt the noun name: Sampler

This makes some sense when doing inheritance, as the interface describe what the class will do, but if a class does multiple things it may not be named after any of them, e.g.

SO2Sampler : Sampleable
TSR : Sampleable, Projectable

Interpolator vs a local planner

@cdellin raised a good point on Reviewable:

I feel like this interpolation idea, if removed from the StateSpace (which is a reasonable design decision), starts to look more like a local planner class which returns a trajectory (or path) between the two states. If we're already committing to an API for planners and for trajectories (i.e. plan between two states, get a trajectory, make queries on the trajectory such as different derivatives, etc) , it seems weird to duplicate that functionality here.

There may be an argument that planners will want to interpolate things very quickly, without for instance instantiating a path object at each step. But at least we should make the API identical, I'd think. And it would be cool if planners could be passed different types of local planners (here called interpolators) and concatenate the resulting path segments using the stock trajectory API.

Add first-class support for SE(2) and SE(3) state spaces

We have SE(2) and SE(3) state spaces implemented in aikido::statespace, but they are not fully supported by the rest of Aikido (e.g. distance metrics, interpolation methods, DART conversions). We need to add this functionality to make them first-class citizens.

@aaronjoh, @lgw903, and I discussed this offline and concluded that it is not possible to represent SE(2) and SE(3) as CompoundStateSpaces of SO(2), SO(3), and a real vector space because the group operation differs. However, we may be able to gracefully handle both of these by implementing an AffineStateSpace or an semi-direct product space. We should investigate this idea further before proceeding with implementation.

---

TODOs

• SE(2) distance metrics
• SE(3) distance metrics
• SE(2) interpolation methods
• SE(3) interpolation methods
• SE(2) DART conversions
• SE(3) DART conversions

Rename `r3` to match dart renaming.

When dart becomes kido:
r3 should become... you know what.

Define an interface for Project-able regions

Set up coveralls code coverage reporting

@jslee02 recently set up code-coverage auditing for DART in dartsim/dart#632. I think it makes sense to have here as well, since we would like to have the same high-quality code in aikido.

@jslee02 and @mkoval: can we make this happen?

CollisionConstraint needs setCollisionMask.

Current implementation does not have setCollisionMask.

RBoxConstraint's getConstraintDimension

Current implementation returns all dimension. This should be changed to return only the dimensions with bounded constraints.

evaluateDerivative returns derivatives in the wrong tangent space

evaluateDerivative on SplineTrajectory currently returns a tangent vector in the tangent space of the start of the segment, instead of the local ("body") frame. We should use the adjoint transformation to transform the first derivative into the correct frame. I am not sure what the general form of this transformation is for higher-order derivatives.

Setup online API documentation

AFAIK, there are two ways:

• using GitHub gh-pages branch: DART uses this way
  • Pros: the version of documentation can be selected arbitrarily
  • Cons: tedious initial setup, manual upload for new documentation
• CodeDocs
  • Pros: easy initial setup, automatic update per github commit
  • Cons: documents are generated only for the default branch (i.e., master)

Resolve sampling interface for point TSRs

The current aikido::constraint::SampleGenerator interface implies a strong distinction between a finite and infinite sequence of samples (via the getNumSamples() and canSample() methods). This distinction might be useful e.g. for planners which want to populate their graphs with unique start and goal vertices. The aikido::constraint::TSR class currently handles point TSRs (defined with no freedoms at the w frame) as an infinite sequence of identical samples, which may be a problem for planners which naively implement against the SampleGenerator API. We should decide how we'd like to handle such cases in a way that is clear to both (a) planners that may make use of SampleGenerators and (b) users who are constructing TSRs. This is important because I expect that all existing users of TSRs commonly use point TSRs to specify pose constraints, so if our software handles this differently (e.g. via a dedicated PoseConstraint class, which I'm in favor of in any case), we should at least document it well (e.g. by throwing a warning when a point TSR is constructed).

Travis build is broken with catkin_tools >= 0.4

Improve efficiency of allocState in GeometricStateSpace

Address this comment on the current allocState implementation:

This is actually a very inefficient way to allocate the state. It would be best to allocate a single block of memory for both the OMPL wrapper and the Aikido state, use placement-new to construct the OMPL state, and use allocateInBuffer to construct the Aikido state. Note that we would have to manually call the OMLP state destructor in freeState for this to work. It's fine if we don't do this for now - but we should file an issue as a note for future improvement.

Behaviour of `AprilTagsModule` with respect to marker topic

With reference to #24

We need to make a call on if waitForMessage is the right way to get marker data from the apriltags topic, or if it makes sense to subscribe to the 'apriltags` topic instead.

Handle failed samples by the StateSampler

Current the aikido::ompl::StateSampler throws a runtime_error if a valid state sample cannot be generated. Many times failed sampling does not indicate that planning will fail, and we would like to allow planners to handle the failed sampling and continue. or_ompl returns a state full of ``nan`s that will fail validity checking, rather than throw an exception. This issue is to discuss how we want to handle this in aikido.

Update Readme for DART installation

Instruct users to build/install DART from our fork. (Provide package?)

Implement remaining joint types

In aikido/aikido/include/aikido/statespace/dart/detail/MetaSkeletonStateSpace.hpp, we can implement the remaining joint types:

• ScrewJoint
• WeldJoint - a 0 dimensional RealVector: #146
• UniversalJoint - a compound joint of SO(2)xSO(2) (or R^1xR^1 if there are joint limits)
• EulerJoint - a compound joint of SO(2)xSO(2)xSO(2) (or R^1xR^1xR^1 if there are joint limits)
• TranslationalJoint2D - a 2 dimensional RealVector

aikido_rviz doesn't seem to properly install headers

When I do a sudo make install on aikido_rviz I get the following output

Install the project...
-- Install configuration: ""
-- Up-to-date: /usr/local/_setup_util.py
-- Up-to-date: /usr/local/env.sh
-- Up-to-date: /usr/local/setup.bash
-- Up-to-date: /usr/local/setup.sh
-- Up-to-date: /usr/local/setup.zsh
-- Up-to-date: /usr/local/.rosinstall
-- Up-to-date: /usr/local/lib/pkgconfig/aikido_rviz.pc
-- Up-to-date: /usr/local/share/aikido_rviz/cmake/aikido_rvizConfig.cmake
-- Up-to-date: /usr/local/share/aikido_rviz/cmake/aikido_rvizConfig-version.cmake
-- Up-to-date: /usr/local/share/aikido_rviz/package.xml
-- Up-to-date: /usr/local/lib/libaikido_rviz.so
-- Up-to-date: /usr/local/lib/python2.7/dist-packages/aikido_rviz.so

It seems to be installing the .so files, but doesn't install any header files. Consequently aikido_rviz_INCLUDE_DIRS appears to be empty in aikido_rvizConfig.cmake?

Additional Enum ConstraintType representing BoxConstraint

We now have EQUALITY, INEQUALITY. We may later want to add BOXCONSTRAINT as some optimizers handle a box constraint more efficiently than two inequality constraints.

Fix IkSampleableConstraint to handle finite pose constraints

The current aikido::constraint::SampleGenerator interface implies a strong distinction between a finite and infinite sequence of samples (via the getNumSamples() and canSample() methods). The current implementation of aikido::constraint::IkSampleableConstraint uses a generator for its underlying pose constraint. If this pose generator samples from a finite set of samples, then the IkSampleableConstraint generator will also return a finite set, even if there exists an infinite sequence of actual samples (e.g. because of kinematic redundancy). Even in the case that the IK set is finite (e.g. due to a 6-DOF arm), there are often multiple solutions for each pose sample.

Compiling without Bullet seems to cause issues.

I have a version of DART built without Bullet, and I am getting the following error when compiling AIKIDO:

CMake Error: The following variables are used in this project, but they are set to NOTFOUND.
Please set them or make sure they are set and tested correctly in the CMake files:
BULLET_INCLUDE_DIR

This might be a configuration issue in the exported DART config, I'm not sure yet.

Use custom predicate for comparing Eigen matrices

I wrote a custom GTest predicate CompareEigenMatrices for comparing Eigen matrices. We should replace all of our EXPECT_TRUE(a.isApprox(b)) calls with this predicate. There are a few advantages:

1. reduces code duplication
2. generates much more informative error messages
3. handles comparing against a matrix that contains zero elements.

Why must I still link manually against assimp?

I am trying to link against aikido, and I get the error:

undefined reference to symbol '_ZNK6Assimp8IOSystem12ComparePathsEPKcS2_

When linking against r3 earlier, the only way to fix this issue was to manually link against -lassimp in my CMakeLists.txt file. It seems I still have to do this. Why?

Implement ```getMaximumExtent```

Implement the getMaximumExtent function on aikido::ompl::GeometricStateSpace. This is used by OMPL planners to compute collision checking resolution. We are avoiding this by defining a custom MotionValidator. But there may be other uses of the function that could cause planning errors.

aikido_rviz causes IPython to exit uncleanly

I believe this is related to the roscpp node being forcefully exited:

terminate called after throwing an instance of 'std::system_error'
  what():  Invalid argument

Implicit metric in the tangent space

I think we are implicitly choosing a metric for state space operations using the tangent space. @mkoval had an idea to recover a metric by using exp and log maps but I think this is just recovering this hidden metric. I'll do some more research on this, but wanted to leave an issue open.

Move DART-specific constraints into a subdirectory

@cdellin pointed out that we put the DART-specific state spaces in the aikido/statespace/dart subdirectory, but make no such distinction for aikido/constraint. We should consider the same directory structure for aikido/constraint for consistency.

Move `test_resources` into `tests/resources`?

It seems like it would make sense to put the contents of test_resources into the existing test directory i.e. tests/resources.

There is already tests/gtest in there, so it seems reasonable to put other test dependencies in the sample place and avoid another top-level directory.

Define an interface for Sample-able regions

Set up automated style-checking for c++ code.

It can be really hard to validate style conventions when reviewing PRs. Let's try to set up automated style checking for aikido before the codebase gets too large.

Some promising options:

• https://github.com/alexmurray/cstyle
• http://astyle.sourceforge.net/
• https://bitbucket.org/verateam/vera/wiki/Home
• https://github.com/kunaltyagi/nsiqcppstyle

Inconsistent naming: SO2Sampler vs SO3UniformSampler

We should decide on one of the two.

Excluded external dependencies from coveralls

Our test coverage is currently being distorted by the presence of Hauser parabolic smoother. We don't use all of the functionality of the external library in our wrapper, so we clearly will not test all of it.

We should disable coveralls on these source files to get a more accurate reflection of our actual test coverage.

Fix TinyXML linking errors

@lgw903 and @Shushman both ran into missing TinyXML symbols. I need to figure out the right find_package incantations to add to CMakeLists.txt to make this portable.

Configure Travis to build with -Werror

We should have no build warnings

Add a default ResourceRetriever to Aikido

We generally want a CompositeResourceRetriever of a LocalResourceRetriever and a CatkinResourceRetriever. This is quite verbose to setup, so it would nice to wrap this in a helper function in aikido::util.

@Shushman got burned by this in #24.

Customizable `frame_id` for `aikido_rviz` InteractiveMarkers

Right now, aikido_rviz is hard-coded to publish in the map frame in two separate places.

https://github.com/personalrobotics/aikido/blob/master/aikido_rviz/src/FrameMarker.cpp#L62
https://github.com/personalrobotics/aikido/blob/master/aikido_rviz/src/BodyNodeMarker.cpp#L27

We should consolidate this to one location, and allow it to be changed to publish markers in another frame. This would handle situations where something else in TF corresponds to the kido root frame.