Function to convert trajectory_msgs::JointTrajectory into aikido::trajectory::Spline #147

gilwoolee · 2017-04-02T23:13:46Z

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.

ClintLiddick · 2017-04-03T12:21:54Z

I took a look and I'm not exactly sure. I think this is a latent bug or, like you said, implicitly assuming a single-DOF joint (for JointTrajectory not MultiDOFJointTrajectory). I think if we gave this a true multi-DOF joint it would throw an incorrect error. Correct thing is probably to change the strings + docs + error message, and add a comment that this only works for single-DOF joints (i.e. no SO3/SE3 I think). I'm not quite sure what the right logic would be for handling multi-DOF joints, or if it could live in the same conversion functions. @mkoval thoughts?

mkoval

This generally looks good. It's not surprising that I think so, since I originally wrote it! 😁 @gilwoolee, @jslee02, or @psigen could you review this as well to catch any errors I made?

@gilwoolee and @ClintLiddick are correct that this code only handles single-DOF joints. There is not a standard way of representing a multi-DOF joint in a trajectory_msgs/JointTrajectory message. We jump through a lot of gymnastics in Aikido (notably: representing the trajectory in the tangent space) to handle this.

I suggest:

Renaming numControlledDofs to numControlledJoints to make this clear.
Adding a check that raises an exception if there is a multi-DOF joint in the _stateSpace.
Revisiting this decision later if necessary.

Thanks @gilwoolee for splitting this into a separate pull request!

mkoval · 2017-04-04T00:37:50Z

src/control/ros/Conversions.cpp

+    throw std::invalid_argument{"StateSpace must be non-null."};
+
+  const auto numControlledDofs = _space->getNumSubspaces();
+  if (_jointTrajectory.joint_names.size() != numControlledDofs)


Rename numControlledDofs to numControlledJoints.

mkoval · 2017-04-04T00:38:15Z

src/control/ros/Conversions.cpp

+  if (_jointTrajectory.points.size() < 2)
+  {
+    throw std::invalid_argument{
+      "Trajectory must contain at least two or more waypoints."};


Nit: "Trajectory must contain two or more waypoints."

mkoval · 2017-04-04T00:39:58Z

src/control/ros/Conversions.cpp

+          << _jointTrajectory.joint_names[i] << ".";
+      throw std::invalid_argument{message.str()};
+    }
+  }


This check will fail if _jointTrajectory and _space have the same joints, but in a different order. We should build a map from joint name to subspace and use it here.

Also, add a check that throws an exception if any of the joints are not single DOF.

@mkoval I am a bit confused about your first comment. If we can't assume that they are given in the same order, how do we build the initial mapping? Should we compare each _jointTrajectory.joint_names[i]with all joint names in _space->getJointSpace(j)->getJoint()->getName()?

One more question: is it joint_name or dof_name that we get from _jointTrajectory.joint_names[] when we get it from actual HERB? (Assuming single dofs. Are they the same?)

If we can't assume that they are given in the same order, how do we build the initial mapping? Should we compare each _jointTrajectory.joint_names[i] with all joint names in _space->getJointSpace(j)->getJoint()->getName()?

That was my idea. None of these solutions are perfect, but I trust the names to be correct more than I trust the indices to be.

One more question: is it joint_name or dof_name that we get from _jointTrajectory.joint_names[] when we get it from actual HERB?

There is no correct answer here, since the JointTrajectory message has no distinction between joints and DOFs. I would strongly prefer Joint, since that: (1) it seems that DegreeOfFreedom names are typically (but not always) constructed from their Joint's name, (2) URDF only provides a mechanism for specifying joint names, and (3) it semantically matches the joint_name field in the message.

(Assuming single dofs. Are they the same?)

They are not guaranteed to be the same in DART, but I believe they are by default. @jslee02 Is that correct?

mkoval · 2017-04-04T00:40:34Z

src/control/ros/Conversions.cpp

+  else if (isVelocityRequired)
+    numCoefficients = 4; // cubic
+  else
+    numCoefficients = 2; // linear;


Nit: Remove extra ;.

mkoval · 2017-04-04T00:48:04Z

src/control/ros/CMakeLists.txt

+# Dependencies
+#
+find_package(trajectory_msgs REQUIRED)
+aikido_check_package(trajectory_msgs "aikido::control::ros" "trajectory_msgs")


The control/ros namespace is intended to be optional, so traectory_msgs should not be marked as REQUIRED. We should change this to:

find_package(trajectory_msgs QUIET) aikido_check_package(geometry_msgs "control::ros" "trajectory_msgs")

mkoval · 2017-04-04T00:48:58Z

tests/control/ros/CMakeLists.txt

@@ -0,0 +1,6 @@
+


Add a check here to only build these tests if the ros::control component was built.

Shushman · 2017-04-04T15:11:49Z

FYI, the test_Conversions is failing here with the following error:

[ RUN      ] ConvertJointTrajectoryTests.LinearTrajectory
unknown file: Failure
C++ exception with description "Unknown joint name: expected Joint1, got DegreeOfFreedom1." thrown in the test body.
[  FAILED  ] ConvertJointTrajectoryTests.LinearTrajectory (0 ms)

and similarly for CubicTrajectory and QuinticTrajectory

gilwoolee · 2017-04-04T22:04:32Z

@Shushman Yeah that's because it tries to match joint names with dof names. Fixing it now. :D

mkoval · 2017-04-05T02:25:07Z

Yeah that's because it tries to match joint names with dof names.

I don't know what that means, but I'm glad @gilwoolee figured it out! 😈

coveralls · 2017-04-05T16:11:17Z

Coverage remained the same at 85.089% when pulling 22ba7b4 on convertJointTrajectory into 6779dd2 on master.

jslee02 · 2017-04-05T20:03:22Z

tests/control/ros/CMakeLists.txt

@@ -0,0 +1,6 @@
+if(${FOUND_${PROJECT_NAME}_control_ros})


aikido_control_ros is a local target so FOUND_aikido_control_ros wouldn't be defined. Please use if(TARGET "${PROJECT_NAME}_control_ros") instead.

if(TARGET target-name)
True if the given name is an existing logical target name such as those created by the add_executable(), add_library(), or add_custom_target() commands.

jslee02 · 2017-04-05T20:12:13Z

src/control/ros/Conversions.cpp

+    if (n != 1)
+    {
+      std::stringstream message;
+      message << "[Conversion] Expected 1 dof. Joint "


Nit: Only this warning message has the heading [Conversion]. It would good to be consistent by either removing the heading from here or adding the heading to all other messages.

👍 For removing the heading here.

coveralls · 2017-04-07T00:50:21Z

Changes Unknown when pulling 6714011 on convertJointTrajectory into ** on master**.

coveralls · 2017-04-07T00:51:54Z

Changes Unknown when pulling 6714011 on convertJointTrajectory into ** on master**.

gilwoolee · 2017-04-07T01:18:20Z

@jslee02 Thanks for a quick review! @mkoval Would you mind giving it another pass? :D Thanks a lot!!

coveralls · 2017-04-07T01:37:50Z

Changes Unknown when pulling 785871b on convertJointTrajectory into ** on master**.

coveralls · 2017-04-07T14:24:45Z

Coverage increased (+0.007%) to 85.215% when pulling c98e8ea on convertJointTrajectory into a9b50ae on master.

mkoval · 2017-04-08T19:08:04Z

include/aikido/control/ros/Conversions.hpp

+/// This method only handles single-dof joints.
+/// \param[in] _space MetaSkeletonStateSpace for Spline trajectory.
+/// \param[in] _jointTrajectory ROS JointTrajectory to be converted.
+/// \return Spline trajectory.


Nits:

"aikido's Spline Trajectory" -> "aikido's Spline trajectory"

"single-dof" -> "single-DOF"

Add a note about only handling Rn and SO2 joints.

mkoval · 2017-04-08T20:40:39Z

src/control/ros/Conversions.cpp

+      std::stringstream message;
+      message << "Expected 1 dof. Joint " << i << " has " << n << " dofs.";
+      throw std::invalid_argument{message.str()};
+    }


Check that getJointSpace(i)is Rn or SO2.

mkoval · 2017-04-08T20:41:16Z

src/control/ros/Conversions.cpp

+    if (n != 1)
+    {
+      std::stringstream message;
+      message << "Expected 1 dof. Joint " << i << " has " << n << " dofs.";


Nit: It would be helpful to include joint->getName() and joint->getType() to message, e.g.:

Only single-DOF joints are supported. Joint "my_joint1" (index: 1) is a SphericalJoint with 3 DOFs.

mkoval · 2017-04-08T20:50:43Z

src/control/ros/Conversions.cpp

+        rosJointToMSSSJoint.emplace(std::make_pair(i, j));
+        found_match = true;
+        break;
+      }


Encapsulate this in a helper function. Putting this search inline means that we can't easily improve it's efficiency later (e.g. using unordered_map) and makes it harder to understand this function.

I suggest writing a helper function to get a Joint by name from a MetaSkeleton. If the name is ambiguous, which can occur in a MetaSkeleton that contain Joints from multiple Skeletons, throw an exception. Then, this code simply becomes:

auto metaSkeleton = space->getMetaSkeleton(); auto joint = getJointByName(*metaSkeleton, jointName); auto index = metaSkeleton->getIndex(joint);

mkoval · 2017-04-08T20:56:19Z

src/control/ros/Conversions.cpp

+    extractJointTrajectoryPoint(jointTrajectory, iwaypoint, numControlledJoints,
+      &nextPosition, isPositionRequired,
+      &nextVelocity, isVelocityRequired,
+      &nextAcceleration, isAccelerationRequired);


I'd like to perform the reorder operation here. These two operations are closely related because they convert from a ROS data structure to the Aikido data structure that we expect. The rest of this loop purely performs operations on Aikido data structures, e.g. fitting splines.

mkoval · 2017-04-08T20:58:02Z

src/control/ros/Conversions.cpp

+  }
+}
+
+} // namespace


I suspect that using a permutation matrix here will be a lot slower than operating directly on the list of std::pair<size_t, size_t>s. I also think it is is more complicated to implement this way, since it involves an intermediate data structure (the permutation matrix) that would not be otherwise necessary.

Since this is an entirely an implementation detail, I am fine keeping it as it is for now. However, we may want to consider refactoring this in the future if performance becomes an issue.

mkoval · 2017-04-08T21:00:12Z

tests/control/ros/test_Conversions.cpp

+  EXPECT_EIGEN_EQUAL(make_vector(6.), values, kTolerance);
+}
+
+TEST_F(ConvertJointTrajectoryTests, LinearTrajectoryWithDifferentOrdering)


Nit: The fact that you are using a linear trajectory in this test is an implementation detail. I suggest renaming the test to just DifferentJointOrdering.

mkoval · 2017-04-08T21:01:01Z

tests/control/ros/test_Conversions.cpp

+
+  trajectory->evaluateDerivative(0.5, 2, values);
+  EXPECT_EIGEN_EQUAL(make_vector(0., 0.), values, kTolerance);
+}


Add a test for a MetaSkeleton that has multiple joints with the same name. This is possible by creating a Group that contains the Joint with the same name in two different Skeletons.

Add a test for a MetaSkeleton that is missing one of the joints named in the JointTrajectoryMessage.

Add a test for a JointTrajectory that contains duplicate joints.

coveralls · 2017-04-09T04:03:35Z

Coverage increased (+0.05%) to 85.259% when pulling d4bdaa3 on convertJointTrajectory into a9b50ae on master.

mkoval

Last round of (hopefully) minor comments! 😓

mkoval · 2017-04-09T15:32:59Z

src/control/ros/Conversions.cpp

+  {
+    if (metaSkeleton->getJoint(i)->getName() == jointName)
+      return metaSkeleton->getJoint(i);
+  }


Check if the MetaSkeleton has more than one joint with this name.

As a note, I'm thinking of implementing this to dart::dynamics::MetaSkeleton. That function would return a list of joint pointers whoes names are all jointName because MetaSkeleton possibly has multiple joints with the same name.

@jslee02 That would be a great addition. 👍

In fact, it would be nice to have two functions: (1) one that returns a list of joints and (2) one that returns a single joint and prints a warning [or returns nullptr?] if multiple joints have the same name.

In any case, I don't want to wait on a new version of DART before merging this pull request. So we should also implement it here. 😉

dartsim/dart#879

Thanks @jslee02!

mkoval · 2017-04-09T15:34:20Z

src/control/ros/Conversions.cpp

+
+//=============================================================================
+dart::dynamics::Joint* findJointByName(
+    dart::dynamics::MetaSkeleton* metaSkeleton, const std::string& jointName)


Nit: metaSkeleton is not an out parameter, so we should take it by reference. In this case, I think we can actually take it const &.

Hm, I know polymorphisms work both for pointers and references, but I thought our convention is to take a const pointer.

The convention I've seen (and generally follow) is to prefer references and only use pointers for: (1) output parameters or (2) optional parameters that may be nullptr.

@psigen Want to break the tie? 😬

I haven't decided my preference here yet. If we want to use references for this case, then I see there are tons of changes in Aikido code base we need to make for the consistency. Just saying. 😅

Are you sure? Note that State variables are an exception to this rule because I did some sketchy business to reduce their memory footprint and am not sure whether or not calling *state to create a reference invokes undefined behavior.

mkoval · 2017-04-09T15:35:43Z

src/control/ros/Conversions.cpp

+    throw std::invalid_argument{message.str()};
+  }
+
+  // Check that all joints are single DOF.


Nit: "...RnJoint or SO2Joint state spaces."

mkoval · 2017-04-09T15:36:34Z

src/control/ros/Conversions.cpp

+
+  for (size_t i = 0; i < jointTrajectory.joint_names.size(); ++i)
+  {
+    std::string jointName = jointTrajectory.joint_names[i];


Nit: const auto&.

mkoval · 2017-04-09T15:37:26Z

src/control/ros/Conversions.cpp

+    auto index = metaSkeleton->getIndexOf(joint);
+    assert(index != dart::dynamics::INVALID_INDEX);
+
+    rosJointToMetaSkeletonJoint.emplace(std::make_pair(i, index));


Verify that this is not a duplicate, i.e. that rosJointToMetaSkeletonJoint[i] does not already exist. You can do this by checking the bool flag in the std::pair returned by emplace.

Nit: Also, it would be nice to more descriptive variable names than i and index. 😉 Maybe metaSkeletonIndex and trajectoryIndex?

coveralls · 2017-04-11T01:55:17Z

Coverage increased (+0.05%) to 85.259% when pulling 3a10630 on convertJointTrajectory into a59975f on master.

…obotics/aikido into convertJointTrajectory

coveralls · 2017-04-11T21:36:36Z

Coverage increased (+0.01%) to 85.223% when pulling 8f1a94b on convertJointTrajectory into a59975f on master.

mkoval

This looks great. Thanks @gilwoolee for putting up with the deluge of our comments. 😅

I left a few minor style comments. Feel free to address or ignore.

mkoval · 2017-04-12T00:50:53Z

src/control/ros/Conversions.cpp

+  {
+    if (metaSkeleton.getJoint(i)->getName() == jointName)
+    {
+      joints.emplace_back(metaSkeleton.getJoint(i));


Nit: I'd prefer to store metaSkeleton->getJoint(i) in a variable so we don't have to call it twice.

mkoval · 2017-04-12T00:54:41Z

src/control/ros/Conversions.cpp

+        << joint_names[i] << "].";
+      throw std::invalid_argument{message.str()};
+    }
+  }


Nit: it would be cleaner to replace this loop with:

auto duplicate_it = std:: adjacent_find(std::begin(joint_names), std::end(joint_names)); if (duplicate_it != std::end(joint_names)) { /* ... */ }

mkoval · 2017-04-12T00:56:25Z

src/control/ros/Conversions.cpp

+  for (size_t i = 0; i < numControlledJoints; ++i)
+  {
+    joint_names.emplace_back(jointTrajectory.joint_names[i]);
+  }


Nit: Why is this loop necessary? I believe this is equivalent to:

auto joint_names = jointTrajectory.joint_names;

mkoval · 2017-04-12T00:57:47Z

src/control/ros/Conversions.cpp

+  auto metaSkeleton = space->getMetaSkeleton();
+  auto nJoints = jointTrajectory.joint_names.size();
+
+  for (size_t trajIndex = 0; trajIndex < nJoints; ++trajIndex)


Nit: The name trajIndex is ambiguous, since it could refer to a waypoint index or a joint index.

mkoval · 2017-04-12T00:58:01Z

src/control/ros/Conversions.cpp

+    const auto& jointName = jointTrajectory.joint_names[trajIndex];
+    auto joints = findJointByName(*metaSkeleton, jointName);
+
+    if (joints.size() == 0)


Nit: joints.empty().

mkoval · 2017-04-12T00:58:59Z

src/control/ros/Conversions.cpp

+    {
+      std::stringstream message;
+      message << "Joint " << jointName << " (index: " << trajIndex << ")"
+        << " does not exist in metaSkeleton.";


Nit: This index is ambiguous because it could refer to an index in the JointTrajectory or an index in the MetaSkeleton. I suggest explicitly saying "trajectory index."

mkoval · 2017-04-12T00:59:47Z

src/control/ros/Conversions.cpp

+    {
+      std::stringstream message;
+      message << "Multiple (" << joints.size()
+        << ") joints have the same name [" << jointName << "].";


Nit: This message is ambiguous because it could refer to duplicate joints in the JointTrajectory or the MetaSkeleton. I suggest explicitly saying "in the JointTrajectory."

coveralls · 2017-04-12T02:18:52Z

Coverage decreased (-0.02%) to 85.189% when pulling 4fb24a5 on convertJointTrajectory into a59975f on master.

gilwoolee · 2017-04-12T03:05:55Z

Closing. Thanks a lot @mkoval @jslee02 @ClintLiddick !

gilwoolee added 5 commits April 2, 2017 16:35

ros/Conversions

43ea8bf

Cleaning up CMakeLists.

e3f783b

CMakeLists cleanup

4391bf9

Style change to match AIKIDO style guide.

338f1b7

Checking joint-name matches

aafa1cf

gilwoolee requested a review from mkoval April 2, 2017 23:14

mkoval mentioned this pull request Apr 2, 2017

Aikido ROS control #111

Closed

gilwoolee requested review from jslee02 and aditya-vk April 3, 2017 19:20

gilwoolee self-assigned this Apr 4, 2017

jslee02 added this to the Aikido 0.1.0 milestone Apr 4, 2017

mkoval requested changes Apr 4, 2017

View reviewed changes

mkoval reviewed Apr 4, 2017

View reviewed changes

tests/control/ros/CMakeLists.txt

@@ -0,0 +1,6 @@

Copy link

Member

mkoval Apr 4, 2017

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Add a check here to only build these tests if the ros::control component was built.

mkoval changed the title ~~Convert ROS/trajectory_msgs/JointTrajectory into aikido/trajectory/Spline~~ Function to convert trajectory_msgs::JointTrajectory into aikido::trajectory::Spline Apr 4, 2017

mkoval mentioned this pull request Apr 4, 2017

ros_control/RosTrajectoryExecutor #149

Merged

Merge branch 'master' into convertJointTrajectory

a2e0ed8

Addressing some of Mike's comments

22ba7b4

jslee02 reviewed Apr 5, 2017

View reviewed changes

gilwoolee added 2 commits April 6, 2017 20:29

Addressing Miks and JS's comments.

9e09781

Merge branch 'master' into convertJointTrajectory

6714011

jslee02 approved these changes Apr 7, 2017

View reviewed changes

Minor bug fix, adding one test for joint mapping.

785871b

Merge branch 'master' into convertJointTrajectory

c98e8ea

mkoval requested changes Apr 8, 2017

View reviewed changes

Addressing Mike's comments.

d4bdaa3

mkoval requested changes Apr 9, 2017

View reviewed changes

Merge branch 'master' into convertJointTrajectory

3a10630

gilwoolee added 2 commits April 11, 2017 17:14

Addressing Mike's comments

064aa8a

Merge branch 'convertJointTrajectory' of https://github.com/personalr…

8f1a94b

…obotics/aikido into convertJointTrajectory

mkoval approved these changes Apr 12, 2017

View reviewed changes

Addressing some style comments

4fb24a5

gilwoolee merged commit 3c9c8cc into master Apr 12, 2017

jslee02 deleted the convertJointTrajectory branch April 15, 2017 00:33

mkoval mentioned this pull request Apr 15, 2017

Add wrapper for OMPL path simplifier #164

Merged

Function to convert trajectory_msgs::JointTrajectory into aikido::trajectory::Spline #147

Function to convert trajectory_msgs::JointTrajectory into aikido::trajectory::Spline #147

Conversation

gilwoolee commented Apr 2, 2017

ClintLiddick commented Apr 3, 2017

mkoval left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

gilwoolee Apr 5, 2017 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Shushman commented Apr 4, 2017

gilwoolee commented Apr 4, 2017

mkoval commented Apr 5, 2017

coveralls commented Apr 5, 2017

jslee02 Apr 5, 2017 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

coveralls commented Apr 7, 2017

coveralls commented Apr 7, 2017

gilwoolee commented Apr 7, 2017

coveralls commented Apr 7, 2017

coveralls commented Apr 7, 2017

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

mkoval Apr 8, 2017 • edited by jslee02 Loading

Choose a reason for hiding this comment

coveralls commented Apr 9, 2017

mkoval left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

mkoval Apr 11, 2017 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

coveralls commented Apr 11, 2017

coveralls commented Apr 11, 2017

mkoval left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

coveralls commented Apr 12, 2017

gilwoolee commented Apr 12, 2017

gilwoolee Apr 5, 2017 •

edited

Loading

jslee02 Apr 5, 2017 •

edited

Loading

mkoval Apr 8, 2017 •

edited by jslee02

Loading

mkoval Apr 11, 2017 •

edited

Loading