New isValidVelocityMove() for checking time between two waypoints given velocity #684

davetcoleman · 2017-11-13T00:19:49Z

This is a more computationally efficient version of the previously used isValidVelocityMove() functions added in #294

This allows faster Descartes-type functionality

gavanderhoorn · 2017-11-13T08:08:07Z

moveit_core/robot_state/include/moveit/robot_state/robot_state.h

+
+  bool isValidVelocityMove(const JointModelGroup* group, const double* from_joint_pose,
+                           const double* to_joint_pose, std::size_t array_size, double dt) const;
+


why the overload with raw pointers? Convenience?

speed! this is used in a more MoveIt!-integrated version of Descartes, where it is called hundreds of thousands of times in an iterative IK brute force-type approach. It uses Boost-graph to save each raw array of joints

Passing a reference is slower than passing a pointer? Afaik they are basically the same, and the difference is purely syntactical.

Oh I thought you were asking why these two functions instead of the RobotState version. Its overloaded just as a convenience I suppose, I don't recall why I have both, I'm just trying to get my research code to work with mainstream moveit again and not my fork. I think this style is not unfounded within RobotState however - e.g. setJointPosition() has it overloaded.

robot state uses both vectors and raw pointers for lots of its functions, so i think this should remain.

gavanderhoorn · 2017-11-13T08:08:59Z

moveit_core/robot_state/src/robot_state.cpp

+
+    double max_dtheta = dt * max_velocity;
+    if (dtheta > max_dtheta)
+      return false;


It's going to be less efficient, but would it be an idea to add a ROS_DEBUG.. here with the joint that makes the check fail?

ROS_DEBUG isn't used inside RobotState, but we could use logDebug. However I agree that its less efficient so would prefer not.

Jmeyer1292 · 2017-11-25T01:32:37Z

Side note, @davetcoleman: Is this moveit-integrated version of Descartes something you plan on making open source or something you're going to keep internal to your company?

Jmeyer1292 · 2017-11-25T02:41:08Z

In either case, is there a reason you've decided to clone the Descartes work rather than work with us to make it better or fit your needs?

davetcoleman · 2017-11-26T23:22:43Z

@Jmeyer1292 my Descartes-MoveIt work is https://github.com/davetcoleman/bolt from my thesis last year, which was funded by NIST and a collaboration with you guys at SwRI. This patch was created then and I'm trying to get back to the mainstream MoveIt!. Let's have a phone call to discuss collaborations on this effort.

mlautman · 2017-12-22T23:56:00Z

+1

davetcoleman · 2018-03-29T14:21:49Z

ping @v4hn can I get a review? I already have one +1

rhaschke

I think the old function RobotState::isValidVelocityMove(const RobotState& other, const JointModelGroup* group, double dt) const should call into these new functions. Currently the old function, doesn't protect against non-standard joints.

rhaschke · 2018-04-03T09:23:10Z

moveit_core/robot_state/src/robot_state.cpp

+
+    if (var_bounds->size() != 1)
+    {
+      logError("Attempting to check velocity bounds for waypoint move with joints that have multiple variables");


I'm not sure, how to handle non-standard joints here. Why do you want to bail out? You could also skip those joints.
Now, it's not possible to use this function for JMG's containing non-standard joints.

This is a future todo. I'm not inclined to support it since its an uncommon use case, but when the need arises i welcome the change

I added a TODO as such

davetcoleman · 2018-10-25T21:01:36Z

I think the old function RobotState::isValidVelocityMove(const RobotState& other, const JointModelGroup* group, double dt) const should call into these new functions. Currently the old function, doesn't protect against non-standard joints.

I disagree - the RobotState version supports arbitrary groups of joints that are not necessary inline, whereas these new ones are optimized for the most common use case of a simple kinematic chain where the vector is in order. This can't be easily configured to call into each other without a big performance hit.

…en velocity

davetcoleman · 2018-10-25T21:02:24Z

Addressed feedback, retargeted for melodic

henningkayser · 2019-03-04T11:43:22Z

I think the old function RobotState::isValidVelocityMove(const RobotState& other, const JointModelGroup* group, double dt) const should call into these new functions. Currently the old function, doesn't protect against non-standard joints.

I disagree - the RobotState version supports arbitrary groups of joints that are not necessary inline, whereas these new ones are optimized for the most common use case of a simple kinematic chain where the vector is in order. This can't be easily configured to call into each other without a big performance hit.

Then this should probably be documented in the API.
And as long as there is still the 'old' RobotState::isValidVelocityMove() supporting non-standard joints I don't really see an issue here.
Is anything else missing to get your approval @rhaschke .

@davetcoleman wouldn't it better fit as a member function of JointModelGroup since the robot state is not used?

rhaschke · 2019-03-04T20:05:05Z

No further complaints from my side. But I like Henning's idea to move this into the JointModelGroup.

davetcoleman · 2019-03-04T23:20:03Z

That's a good point about not actually needing to be in RobotState but just JointModelGroup

I don't have time for this right now but we could go two routes:

Abandon this since we aren't currently working on Bolt
@henningkayser makes the quick move into JointModelGroup

Move isValidVelocityMove() to JointModelGroup

henningkayser

@rhaschke can we get this merged?

rhaschke · 2019-03-08T19:54:01Z

Should this be merged into master instead of melodic-devel?

davetcoleman · 2019-03-08T23:50:16Z

I'm the only current user of this API (via the Bolt planner) so its only needed in master. Retargeting.

davetcoleman · 2019-03-08T23:51:47Z

Looks like Travis has gotten a lot pickier... ?

rhaschke · 2019-03-09T00:48:59Z

@davetcoleman, I think pull requests shouldn't include merge commits (d6fb60c). Please obey our merge policies.

Looks like Travis has gotten a lot pickier... ?

Yes, and I'm very proud of the fact that I resolved all warnings such that Travis now can check for high code quality automatically. So please fix those warnings.
By the way, Travis only complains about warnings in the CI repo, i.e. the srdfdom warnings don't harm for MoveIt.

moveit_core/robot_model/src/joint_model_group.cpp

davetcoleman · 2019-03-14T21:11:42Z

Applied fix to clang warning

…en velocity (moveit#684) * New isValidVelocityMove() for checking time between two waypoints given velocity * Move isValidVelocityMove() to JointModelGroup * Update moveit_core/robot_model/src/joint_model_group.cpp

gavanderhoorn reviewed Nov 13, 2017

View reviewed changes

davetcoleman mentioned this pull request Nov 13, 2017

Update to package.xml davetcoleman/moveit#6

Closed

5 tasks

davetcoleman force-pushed the kinetic-bolt2 branch from 8b34f70 to 0105b3f Compare November 26, 2017 23:45

davetcoleman requested a review from v4hn March 29, 2018 14:21

rhaschke requested changes Apr 3, 2018

View reviewed changes

New isValidVelocityMove() for checking time between two waypoints giv…

c02475f

…en velocity

davetcoleman force-pushed the kinetic-bolt2 branch from 0105b3f to c02475f Compare October 25, 2018 21:01

davetcoleman changed the base branch from kinetic-devel to melodic-devel October 25, 2018 21:02

Move isValidVelocityMove() to JointModelGroup

0a9662c

henningkayser mentioned this pull request Mar 6, 2019

Move isValidVelocityMove() to JointModelGroup davetcoleman/moveit#9

Merged

Merge pull request #9 from henningkayser/kinetic-bolt2-pr

d6fb60c

Move isValidVelocityMove() to JointModelGroup

henningkayser approved these changes Mar 8, 2019

View reviewed changes

rhaschke approved these changes Mar 8, 2019

View reviewed changes

davetcoleman changed the base branch from melodic-devel to master March 8, 2019 23:50

davetcoleman commented Mar 14, 2019

View reviewed changes

moveit_core/robot_model/src/joint_model_group.cpp Outdated Show resolved Hide resolved

Update moveit_core/robot_model/src/joint_model_group.cpp

47fdad4

henningkayser merged commit 4e7f360 into moveit:master Mar 15, 2019

JafarAbdi pushed a commit to JafarAbdi/moveit that referenced this pull request Mar 24, 2022

Delete duplicate entry in Servo launch file (moveit#684)

cee20d6

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

New isValidVelocityMove() for checking time between two waypoints given velocity #684

New isValidVelocityMove() for checking time between two waypoints given velocity #684

davetcoleman commented Nov 13, 2017 •

edited

Loading

gavanderhoorn Nov 13, 2017

davetcoleman Nov 13, 2017

gavanderhoorn Nov 13, 2017

davetcoleman Nov 13, 2017

davetcoleman Oct 25, 2018

gavanderhoorn Nov 13, 2017

davetcoleman Nov 13, 2017

davetcoleman Oct 25, 2018

Jmeyer1292 commented Nov 25, 2017

Jmeyer1292 commented Nov 25, 2017

davetcoleman commented Nov 26, 2017

mlautman commented Dec 22, 2017

davetcoleman commented Mar 29, 2018

rhaschke left a comment

rhaschke Apr 3, 2018

davetcoleman Oct 25, 2018

davetcoleman commented Oct 25, 2018

davetcoleman commented Oct 25, 2018

henningkayser commented Mar 4, 2019

rhaschke commented Mar 4, 2019

davetcoleman commented Mar 4, 2019

henningkayser left a comment

rhaschke commented Mar 8, 2019

davetcoleman commented Mar 8, 2019

davetcoleman commented Mar 8, 2019

rhaschke commented Mar 9, 2019

davetcoleman commented Mar 14, 2019


		bool isValidVelocityMove(const JointModelGroup* group, const double* from_joint_pose,
		const double* to_joint_pose, std::size_t array_size, double dt) const;

New isValidVelocityMove() for checking time between two waypoints given velocity #684

New isValidVelocityMove() for checking time between two waypoints given velocity #684

Conversation

davetcoleman commented Nov 13, 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

Jmeyer1292 commented Nov 25, 2017

Jmeyer1292 commented Nov 25, 2017

davetcoleman commented Nov 26, 2017

mlautman commented Dec 22, 2017

davetcoleman commented Mar 29, 2018

rhaschke left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

davetcoleman commented Oct 25, 2018

davetcoleman commented Oct 25, 2018

henningkayser commented Mar 4, 2019

rhaschke commented Mar 4, 2019

davetcoleman commented Mar 4, 2019

henningkayser left a comment

Choose a reason for hiding this comment

rhaschke commented Mar 8, 2019

davetcoleman commented Mar 8, 2019

davetcoleman commented Mar 8, 2019

rhaschke commented Mar 9, 2019

davetcoleman commented Mar 14, 2019

davetcoleman commented Nov 13, 2017 •

edited

Loading