ipab-slmc / exotica Goto Github PK

Use initialization classes instead of multitude of init functions.
Use cmake to generate these classes in a similar way the ROS messages are generated.
Use wrappers to enable initialization from python, xml, matlab, JSON, ...

I have have looked into cleaning up the exotica repo to remove big files and to remove our current working code (DRM, drake interface, OMPL customization for whole body planning, ...).
Strictly speaking, current code should only contain current files and their history. I would like to then open source the clean code but also keep working on the clean code.

There are the following options for cleaning up the code:

1. Leave the code as it is. The code currently in the history will remain there but all new development will happen in a different private repo (which already exists).
2. Remove all unwanted files from the history, removing any legal issues that these files may cause. The SHA ids will be rewritten, so current users will be affected and they will all will have to pull the version of the repo. This will leave behind some code that may reference the deleted files. These invalid references have to be tracked down manually to be removed they can probably be kept in the history, because the implementation has been removed. Some old state of the repo may not compile/run but this won't affect anyone using the code.
3. Remove the history completely and start EXOTica with a blank slate. This will also result in all the users having to pull the updated repo.

Licensing has similar-ish issue because git history contains code without any licenses. This is especially troublesome because the licences should be included in every source file. According to GNU (paraphrased on a forum):

The lack of license and copyright information basically means:
you are not allowed to do anything with the code, well, except for
looking at it if its on some public hosting site.

1. According to this, adding the license in a new commit should be enough to cover the code legally.
2. Another option would be to edit the history to add the licenses to every file when it was added into the repo (possible with a script).

I'm leaning towards option 1 for the licensing and also option 1 for cleaning up the repo, with the addition of removing accidentally/incorrectly committed files (there is about 10 of them).

@yimingyanged @iamwolf @mauricefallon any suggestions?

Redefine error checking based on SUCCESS/FAILURE values and the ok() utility function with exceptions.

Use try/catch mechanism If the return value should be converted to something useful that doesn't halt the execution, e.g. solver failed >> report error code.

There should still be an option to print coloured messages with line numbers when exception gets thrown.

Either use ros pluginlib or find a non-ros alternative (maybe boost?)

• Historically, a <Urdf> node underneath <Kinematica> specified the rosparam on which the URDF was. These are still in all initialisation files, but unused.
• Instead, robot_description is hard-coded in the Scene.cpp initialisers.
• In some cases, e.g. for the husky, the robot_description channel contains another description than the one we'd like. [We're seeing rviz-crashes due to the differences for instance]
• We'd like to be able to set the param on which to look for the description as one of the initialisation flags, as it used to be. E.g. /exotica/robot_description

Hence,

a) would we like to reactivate use of the <Urdf> tag - or perhaps another tag or directly in a server parameter (where we now have packages and urdf file names inside packages)
b) would we like to delete all instances of the <Urdf> tag remnants and switch to something else?

cc @VladimirIvan

It calls different scenes so returned results are different, cf:

Code:

bool is_valid;
is_valid = prob_->getScene()->getCollisionScene()->isStateValid(q);
std::cout << "Direct update without self check: " << is_valid << std::endl;
is_valid = prob_->getScene()->getCollisionScene()->isStateValid(q, true);
std::cout << "Direct update with self check: " << is_valid << std::endl;
prob_->getScene()->getCollisionScene()->update(q);
is_valid = prob_->getScene()->getCollisionScene()->isStateValid();
std::cout << "Two-step update without self check: " << is_valid << std::endl;
prob_->getScene()->getCollisionScene()->update(q);
is_valid = prob_->getScene()->getCollisionScene()->isStateValid(true);
std::cout << "Two-step update with self check: " << is_valid << std::endl;
prob_->getScene()->getCollisionScene()->update(q);
is_valid = prob_->getScene()->getCollisionScene()->isStateValid(true, 0.01);
std::cout << "Two-step update with self check and 1cm dist: " << is_valid << std::endl;

Result: (1 = valid, 0 = in collision)

Direct update without self check: 0
Direct update with self check: 0
Two-step update without self check: 1
Two-step update with self check: 1
Two-step update with self check and 1cm dist: 1

I can't find a way to pass the parameters in the OMPLImplementationInitializer from the XML - e.g. the Timeout (it's not a solver, but the implementation of a solver). Overall, it may make sense to move these parameters to the OMPLsolverInitializer?

Cf. https://github.com/openhumanoids/exotica/blob/master/exotica/src/KinematicTree.cpp#L434-L454

Replace std::mapstd::string,exotica::Scene_ptr with just exotica::Scene_ptr.

This variable and corresponding getters/setters appear in a lot of depending source files - refactor accordingly.

The kinematic scene works with a group of joints. The remaining joints have to be set somehow.

• Add a method for setting the whole robot state.

Server holds a shared_ptrros::NodeHandle instead of just the handle

Currently broken and deactivated after #59

Use Lapack's dpotrf and dpotri for matrix inversion (see AICO implementation).

The kinematic solver currently outputs Eigen vectors and matrices (for eff. poses and Jacobians respectively). The solver does use KDL internally and the task maps can easily use the KDL as input (KDL::Frame and KDL::Jacobian). This would make the kinematic solver simpler and the operations in task map more transparent (e.g. dealing with frames instead of indexing into matrices).
The task map will have to convert the output into a Eigen::Vector/Matrix in the end but this will make the operation explicit (unlike doing it implicitly in the kinematic solver).

The IK solver has been accumulating a lot of code.
The implementation should only be a couple of lines long.

Unused code should be removed. Useful code should be simplified. Variables and methods have to named more appropriately.

Would it be a good idea to add a default instantiation of problem parameters that are not passed in the XML? E.g. for the weighting W in the UnconstrainedEndPoseProblem use ones(N) or N..1 as the default initialisation here.

This would make W optional instead of required (here).

As discussed

Anything apart from updating the collision scene with planning scene messages as in exotica_json and using binary collision checking (bar the inconsistency issues laid out in #39) is currently broken. Issues among others:

• the fcl links and geometries are not being updated

• Indigo currently uses FCL 0.3.x (0.3.4 as of writing)
• Kinetic uses FCL 0.5, with the requirement for Octomap 1.8
• Upstream FCL is on track to release 0.6 which has narrow phase signed distance support.

There are a ton of fixes regarding computing distances including signed distances in the current head and I consider it worth using the new FCL version.

Of particular note:

• flexible-collision-library/fcl#172.
• Issues and fixes discussed in flexible-collision-library/fcl#97
• Capsule support in FCL (MoveIt tracking issue: moveit/moveit#158)

• Move the collision scene functionality into a task map.
• Use KinematicTree and FCL directly.
• Find an easy way how to populate the scene/map from MoviIt planning scene.
• Handle attaching/detaching objects to/from the robot.
• Use this task map in the SamplingProblem

Running list of current issues with the Exotica Python wrapper (WIP):

• 1. Python 3 support - should be by easy by setting -DPYBIND11_PYTHON_VERSION=3, however, the libraries are not correctly picked up. I suspect this to be due to the non-standard why in which pybind/cmake/pybind.cmake is doing the python lookup
• 2. Eigen version - requires >3.2.7 which is non-standard on 14.04 - need a way to deal with it
• 3. The macro pybind_add_module should not use custom commands but the CMake native versions of creating folders and copying files; they also prevent system-specific issues
• 4. Change submodule to externalproject if no changes
• 5. Perhaps we should have a configure step to checkout header only externals such as pybind11 and Eigen to make sure we don't run into these issues?

Also, I would like to suggest to rename exotica_py to pyexotica.

This will replace kinematica.

• Remove unused code.
• Remove or improve the EParam handling.

The CMake macro logic for the initializers doesn't work with catkin tools if configured to not use an install directory as the files are generated in the devel and not the source tree whereas ${catkin_INCLUDE_DIRS} will return the source tree path. Generating into the source tree is not a good CMake option either.

Questions thus are how to handle non-install configured workspaces?

[The issue arises by just building exotica - when building the examples it will fail as it cannot find the generated {something}Initializer.h file]

Error handling, e.g. catch if a link does not exist and give helpful warnings/messages. (Longer standing issue to track issues as they pop up)

• Kinematic frame requests - report if a frame does not exist rather than having the map.at( throw a cryptic error - here

As discussed, after #125 gets merged the currently existing getDistance(object_name, object_name) function won't work anymore. Furthermore, the collision objects are now named link_name_collision_number-of-shape which is hard to query, better would be a map so that we can use e.g. lwr_arm_7_link, shelf again

Thus, follow-ups on #125:

• Add getDistance(name, name) back
• Add way to resolve link name to the collision shapes internally, i.e. outside query uses names of robot links or world objects
• Add margin/safe distance back

Symptom: problem.update wrongly updates the floating-base when not part of the state

Here's how to reproduce this:

1. Consider a floating-base robot where the current planning group does not include the virtual floating joint, i.e. the state is a set of other joints, but fixed w.r.t. to the world
2. Using scene.update(x) or problem.update(x) will incorrectly update the planning scene internal state as the CollisionScene::update function first updates the floating base (which in Kinematica is correctly stored as floating even though for this problem is not) and then the joint values offset by the virtual floating joint DoF.

The cause for this is that the BaseType of the Scene is incorrectly defined by the BaseType of the KinematicTree and then gets passed to CollisionScene.

Solution: The Scene/CollisionScene needs to define their own BaseType based on the planning group used on instantiation.

As currently, the reduction of the OMPL safety margin is only carried out for the goal state (and there's a bug in that which I am currently fixing) - during the sampling itself the original margin is used which leads to failures in cluttered scenarios.

We probably should consider removing the damping of the safety margin altogether and let people specify it per problem via the Margin initializer parameter.

E.g. Google

As discussed via WhatsApp yesterday, we need a way to pass ROS messages through the Python wrapper to Python-wrapped functions. We are currently investigating the use of serialization/deserialization of messages through buffers, however, they currently segfault at the conversion on the C++ side. Additionally, StringIO isn't available in Python3 anymore: http://python-future.org/compatible_idioms.html#stringio

Build on #28

Wrap the initializer classes.
Keep other wrapper code minimal.
Use Boost::Python.

The inputs/outputs/parameters for task maps and task definitions are currently allocated by the solver (see here).
This makes the code hard to read and easy to introduce difficult to track memory access issues.
Each class should hold its own data or get it passed as an argument in a function.

To remove this feature (which was mistakenly added in the past):

• Make task map output the task space value on update.
• Move map/definition parameters back to the map/definition.
• Remove registerPhi, registerJacobian, ...
• Update existing solvers/problems.

The joint group used for planning is currently defined in the initializer of the Scene.
The same information is already specified in the SRDF though.
Using the SRDF set of joints would make the initializer shorter and put the joint group in one unique place (the SRDF). This will also make it easier to use the MoveIt features more directly, e.g. setting the joint state, joint state indexing, joint limits, setting the unspecified joints (robot state), ...

Using several Maps in one problem, e.g. CoM+EffFrame works fine. As soon as more than one EffFrame is part of a problem, memory corruption occurs - perhaps due to the changing size from the rotational part?

and link names, e.g. for use in explicit collision checking

testing_pkg/include/Exotica is capitalized. This is inconsistent with the rest of the packages.

Problems are currently tightly bound to solvers (AICOproblem, IKproblem, ...).
Problems should be independent of solvers. The code is already separated and moved into the core library where the problems will live. A proper naming should be decided. We need a discussion about problem types, how to categorize them and how to give them meaningful names.

The only action in code is to then rename the existing problems based on this discussion.

There are three ways to initialize EXOTica classes in the old code:

1. From XML: initDerived() for base classes.
2. From JSON: reinitialise() for specific problems.
3. Manual: initialiseManual for Scene.

These are now deprecated in favour of the new initialization. All references to old initialization should be removed.

Switch from boost::shared_ptr to std::shared_ptr

• testing_pkg/include/Exotica - is non-functional and should be updated.
• testing_pkg/include/Kinematica - the package has been removed, so should be the test.
• testing_pkg/include/Exotica_old - this is no longer relevant.

• Add TaskMaps to the Sampling problem.
• Add thresholding mechanism to turn task error into a binary variable.
• Extend the validity checker to use this binary variable.

Implement FK caching.

MotionSolver::Solve takes the start state q0 as an argument at the moment.

• Set start state as a parameter of the PlanningProblem.
• Use the whole robot state (including uncontrolled joints.

After implementing #46, the problem can hold everything necessary for handling a task map within its scope. The task definition is then redundant. The precision parameter (rho) can moved directly into the problem together with any time parametrization and other specifics.

Action: remove task definition class and move some of its functionality into the problems (where appropriate).

The number of controlled joints can be obtained using:

Scene::getNumJoints()

or

UnconstrainedEndPoseProblem::N
UnconstrainedTimeIndexedProblem::N

The result is the same. Consolidate the calling convention!

Write an interface for printing messages (info, warning, error, highlight,...).
Implement modules to display these messages in different environments (ROS, Python, Matlab, LCM).
Create a similar interface for plotting a visualization (e.g. display mesh, plot pose, ...).

The Scene's PlanningScene joint state (controlled and "uncontrolled") are only updated via problem.update(q). Updating the model state via setModelState() or setModelStateMap() does not update the planning scene states.

The catkin wrapper should either be renamed to pybind11_catkin to avoid clashes and use external project to checkout the source, or upstream the catkin related changes.