End Effector, Inverse Kinematics, and OpenSceneGraph

[mxgrey]

This pull request introduces three new major features. While it's not preferable for me to dump out so many features simultaneously, this is happening because the features were all developed in parallel and tested by using each other.

A very brief overview of the three new features is:

1. End Effector

We are extending the Jacobian features of the BodyNode class to an EndEffector class which is able to rigidly attach to BodyNodes with an arbitrary transform offset. The Jacobian API of the BodyNode has been moved into a new JacobianNode base class which is inherited by both BodyNode and EndEffector. The EndEffector also provides functionality for specifying support polygons, and in the future I intend to implement grasping context information into EndEffectors.

2. Inverse Kinematics

The new Inverse Kinematics module in DART is a force to be reckoned with. With the Inverse Kinematics module, you can customize the:

• Error Method
• Gradient Method
• Objective
• Null space objective
• And everything else

The way the IK module works is it sets up an optimizer::Problem with Objective and Constraint functions that it ties together. The Problem can be fully customized, but the IK module starts it out with a setup that is sufficient for most standard IK usage. The IK modules are clonable and can be cloned to operate on a new BodyNode/EndEffector.

There is also a HierarchicalIK class that can tie together a hierarchy of individual InverseKinematics modules. Modules that have lower priority in the hierarchy will be projected through the null spaces of the modules with higher priority.

3. OpenSceneGraph

OSG is an open source C++ wrapper for OpenGL that has a very nice API and a boatload of features, like mouse picking. The new osgDart library provides an interface between DART and OSG, and offers drag-and-drop functionality for SimpleFrames and BodyNodes.

Some of the nitty gritty details inside osgDart are not fully documented because much of it is liable to be replaced with the upcoming ShapeNode changes, so it would feel like a waste to spend time on that. Also, all of my changes to the Shape class will be migrated into the ShapeNode class once it's implemented, so if those changes look unappealing, don't worry; they won't last long.

Other notes

I've split out the nlopt, ipopt, and snopt portions of the optimizer namespace into their own libraries so that they're not hidden in the source code. I haven't updated the package management to support either of them yet, because I don't know yet how DART does package management, but I think it would be good if those could be packaged up and offered on the repos so they can be utilized without needing access to the source code.

I wrote a simple GradientDescentSolver that optimizes using stochastic gradient descent. This way we have a solver native to DART instead of requiring a third party library. The GradientDescentSolver is what is used by default in the InverseKinematics modules.

The SNOPT extension is definitely clobbered right now. Tomorrow I'll try to take a pass at it and attempt to make it functional, but since I don't have access to SNOPT it probably still won't work even after my fix attempts, so I'll need someone with SNOPT access to get it fully functional.

Atlas Puppet

A good summary of this pull request can be found in a single app: osgAtlasPuppet, which you can find in osgDart/examples/osgAtlasPuppet.cpp. The app lets you puppeteer an Atlas by clicking and dragging. You can see Atlas's support polygon and watch it try to solve its balance constraint.

Here's a screenshot from the app:

The green polygon is its support polygon, the blue sphere is its center of mass (projected onto the support polygon), and the green sphere is the centroid of the support polygon. You can also turn on interactive targets for more precise control over the end effectors:

You can even toggle whether each foot can be used as a support polygon, and then have it balance on one leg:

[jslee02]

InverseKinematics::GradientMethod::evalGradient?

[mxgrey]

Right, the function name was changed after this line was written. We should probably look into having dterr and dtwarn automatically include the current function name in their printout.

[jslee02]

I found that there is a standard predefined macro for function name, __func__, but it seems there is no one for class name even of non standard macros.

[jslee02]

Would it be problematic if initialize() is called here?

[mxgrey]

The comment isn't quite right. initialize must be called immediately after the construction of a HierarchicalIK instance. This is why the derived classes have create() functions and protected constructors.

[jslee02]

I see. I will update the comment as:

  // initialize MUST be called immediately after the construction of any
  // directly inheriting classes.

[jslee02]

Is there a reason why the file name osgDart/utils.h begins with lowercase? We've used file names that begin with uppercase except for the automatically generated header files such as dynamics.h and dart.h.

[mxgrey]

Is there a reason why the file name osgDart/utils.h begins with lowercase?

I don't have a problem with making it uppercase. To me, an uppercase filename implies that it contains the implementation of some class, but I guess in DART we haven't been using the convention (e.g. math/Geometry.h and math/Helpers.h.

How about this?

It's more verbose than I would have liked (I'm a fan of the way Eigen can accept an array pointer), but I suppose it would work for lack of a better alternative.

[jslee02]

I found that osg::Matrix also takes an array pointer. So this works for Eigen::Isometry3[d/f]

  return osg::Matrix(tf.data());

but it doesn't work for Eigen::MatrixBase<Derived> since it doesn't have data() member function.

[mxgrey]

We would probably also need to worry about row and column ordering. It
doesn't look like you can specify those parameters for the
osg::Matrix(double*) constructor.

The explicit version is probably the safest thing to use.

On Mon, Sep 28, 2015 at 11:05 PM, Jeongseok Lee notifications@github.com
wrote:

I found that osg::Matrix also takes an array pointer. So this works for
Eigen::Isometry3[d/f]

return osg::Matrix(tf.data());

but it doesn't work for Eigen::MatrixBase since it doesn't have
data() member function.

—
Reply to this email directly or view it on GitHub
#461 (comment).

[jslee02]

One possible way is to provide another Eigen::Matrix<Scalar, 4, 4> version of eigToOsgMatrix with array pointer and leave Eigen::DenseBase<Derived> version verbose.

So the functions would be:

//==============================================================================
template<typename Scalar>
osg::Matrix eigToOsgMatrix(const Eigen::Transform<Scalar,3,Eigen::Isometry>& tf)
{
  return osg::Matrix(tf.data());
}

//==============================================================================
template<typename Scalar>
osg::Matrix eigToOsgMatrix(const Eigen::Matrix<Scalar, 4, 4>& M)
{
  return osg::Matrix(M.data());
}

//==============================================================================
template<typename Derived>
osg::Matrix eigToOsgMatrix(const Eigen::DenseBase<Derived>& M)
{
  return osg::Matrix(
        M(0, 0), M(1, 0), M(2, 0), M(3, 0),
        M(0, 1), M(1, 1), M(2, 1), M(3, 1),
        M(0, 2), M(1, 2), M(2, 2), M(3, 2),
        M(0, 3), M(1, 3), M(2, 3), M(3, 3));
}

[jslee02]

We would probably also need to worry about row and column ordering.

It seems osg and Eigen use the same order (I tested it with simple code). But I'm fine with explicit version as well. Then let's use the explicit version and consider again when this comes as serious problem in the future.

[mxgrey]

It's probably not worth having multiple versions, since I don't really
imagine that would accomplish anything for us.

--Grey

On Mon, Sep 28, 2015 at 11:11 PM, Jeongseok Lee notifications@github.com
wrote:

One possible way is to provide another Eigen::Matrix<Scalar, 4, 4>
version of eigToOsgMatrix with array pointer and leave
Eigen::DenseBase version verbose.

So the functions would be:

//==============================================================================template
osg::Matrix eigToOsgMatrix(const Eigen::Transform<Scalar,3,Eigen::Isometry>& tf)
{
return osg::Matrix(tf.data());
}
//==============================================================================template
osg::Matrix eigToOsgMatrix(const Eigen::Matrix<Scalar, 4, 4>& M)
{
return osg::Matrix(M.data());
}
//==============================================================================template
osg::Matrix eigToOsgMatrix(const Eigen::DenseBase& M)
{
return osg::Matrix(
M(0, 0), M(1, 0), M(2, 0), M(3, 0),
M(0, 1), M(1, 1), M(2, 1), M(3, 1),
M(0, 2), M(1, 2), M(2, 2), M(3, 2),
M(0, 3), M(1, 3), M(2, 3), M(3, 3));
}

—
Reply to this email directly or view it on GitHub
#461 (comment).

[mxgrey]

Then let's use the explicit version and consider again when this comes as serious problem in the future.

👍

On Mon, Sep 28, 2015 at 11:14 PM, Jeongseok Lee notifications@github.com
wrote:

We would probably also need to worry about row and column ordering.

It seems osg and Eigen use the same order (I tested it with simple code).
But I'm fine with explicit version as well. Then let's use the explicit
version and consider again when this comes as serious problem in the future.

—
Reply to this email directly or view it on GitHub
#461 (comment).

[jslee02]

Finally !! 👍