F#182 partial trajectory mod #185

beatrizleon · 2015-08-03T16:18:46Z

Enables the control of partial trajectories. In order to do this, a trajectory now is composed of a vector of vectors of segments (one vector of segments per each joint), so each joint can a vector with different number of segments.

The changes of code are the minimum I could modify to make it work. If the changes are accepted, we can improve it. Specially, the definition of a segment, as it is only per joint, it does not need a permutation vector, and all its related methods.

All test are passing, but I had to modify some of them to the new implementation. If it is accepted, we can think of new test needed specifically to test partial trajectories.

…n only hold one goal at a time

adolfo-rt · 2015-08-04T12:33:29Z

I did a first pass reading the PR. I'll do a second, more detailed one soon and come back with any comments that might arise. I'd also like to do some experimental testing.

Thanks for putting this together.

beatrizleon · 2015-08-07T14:15:49Z

Thanks @adolfo-rt for reviewing it. I found an error in my branch when a new trajectory was send with an empty vector of accelerations. It is fixed now.

adolfo-rt · 2015-08-13T15:57:29Z

joint_trajectory_controller/include/joint_trajectory_controller/init_joint_trajectory.h

+template <class Trajectory>
+bool IsNotEmpty(typename Trajectory::value_type trajPerJoint)
+{
+  return trajPerJoint.size()>0;


return !trajPerJoint.empty();

adolfo-rt · 2015-08-13T16:03:12Z

@beatrizleon Update: I'm in the middle of a detailed code review. I'm about halfway done, and I'd like to give some feedback tomorrow.

adolfo-rt · 2015-08-13T16:10:31Z

joint_trajectory_controller/include/joint_trajectory_controller/init_joint_trajectory.h

@@ -114,6 +113,12 @@ struct InitJointTrajectoryOptions
  ros::Time*                 other_time_base;
 };

+template <class Trajectory>
+bool IsNotEmpty(typename Trajectory::value_type trajPerJoint)


isNotEmpty

adolfo-rt · 2015-08-14T16:31:38Z

I ran the tests and tested it in simulation. Seems to work as expected (yay).

Make it opt-in
One important thought: Partial joint specification should be opt-in and disabled by default. Rationale:

When a new command specifying a subset of the joints is processed, the unspecified joints keep on executing their previous goal as if nothing happened. I see potential for robots doing unexpected things when different commands that don't know about each other interact. There's always been the risk of controller clients sending bogus commands and crashing the robot or some other unexpected behavior, but the interaction of conflicting commands within a controller was never an issue. I would consider this an advanced behavior that has to be enabled explicitly, as in I know what I'm doing.

Review thoroughness
Review-wise, I have to confess that the changes are so extensive, that even after having spent a few hours reviewing them, I don't get the feeling of having been thorough enough. It's good that tests are passing, although I don't feel very confident about the completeness of the tolerance checking tests (my fault). When I wrote the tests, that was the only part that was not exercised to the fullest.

To make the test suite more complete and exercise the new feature, I'm thinking of: while the controller executes a trajectory involving all joints (not holding position, but actually moving), send a new command (topic interface) or goal (action interface) that:

Send trajectories with all waypoints in the past: Nothing gets executed.
Send trajectories with some waypoints on the past, and some in the future: Some get executed.
Send trajectories with all waypoints in the future. All get executed, in the future.

I would also like the implementation to be validated on a real-time system.

More

The structure and style of the patch can be improved for better code reading and brevity ,but I'll skip those comments for now.

beatrizleon · 2015-08-17T09:25:28Z

@adolfo-rt Thanks for the review. I am going through your comments now.

…ent time

beatrizleon · 2015-08-18T15:13:43Z

How are you thinking we could implement the opt-in option?

adolfo-rt · 2015-08-18T16:16:05Z

How are you thinking we could implement the opt-in option?

With a ROS parameter, say allow_partial_joint_specification, or something shorter, but equally descriptive. One would have to explicitly set it to True to enable partial joint specification. To enforce the behavior, you would have to do something similar to what was done up to now:

When using the action inteface. Keep the check that rejected goals early on, but disable it if allow_partial_joint_specification == True
For topic commands, the check that kicked-in was this one. Same story, keep the check, but disable it if allow_partial_joint_specification == True.

Note that these checks relied on the permutation function, which actually checked for permutations (not mappings). As an alternative, you can call your new mapping function and additionally check that the two input vectors have the same size.

beatrizleon · 2015-08-19T15:36:07Z

Added the ros param. I named it: "allow_partial_joints_goal" but it is opened to suggestions :)

beatrizleon · 2015-08-20T16:17:40Z

@adolfo-rt I added a set of tests (very similar to the existing ones) to test partial trajectories which included the ones you mentioned in your previous comment.

We do not have a real time system, then I would not be able to test that.

Appart from that I think I have addressed all your comments. Let me know what you think.

ugocupcic · 2015-09-09T08:34:28Z

@adolfo-rt any news on this? We're kind of waiting for that functionality at Shadow :)

Cheers,

Ugo

sachinchitta · 2015-09-09T18:55:29Z

@ugocupcic - @adolfo-rt did mention on the mailing list that he would be offline for 3 weeks starting Sept. 8.

ugocupcic · 2015-09-10T04:58:19Z

Ah! OK thanks @sachinchitta :)

toliver · 2016-01-20T18:23:32Z

@adolfo-rt any new on this? If there is anything else that we could improve to get it merged please let us know.

v4hn · 2016-05-10T09:37:39Z

It has been nearly three month since @adolfo-rt was last active on github.
Is there anyone else who could look at that?
It was mentioned that this should be in the kinetic release in ros-controls/ros_control#234 . Is there any reason why this should not be merged into indigo as well?
It would help me quite a lot if it were.

bmagyar · 2016-05-10T10:58:33Z

I made the releases for Kinetic and if all goes fine (since it's a well discussed issue) I will merge it to kinetic-devel soon. I'm not sure though if these changes should go to Indigo&Jade as well, so my approach so far was to merge all new stuff to kinetic-devel, and wait for some of our sleeping agents to appear so we can discuss propagating them back to indigo&jade.

toliver · 2016-05-23T15:32:13Z

We are going to try to rebase this branch and open a new PR against kinetic-devel. Hopefully this week or the next.

@bmagyar please let us know if we should also keep this PR open just in case it's getting merged into indigo later on.

bmagyar · 2016-05-23T16:10:51Z

Great Toni!

For the rebase may I ask you to also either squash the commits into one huge commit or into bigger chunks so that the "work in progress" type of commits are not reflected in the proposed changes? It's better if we track feature additions rather than the entire development processes.

Once the new PR is open, this can be closed. I am personally ok to push through all the new stuff for ROS Kinetic but if you'd like back-support for Indigo we will have to gather some more people and get a lot more feedback. If you wish to have the Kinetic features in older distros, feel free to open an issue and let's discuss them there.

bmagyar · 2016-11-16T16:15:51Z

Closing since we merged it to Kinetic.

Beatriz Leon added 10 commits July 23, 2015 16:14

moving but only with one goal

f4371b4

working with one goal, still needs to set the goal handlers correctly

38f6a1f

Trial with vector of goals but it turns out that the action server ca…

57c7bcd

…n only hold one goal at a time

Partial trajectory working. Needs cleaning and check tolerances

dc156d5

Working and cleaned

c54846f

Working and cleaned

c53f577

Changes made after modifing tests

6b8aa38

working on tests

37d0326

Fixed test and all passed

093104b

Cleaning

324f851

Fixed the import of new trajectories with accelerations empty and a test

36d6bc5

Small fix to error message

2d939d7

adolfo-rt reviewed Aug 13, 2015
View reviewed changes

Beatriz Leon added 3 commits August 17, 2015 14:26

Fixed comments

af5c237

Removed permutation from vector class and fixed tests

eebbc96

Fixed comment about assigning goal handle only to segments after curr…

967d316

…ent time

Beatriz Leon added 2 commits August 19, 2015 13:23

Fixed setting of success to goal handle when all joints succeeded

a442849

Added rosparam to allow partial joints goals

b82cf4e

added tests for partial joint goals

e19db70

toliver mentioned this pull request May 6, 2016

Kinetic release ros-controls/ros_control#234

Closed

guihomework mentioned this pull request May 10, 2016

Trajectories keep violating constraints shadow-robot/sr_interface#133

Closed

beatrizleon mentioned this pull request Jun 8, 2016

jtc: Enable sending trajectories with a partial set of joints #226

Merged

bmagyar closed this Nov 16, 2016

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

F#182 partial trajectory mod #185

F#182 partial trajectory mod #185

beatrizleon commented Aug 3, 2015

adolfo-rt commented Aug 4, 2015

beatrizleon commented Aug 7, 2015

adolfo-rt Aug 13, 2015

adolfo-rt commented Aug 13, 2015

adolfo-rt Aug 13, 2015

adolfo-rt commented Aug 14, 2015

beatrizleon commented Aug 17, 2015

beatrizleon commented Aug 18, 2015

adolfo-rt commented Aug 18, 2015

beatrizleon commented Aug 19, 2015

beatrizleon commented Aug 20, 2015

ugocupcic commented Sep 9, 2015

sachinchitta commented Sep 9, 2015

ugocupcic commented Sep 10, 2015

toliver commented Jan 20, 2016

v4hn commented May 10, 2016

bmagyar commented May 10, 2016

toliver commented May 23, 2016

bmagyar commented May 23, 2016

bmagyar commented Nov 16, 2016

F#182 partial trajectory mod #185

F#182 partial trajectory mod #185

Conversation

beatrizleon commented Aug 3, 2015

adolfo-rt commented Aug 4, 2015

beatrizleon commented Aug 7, 2015

adolfo-rt Aug 13, 2015

Choose a reason for hiding this comment

adolfo-rt commented Aug 13, 2015

adolfo-rt Aug 13, 2015

Choose a reason for hiding this comment

adolfo-rt commented Aug 14, 2015

beatrizleon commented Aug 17, 2015

beatrizleon commented Aug 18, 2015

adolfo-rt commented Aug 18, 2015

beatrizleon commented Aug 19, 2015

beatrizleon commented Aug 20, 2015

ugocupcic commented Sep 9, 2015

sachinchitta commented Sep 9, 2015

ugocupcic commented Sep 10, 2015

toliver commented Jan 20, 2016

v4hn commented May 10, 2016

bmagyar commented May 10, 2016

toliver commented May 23, 2016

bmagyar commented May 23, 2016

bmagyar commented Nov 16, 2016