Added Joint::isCyclic to mark SO(2) topology

[mkoval]

This implements the isCyclic function that we discussed in #433.

[mxgrey]

I wonder about the implementation of isCyclic for FreeJoint and BallJoint... The three generalized coordinates of a log map are deeply coupled, so I wonder if it would be better if all three generalized coordinates were all considered cyclic or non-cyclic together instead of individually.

[mkoval]

As per our discussion in #433 I added an isPositionLimited method to Joint. Since getPositionLowerLimit and getPositionUpperLimit both return 0 for out of range indices, I made isPositionLimited return true for consistency. The same reasoning applies to ZeroDofJoint.

@mxgrey Can you explain coupling breaks the symmetry of FreeJoint and BallJoint? DART uses minimal coordinates, so DegreeOfFreedoms should always be uncoupled, even if they impose very unintuitive constraints on the pose of the child BodyNode.

Tagging @pkv @siddh in case they want to chime in on this.

[mkoval]

I just noticed that there is already a no-arg function called getPositionLimited, which has nothing to do with the isPositionLimited function that I implemented:

/// Get whether enforcing joint position limit
///
/// The joint position limit is valid when the actutor type is one of
/// PASSIVE/FORCE.
///
/// \sa ActuatorType
bool isPositionLimited() const;

This has the potential to be very confusing. In an ideal world, I would like to rename isPositionLimited to isPositionLimitEnforced, but that would break backwards compatibility. Maybe we can rename the function I added to hasPositionLimit?

This is even a bigger problem on DegreeOfFreedom: both functions take no arguments. 😆

[mxgrey]

Ouch. hasPositionLimit does sound good.

I think we can make an isPositionLimitEnforced function and then mark isPositionLimited as deprecated, because it will definitely be confusing with hasPositionLimit.

[mxgrey]

DART uses minimal coordinates, so DegreeOfFreedoms should always be uncoupled, even if they impose very unintuitive constraints on the pose of the child BodyNode.

The thing that gives me pause is that I'm not totally sure if a single BallJoint coordinate can really be cyclic if the other two are limited. Maybe it can, it's just not immediately obvious to me. With Revolute or Euler joints, it's obvious that the individual coordinates will just wrap around, but I don't know if that works for log maps. Log maps definitely do wrap, but I think all the coordinates wrap in sync with each other. I would have to look more into the geometry behind it to be sure, though.

Basically, I don't think the three coordinates are truly separable.

[mkoval]

I added isPositionLimitEnforced/setPositionLimitEnforced and deprecated isPositionLimited/setPositionLimited in e5fc7c1. I can't rename mPositionLimited in Properties because it's a member variable, so we're stuck that name until the next major revision.

I see your point about BallJoint. I'm not sure either..

[mxgrey]

I've opened an issue for mPositionLimited as a reminder once 6.0 rolls around. I'm still not sure about BallJoint and FreeJoint, but I think the API is good, and we can patch the behavior in the future if it turns out that our geometric assumptions about the independence of the coordinates are ill-founded. For now, the pull request looks good to me 👍

[mkoval]

@mxgrey and I discussed this offline.

We definitely can't represent the topology of BallJoint and the rotational components of FreeJoint with this scheme because they fundamentally live in SO(3), not SO(2). We must these specially, at the Joint rather than the DegreeOfFreedom level, when constructing a state space for planning.

We still think it is useful to add hasPositionLimit and, possibly, isCyclic, perhaps with a few tweaks, to the DART API. We just have to be careful about how we define "cyclic".

[mkoval]

I modified BallJoint and FreeJoint in ede1cbc to only have isCyclic return true if all DOFs are unbounded. I also changed the description of isCyclic in b117075 to:

Gets whether this DOF is cyclic. Returns true if and only if an infinite number of DOF positions produce the same local transform. If this DOF is part of a multi-DOF joint, then producing a cycle may require altering the position of the Joint's other DOFs.

@mxgrey @psigen What do you think?

[mxgrey]

I'm in agreement 👍

[jslee02]

How about this:

return (std::isfinite(mSingleDofP.mPositionLowerLimit)
        || std::isfinite(mSingleDofP.mPositionUpperLimit))

Less use of not operator seems more clear to understand to me. std::isfinite is opposite function to std::isinf except for NAN value case. Both of them return false for NAN but I think NAN value is meaningless anyways.

[jslee02]

The implementation looks good based on current DART functionality and API.

I have a side question. Does OMPL have another classes to represent geometric configuration space other than SO(2) such as SE(2), SO(3) and SE(3)? I'm considering to add more base classes for joins that has geometric configuration spaces.

[mkoval]

Yes, OMPL has real, SO(2), SE(2), SO(3), and SE(3) state spaces built in.
They also provide a class that represents the Cartaesian product of other
state spaces. In fact, this is how the SE(2) and SE(3) state spaces are
implemented internally. If none of these fit, you can also implement a
completely custom state space by inheriting from an interface.

When possible, I'd like to do the conversion from DART DegreeOfFreedoms to
single-DOF OMPL state spaces. When this is not possible, e.g. for BallJoint
and the rotational axes of FreeJoint, we will convert the entire joint to a
multi-DOF state space as a single unit.

In that case, we will have to throw an error if you try to plan with a
subset of the coupled DOFs.

On Sun, Jul 12, 2015 at 8:44 PM Jeongseok Lee notifications@github.com
wrote:

Does OMPL have another classes to represent geometric configuration space
other than SO(2) such as SE(2), SO(3) and SE(3)? I'm considering adding
more base classes for joins that has geometric configuration spaces.

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

[jslee02]

Great. I should take a look at the built-in state spaces of OMPL. Thanks.

[mkoval]

I fixed the comment style and switched from isinf to isfinite. Are we good to merge once the Travis and AppVeyor builds finish?

[jslee02]

Sure, I'm merging.