This introduces a way to simplify and shorten paths obtained from the planners used in AIKIDO using OMPL methods.

AIKIDO Planner -> untimed trajectory -> simplifyOMPL -> simplified and shortened untimed trajectory.

-> The feature has been currently introduced same place as the planning methods although really a post-processing step. Is there a better place for this than here, currently?

This is the first attempt at a perception module for AIKIDO.

• Introduction of aikido_perception as a package
• Abstract class in PerceptionModule.hpp
• Implementation in AprilTagsModule.hpp/cpp

This change is

@psigen @mkoval @cdellin This is initial draft of above classes based on discussion with @mkoval last Friday. It's probably not a working code yet, but since it involves API changes and moving namespaces, I thought I'd have it checked early on. Please add any comments!

P.S.: Not familiar with C++11 convention so it'd probably have some mistakes there too.. =P

A first pass at a Kinbody Parser under aikido::util. Points to note:

1. Only works if the KinBody has one Body element with 1 or 2 Geom elements, and Render and Data both defined, and refer to some file.
2. I've mostly followed the template in SkelParser
3. A problem - The plastic bowl and glass are two files that are read in just fine, but I get a strange error (originating from dart::dynamics::MeshShap::loadMesh) upon trying to load say the Pop tarts or kinbody. The error message is:

Warning [MeshShape.cpp:349] [MeshShape::loadMesh] Failed loading mesh 'package://pr_ordata/data/objects/kinova_tool.stl 0.1'.
Error [KinBodyParser.cpp:423] [KinBodyParser] Fail to load model[kinova_tool.stl 0.1].

Sometimes, even without the error message, the Pop tarts looks like:

This change is

TODOs:

• [x] Explicitly instantiates for frequently used dimensions
• [x] Use extern template to force the compiler not to instantiate for the explicitly instantiated types
• [x] Decide name: template <int N> class Rn vs template <int N> class R (I prefer R<N>)
• [x] Support dynamic size

Resolves #152

This PR moves the declaration of smart pointers to separate headers to avoid the circular dependencies.

Before creating a pull request

• [x] Format code with make format

Before merging a pull request

• [x] Set version target by selecting a milestone on the right side

Adds YAML parser for Eigen datatype, originally implemented by @mkoval . Copied from kenv. Allows operations such as node.as<Eigen::VectorXd>, node.as<Eigen::Isometry3d> , etc..

This PR adds BarrettHandPositionCommandExecutor and renames several classes. This is necessary/convenient to have a non-simulation counterpart ros::RosBarrettHandPositionCommandExecutor which has the same execute method signature as the existing sim-mode-executor.

• adds an abstract class BarrettHandPositionCommandExecutor which contains execute method for four finger goal positions (3 primal, 1 spread).
• renames existing Barrett*Executor classes to SimBarrett*Executor
• removes collideWith param from their execute method to match the abstract class's execute signature
• addscollideWith to the contructors for SimBarrett*Executor classes
• addssetCollideWith methods to SimBarrett*Executor classes

One question for @mkoval : shouldn't we expose step (or spin) method for all Executors, including the higher level abstract classes? (applies to #149 as well)

This PR branches from #111. I have one question. It seems like the use of DOF and Joint is mixed here. convertJointTrajectory asks for _space->getNumSubSpaces() which would return the number of Joints, but then considers these as DOFs. This would work fine (except for name-matching) if all joints are single-dof joints. Is that the case since we're using JointTrajectory, not MutliDOFJointTrajectory?

@mkoval @ClintLiddick Please confirm. If that's the case, I'll change the variable names, docstring, and name-checking part to clarify this.

This PR contains:

• TrajectoryExecutor
• TrajectoryResult
• KinematicSimulationTrajectoryExecutor
• Tests for KinematicSimulationTrajectoryExecutor.

This change is

Changes:

• Updated the style configuration to be close to the C++ style in STYLE.md as much as possible.
• Applied it to util's header(todo) and source files as the first stage
• Setup CI to check the format

Reference:

• Clang-Format Style Options guide

TODOs:

• [x] breaking-constructorInitializers-before-comma

// Good!
: mSeq(seq)
, mN(-1)
, mFinalIter(false)

// Bad
: mSeq(seq),
  mN(-1),
  mFinalIter(false)

• [x] Install clang-format-3.8 in pr-cleanroom

Previously, in the case of an empty trajectory (which is possible for an Offset planner with 0 offset or a configuration planner with goal == start), the resulting trajectory was length 0.

In this case, any postprocessor should return a valid (but empty) trajectory.

However, Kunz's Trajectory class initially timed the trajectory using the velocity without checking for 0 velocity. If there is 0 velocity but non-0 position, the trajectory is invalid (this should never happen). But if there is 0 velocity AND 0 change in position, the trajectory is valid but empty, and Kunz shouldn't perform the division (which results in a NaN endTime, which in turn leads to a VERY LARGE sequence of waypoints when naively constructed with aikido::common::StepSequence, which in turn causes the program to loop nearly infinitely).

In addition, the Robot class has been modified to gracefully handle valid and empty trajectories (namely: not executing them).

Before creating a pull request

• [ N/A ] Document new methods and classes
• [x] Format code with make format

Before merging a pull request

• [x] Set version target by selecting a milestone on the right side
• [ ] Summarize this change in CHANGELOG.md
• [x] Add unit test(s) for this change

Builds on top of #614. Adds corresponding controller operations (load, start, stop) to the executors operations (register, activate, deactivate). Corresponding hardware mode switching takes place if a joint mode controller is specified.

Before creating a pull request

• [ ] Document new methods and classes
• [ ] Format code with make format

Before merging a pull request

• [ ] Set version target by selecting a milestone on the right side
• [ ] Summarize this change in CHANGELOG.md
• [ ] Add unit test(s) for this change

There are a few key issues with the RosJointStateClient: https://github.com/personalrobotics/aikido/blob/master/src/control/ros/RosJointStateClient.cpp

(1) There's no reason to lock the mSkeleton mutex in spin, since we don't read or write from it at all.

(2) There is not reason to store the mSkeleton pointer anyway, since we never use it.

(3) We should decide on a better interface. Either [1] we should pass in a MetaSkeletonPtr (assuming the parent skeleton is already locked) and set the corresponding (subset of) DoFs directly, or [2] we should just return a copy of the buffer as a map from DoF name to position, and have the client set their own MetaSkeleton accordingly.

The URDF format does not support cycles in the Joint Tree (for example, 4- or 5-bar linkages). Instead, the kinematics/dynamics of these relationships are specified using the <mimic> joint declaration, where the position of a joint is set to be some linear function of another joint (i.e. joint2 = m * joint1 + offset). See https://wiki.ros.org/urdf/XML/joint

The URDF loader in DART (https://github.com/dartsim/dart/blob/main/dart/utils/urdf/DartLoader.cpp) handles this by registering the joint as normal, and then just setting the ActuatorType to "MIMIC", which is used during dynamic simulation to set the set-point of a given joint based on the position of its corresponding mimic joint.

However, Aikido does not use DART's dynamic simulation. We set the positions/velocities of joints ourselves to do Kinematic simulation and collision detection. Therefore, there is no obvious way to handle mimic joints. And the additional Degrees of Freedom that are registered lead to a StateSpace that doesn't reflect the actual configuration space of the robot.

This makes it impossible to plan and execute a trajectory on, for example, ADA's grippers, which have 4-bar (Gen2) or 5-bar (Gen3) linkages respectively.

Existing workaround: in the URDF, set the joint type of the mimic joint to "fixed". This allows the StateSpace to be constructed only considering the base joint. However, this forces the gripper to go into collision with itself when closed. So you need to disable all self-collision in the hand. This works, and is what we use for ADA Gen2, but it leads to a non-realistic collision model for the gripper, which affects the ability of planning algorithms to accurately plan around objects in the environment.

How We Can Fix This: Update the MetaSkeletonStateSpace Object https://github.com/personalrobotics/aikido/blob/master/src/statespace/dart/MetaSkeletonStateSpace.cpp

1. When creating the state space, make sure that the mimic joints are not registered as joint sub-spaces, and update the Properties object accordingly (i.e. the number of DoFs in the properties object = MetaSkeleton Dofs - Mimic Dofs)
2. For convertStateToPositions and setState, do the mimic multiplier+offset calculates to set the positions of the MetaSkeleton (or, respectively, return the positions Eigen Vector) accordingly.
3. for convertPositionsToState, accept an Eigen Vector of either the length of the number of true DoFs, or the length of the number of MetaSkeleton Dofs (as a utility for passing in MetaSkeleton.getPositions()). If the latter, check to make sure that the mimic constraints are upheld (to appropriate float precision) before creating the state object.
4. Make sure that all relevant calls for setting the state of the MetaSkeleton (esp. in collision, control, and robot) do it through the statespace rather than calling setPositions directly

Currently, planToEndEffectorOffset functions only perform end-effector translations.

But the user may conceivably want to perform rotations as well.

There is an existing PR that is closed for now, but is fairly close to being done. https://github.com/personalrobotics/aikido/pull/545

Currently, we almost exclusively use the CatkinResourceRetriever, which only accepts the package:// schema.

There are other ResourceRetrievers we can use that can accept other schemas (e.g. file://, dart://).

DART has a CompositeResourceRetriever object that is more flexible, delegating to individual retrievers based on the schema (or based on precedence, i.e., try one retriever, and then move on to another if the first fails).

To simplify the process of using ResourceRetrievers, we can add a utility function to the aikido::io module that returns a CompositeResourceRetriever that can handle at least package://, file://, dart:// and potentially no schema (i.e. file path relative to execution point) to use as a default.