Convenience setter functions for positions/velocities/accelerations of FreeJoint

[jslee02]

Since a BodyNode whose parent joint is FreeJoint has full degrees of freedom, we can set its transformation, velocity, and acceleration without any concern of constraints in kinematics level. However, current API may not convenient to set them because they should be set through generalized coordinates that requires some conversion between them. Although we have some utility functions thanks to #322, it still requires more lines to handle transformations between parent/child BodyNode and joint and doesn't take velocity and acceleration into account.

This PR allows us to set generalized positions/velocities/accelerations of a FreeJoint given transformation, spatial velocity, and spatial acceleration. The new functions looks similar to SimpleFrame's ones.

We can consider another setter functions that takes classical terms like rotation, position, [linear/angular][velocity/acceleration]. Furthermore, it might be able to set these terms through BodyNode when the parent joint is FreeJoint, but I'm not sure if this makes sense in the point of API view.

Also, we can consider similar functions for other joints types (except for WeldJoint). For the other joints, we might need single-joint-level inverse kinematics to find best generalized coordinate terms given spatial terms. Would this be straightforward with #461 @mxgrey?

[mxgrey]

For the other joints, we might need single-joint-level inverse kinematics to find best generalized coordinate terms given spatial terms.

I have two concerns about this:

1. Using Jacobians, the process for this would be iterative, and it isn't always obvious how many iterations to use when attempting it, especially when the given target is out of reach. Although, the iterative aspect could be avoided by deriving an analytical method for each joint type, but I'm not sure if this would be worth the time spent since I think most of the time people would want to use full inverse kinematics to achieve a pose for a body instead of setting individual joint positions (with the exception of FreeJoint and WeldJoint).
2. I think our set/get functions should be WYSIWYG (what you set is what you get), but that would not be possible with any Joint type besides FreeJoint and WeldJoint. This would be likely to produce unexpected results where some portion of the requested transform is truncated due to unreachability. Because of this, I think a setTransform or similar convenience function would be semantically deceptive, because I think it would imply that the end result will have the exact transform that was asked for. I don't think documentation would be enough to override the implication that a set function carries with it.

[mkoval]

I tend to agree with @mxgrey here. It's not even clear to me what the "best" solution is, since choosing an error metric depends a lot on the user's application. This seems best handled by the InverseKinematics classes.

However, I do think it would be useful to have a helper function in DART for setting the generalized coordinates from a FreeJoint from a transform. It requires a lot of boilerplate to set the pose of a Skeleton in the environment. To avoid this, I wrote a helper function that takes a BodyNode as input and sets the pose of its parent FreeJoint to a target value. If the BodyNode does not have a FreeJoint as its parent, then I print a warning and return failure.

Maybe we could add something similar to DART?

[mxgrey]

@mkoval, If I'm interpreting your suggestion correctly, I think that's what this pull request does. The only thing missing would be a dynamic_cast of the parent Joint to a FreeJoint and then printing a warning if the dynamic_cast fails, but I feel like dynamic_casts should generally be left to the user to attempt and error handle, because the user would have the best idea of what kind of assumptions make sense for their skeleton structures. I wouldn't want an API that might trick someone into thinking they could arbitrarily set the transform of a BodyNode, only to have them get yelled at when that BodyNode doesn't actually have a FreeJoint parent Joint.

On another note, I do think it would be appropriate to have these same mechanics for WeldJoint. There's some ambiguity over whether we should use it to set the "transform from parent BodyNode" or the "transform from child BodyNode", but I think I would strongly favor the former over the latter.

[jslee02]

@mxgrey's two concerns sound reasonable to me as well. As @mkoval said, single-joint inverse kinematics problem looks solvable using InverseKinematics since it can choose which dof will be used to solve the inverse kinematics problem.

To avoid this, I wrote a helper function that takes a BodyNode as input and sets the pose of its parent FreeJoint to a target value. If the BodyNode does not have a FreeJoint as its parent, then I print a warning and return failure.

This is what I exactly did for Gazebo. But I'm also questionable to add something similar to DART under the function name of setTransform with the reason of @mxgrey . We could consider to add Frame::attemptToMatch[Transform/Velocity/Acceleration] (or with better name). This function would work as FreeJoint::setTransform when the parent is FreeJoint. If not, it attempts to find a generalize positions so that the Frame is close enough to the target transformation. Also, we may want to pass options how to solve the inverse kinematics problem.

On another note, I do think it would be appropriate to have these same mechanics for WeldJoint. There's some ambiguity over whether we should use it to set the "transform from parent BodyNode" or the "transform from child BodyNode", but I think I would strongly favor the former over the latter.

Sounds good. Before we apply this to WeldJoint, I actually would like to discuss about the possibility on use of different meaning of two transformations over the current two transformations to avoid the ambiguity. Let me create an issue for it since it seems beyond the scope of this PR.

[mkoval]

@mxgrey This pull request adds a helper function to FreeJoint. I was suggesting adding a helper function to BodyNode and/or Skeleton that does the same thing. This would indeed require putting a dynamic_cast inside DART.

...but I feel like dynamic_casts should generally be left to the user to attempt and error handle, because the user would have the best idea of what kind of assumptions make sense for their skeleton structures. I wouldn't want an API that might trick someone into thinking they could arbitrarily set the transform of a BodyNode, only to have them get yelled at when that BodyNode doesn't actually have a FreeJoint parent Joint.

I am also not thrilled with this idea, but I don't see a good alternative. Anytime you load a model from URDF or SDF (I think?) it returns a Skeleton with a FreeJoints at the root. Even with the helper functions added in this pull request, the user has to write something like this:

if(auto freeJoint = dynamic_cast<dart::dynamics::FreeJoint*>(skeleton->getRootBodyNode()->getParentJoint()))
  freeJoint->setRelativeTransform(pose, dart::dynamics::Frame::World());
else
  std::cout << "Something went wrong!" << std::endl;

This is a lot of code to set the pose of a body and it's very easy to get the error checking wrong. In fact, this implementation is already missing something: it does not check getNumRootBodyNodes(). It would be convenient if DART came with a helper function that handles this for you.

Thoughts?

On another note, I do think it would be appropriate to have these same mechanics for WeldJoint. There's some ambiguity over whether we should use it to set the "transform from parent BodyNode" or the "transform from child BodyNode", but I think I would strongly favor the former over the latter.

This method should definitely have a different name. There is a fundamental difference between updating the generalized coordinates of a joint (as setRelativeTransform does on FreeJoint) and modifying the kinematic tree itself (as setRelativeTransform would on WeldJoint).

We could consider to add Frame::attemptToMatch[Transform/Velocity/Acceleration](or with better name). This function would work as FreeJoint::setTransform when the parent is FreeJoint. If not, it attempts to find a generalize positions so that the Frame is close enough to the target transformation. Also, we may want to pass options how to solve the inverse kinematics problem.

I don't feel too strongly about this, mostly because I cannot come up with a use case where I would use attemptToMatch, but not want control over the InverseKinematics solver itself. @jslee02 did you have a particular application in mind?

[mxgrey]

You make a lot of fair points. If the primary purpose is to set root transformations, what if we offered something like this:

// Set what the root transform is when the root Joint is at the current set of Joint positions
Skeleton::setCurrentRootTransform(const Eigen::Isometry3d& tf, size_t treeIndex=0);

// Set what the root transform is when the root Joint is at mInitialPositions
Skeleton::setInitialRootTransform(const Eigen::Isometry3d& tf, size_t treeIndex=0);

This would modify Joint::mT_ParentBodyToJoint for the parent Joint of the root. One of them would do it while maintaining the current joint angles, and the other would make it so that the root transform matches tf when the parent Joint of the root is at its initial positions.

This would be completely agnostic to the type of the parent Joint. I suppose the catch is that it wouldn't be setting the positions for FreeJoint even though that's what you're asking for, @mkoval , but is there a particular reason to set the Joint positions instead of setting the transform from the parent?

[jslee02]

I don't feel too strongly about this, mostly because I cannot come up with a use case where I would use attemptToMatch, but not want control over the InverseKinematics solver itself. @jslee02 did you have a particular application in mind?

I was thinking to implement attemptToMatch that uses InverseKinematics internally. I don't have particular application in mind but the motivation was to provide convenience API that sets the parent joint positions through a BodyNode over any kinds of the parent joint types. But it seems the behavior of attemptToMatch wouldn't be clear enough.

If we want helper functions to set the root BodyNode transformation, then setCurrentRootTransform and setInitialRootTransform looks good to me as well.

[mkoval]

I've been putting off replying to this because I'm having a difficult time articulating why setting mT_ParentBodyToJoint, instead of the generalized coordinates, seems like a bad idea. This is a bit of an unorganized brain dump, so hopefully you see what I mean:

• The fact that FreeJoint and WeldJoint are different means that there is value in representing 6-DOF pose in generalized coordinates, instead of entirely in mT_ParentBodyToJoint.
• There is a fundamental difference between changing the kinematic structure (e.g. by modifying mT_ParentBodyToJoint) and changing the value of the generalized coordinates.
• Changing the kinematic structure of a Skeleton may break some algorithms; e.g. because stored generalized coordinates are no longer valid.
• Changing the kinematic structure of a Skeleton complicates sharing state between copies. If I call this helper function, then I can no longer copy state using copy->setPositions(original->getPositions()).

I am fine with these functions being added, since they would be useful when programmatically constructing a Skeleton, but I do not think they are a replacement for a function that changes the value of a FreeJoint's generalized coordinates.

[mxgrey]

There is a fundamental difference between changing the kinematic structure (e.g. by modifying mT_ParentBodyToJoint) and changing the value of the generalized coordinates.

I 100% agree with this, which is actually what led me to think we should just be using the mT_ParentBodyToJoint to be modifying the base transform. To expound on that a bit, I think the 6 DOFs of a FreeJoint are part of the Skeleton's state, and should be treated as such. They should be getting used in conjunction with all the other generalized coordinates of the Skeleton. They should not just be used to casually place an object somewhere in the world. In other words, there's a difference between saying "This is where the Skeleton should start out" and saying "This is the state of the Skeleton at this moment in time".

My interpretation of the use case here is to set up the initial transform of the Skeleton's root within the scene, not to set the initial state of the Skeleton. If the point is to set the initial state of the Skeleton, then you should be setting the generalized coordinates to the desired initial values as a cohesive whole instead of using convenience functions to set a subsection of the state.

Of course, the glaring flaw in all of my logic is that the URDF and SDF formats don't have a way of specifying initial states, so you can't actually encode that information into those model files, and that creates incentive for a convenience function that helps the user set up the initial state based on a desired root transform. So I do recognize the value of what you're asking for.

But I guess key point of the case I'm trying to make is this: If the user wants to alter the generalized coordinate positions of their Skeleton, then user should know the meaning of the Skeleton's generalized coordinates and not rely on a convenience function that has limited applicability. Instead, the convenience function should operate in a way that is entirely agnostic to the particular details of the Skeleton's generalized coordinates, so that it works without exceptions.

Changing the kinematic structure of a Skeleton complicates sharing state between copies. If I call this helper function, then I can no longer copy state using copy->setPositions(original->getPositions())

This is absolutely true, and to me it's the most compelling argument in favor of a convenience function that modifies the generalized coordinates of the FreeJoint. However, I'm still feeling that it should be the user's responsibility to cast the FreeJoint whose positions they want to change, and then use the FreeJoint::setTransform() function. They should be responsible for deciding how to handle a failed dynamic_cast, or they should decide whether they are confident enough in their assumptions to use a static_cast instead. To me it doesn't seem like DART should take responsibility for the assumptions of the user.

[mkoval]

They should not just be used to casually place an object somewhere in the world. In other words, there's a difference between saying "This is where the Skeleton should start out" and saying "This is the state of the Skeleton at this moment in time".

The initial pose of a Skeleton depends on both the (1) mT_ParentBodyToJoint of its parent FreeJoint and (2) initial generalized coordinates of that joint. There is nothing special about the "zero" value, as we discussed in #449, other than it being the default value for initializing generalized coordinates.

In fact, I'd prefer to leave mT_ParentBodyToJoint as identity. This simplifies debugging because the generalized coordinates of the FreeJoint correspond to the pose of the object in the world frame. It's more difficult to interpret the generalized coordinates if they are relative to an arbitrary start pose.

Of course, the glaring flaw in all of my logic is that the URDF and SDF formats don't have a way of specifying initial states, so you can't actually encode that information into those model files, and that creates incentive for a convenience function that helps the user set up the initial state based on a desired root transform. So I do recognize the value of what you're asking for.

URDF or SDF are the only practical ways of loading Skeletons into DART right now (.skel does not seem to be stable yet?). As a result, everyone that uses DART will end up re-implementing the pseudo-code that I wrote above. It seems preferable to provide one, properly tested, version of the function as a utility.

As a compromise, perhaps we could not add the helper method to Joint, since this would imply that it will work on any type of Joint. Perhaps we could add a static method on FreeJoint or a free function in dart::utils?

[mxgrey]

Perhaps we could add a static method on FreeJoint or a free function in dart::utils

I could be very easily convinced of this. Having it be external from the standard Joint API would be a very clean compromise. I think my preference would be to have it in dart::utils, but I wouldn't strongly oppose it being a static function in FreeJoint.

[mkoval]

I slightly prefer FreeJoint for two reasons:

1. It will be much easier to find in the auto-generated API documentation.
2. dart::utils consists mostly of modules that are selectively compiled (e.g. COMPONENTS that have third-party dependencies)

However, I am not strongly opposed to dart::utils.

[mxgrey]

I suppose having it in FreeJoint really makes the most sense, since we only intend for it to be applicable to FreeJoint. Also, there isn't an obvious place to put it in the utils namespace anyway.

👍 for FreeJoint static convenience function that checks if a Joint is a FreeJoint and then changes sets its transform. I guess I would propose this as the API:

static void FreeJoint::setTransform(Joint* joint, const Eigen::Isometry3d& tf, const Frame* withRespectTo)

On a side note, I just noticed that the FreeJoint API in the pull request is inconsistent with the SimpleFrame API:

SimpleFrame::setRelativeTransform(const Eigen::Isometry3d& tf);
SimpleFrame::setTransform(const Eigen::Isometry3d& tf, const Frame* withRespectTo);

versus

FreeJoint::setRelativeTransform(const Eigen::Isometry3d& tf);
FreeJoint::setRelativeTransform(const Eigen::Isometry3d& tf, const Frame* withRespectTo);

[mkoval]

Should withRespectTo default to the parent BodyNode or the World frame? Or should it be required as an explicit argument? I'm not sure what the convention is elsewhere in DART.

Also, from a "I just want to set the bloody pose of this Skeleton I loaded from URDF" perspective, it would be very convenient to also have these overloads:

static void FreeJoint::setTransform(Joint* joint, const Eigen::Isometry3d& tf, const Frame* withRespectTo);
static void FreeJoint::setTransform(BodyNode* joint, const Eigen::Isometry3d& tf, const Frame* withRespectTo);
static void FreeJoint::setTransform(Skeleton* joint, const Eigen::Isometry3d& tf, const Frame* withRespectTo);

I think I'm pushing my luck now. 😓

[mxgrey]

It's not possible for withRespectTo to default to the parent BodyNode, because there's no static function that can refer to the parent BodyNode. It would have to be implied by overloading the function with a version that leaves out that argument.

So far in DART the convention has been that if something has the word Relative in it, it's going to be with respect to the parent Frame. Otherwise, it will be in terms of the Frames specified by the given arguments, which always default to Frame::World(). Personally, I like this convention, but I might be biased since I'm the one who started it.

I think I'm pushing my luck now.

I think the only one that might be pushing it is the Skeleton version, since Skeletons are allowed to have multiple root BodyNodes. I guess if the semantics are to apply the transform to every root BodyNode in the Skeleton, then it's probably fine.

[mkoval]

So far in DART the convention has been that if something has the word Relative in it, it's going to be with respect to the parent Frame. Otherwise, it will be in terms of the Frames specified by the given arguments, which always default to Frame::World(). Personally, I like this convention, but I might be biased since I'm the one who started it.

👍 Defaulting to World makes sense to me.

I think the only one that might be pushing it is the Skeleton version, since Skeletons are allowed to have multiple root BodyNodes

👍 I think that's reasonable. The Skeleton::getRootBodyNode() function already defaults to a zero index, presumably for convenience when there is only one root, so it would be useful to have the same type of convenience here.

[mxgrey]

I'm looking through the changes in this pull request, and I'm wondering why the Eigen structures for FreeJoint::setRelativeSpatialMotion are being passed as pointers instead of by reference. I think it would make a lot more sense to pass Eigen structures by reference, because (at least to me) passing something by pointer typically implies that it may be a nullptr.

Never mind, I read the description of the function, and now I get why they are passed as pointers. 👍

---

I also think that there's some API inconsistency that we should resolve. Specifically, consider the SimpleFrame class:

• SimpleFrame::setRelativeTransform(const Eigen::Isometry3d&) sets the relative transform (in this case, "relative" implies relative to the parent frame) directly to the Isometry3d that gets passed in.
• SimpleFrame::setTransform(const Eigen::Isometry3d& tf, const Frame* withRespectTo = Frame::World()) sets the relative transform such that its overall transform will be tf with respect to withRespectTo.

I would propose we use a similar scheme for these other setter functions. A function named setRelativeXYZ(const Eigen::Structure& xyz) will directly set the relative XYZ value to the Eigen structure that gets passed into it whereas setXYZ(const Eigen::Structure& xzy, const Frame* withRespectTo) will set the relative XYZ such that the overall XYZ will be xyz with respect to withRespectTo.

I think that we should either go with my proposal, or we should change the SimpleFrame API to match the new convention that's being used in this pull request (and deprecate the current API for it).

[jslee02]

In the recent commit, the APIs are updated to be consistent with our codebase, and FreeJoint::setTransform(~) functions are added as suggested.

[mxgrey]

Should we print a warning here?

[jslee02]

👍 Added warning.

[mxgrey]

Looks good 👍