point-streaming: further development based on PR #88 #215

tdl-ua · 2018-05-16T23:02:03Z

I tried to address some of the comments made by @shaun-edwards in PR #88 (created by @jim-rees):

Comment 1: Made JointTrajectoryInterface::jointCommandCB() pure virtual so that JointTrajectoryInterface::jointCommandCB() implementation can be deleted
Comment 2: Message was changed to ROS_DEBUG
Comment 3: Not sure what the point of that sleep call is here so not sure what the constant would be called. No change made.
Comment 4: It is the same reconnect logic as in case TransferStates::STREAMING. I am assuming that it was added to be consistent with case TransferStates::STREAMING. Therefore, no change made.
Comment 5: see Comment 4.

As to why the state TransferStates::POINT_STREAMING comes up multiple times; I have no idea. Was wondering the same thing. Obviously, there must be a good reason for having a class MotomanJointTrajectoryStreamer that extends JointTrajectoryStreamer but I don't really understand it at this point.

I have tested the code on an sia5 manipulator and found that commanded constant-velocity motions are carried out very smoothly. I also teleoperated the manipulator using a 6 DOF haptic interface with this point-streaming implementation and also experienced very smooth end-effector motion.

original author: Brian O'Neil

@shaun-edwards

…al#88 * Addressed comments from @shaun-edwards: - Comment 1: Made JointTrajectoryInterface::jointCommandCB() pure virtual so that JointTrajectoryInterface::jointCommandCB() implementation can be deleted. - Comment 2: Message was changed to ROS_DEBUG * Some other minor changes (e.g., commenting)

gavanderhoorn · 2018-05-31T08:51:08Z

Thanks @tdl-tbslab for getting this up-to-date again. I'll see if I can test this out sometime soon.

@ted-miller: are there any things in here that catch your eye?

gavanderhoorn · 2018-05-31T08:57:51Z

@alemme, @andreaskoepf any comments? This sounds similar to what you described on ROS Discourse, sans the trapezoidal velocity generator (and possibly some other things you didn't mention).

andreaskoepf · 2018-06-01T12:40:05Z

@gavanderhoorn The approach of this PR and other point-streaming impls seems to be a dynamic online trajectory generation. The main difference to our TVP controller which runs inside the realtime OS of the controller is that we operate setpoint based and the setpoint can be changed in every cycle .. e.g. it does not continue the move to a previous target but immediately reacts upon the next target, which allows us especially to minimize latency, see my joystick video.

shaun-edwards · 2018-06-19T20:19:54Z

This has been suggested by multiple people. I'm inclined to accept it as is. Any thoughts @gavanderhoorn, @ted-miller, @alemme, @andreaskoepf?

I'll wait a couple of days, otherwise, I'll merge.

gavanderhoorn · 2018-06-19T20:25:00Z

motoman_driver/src/joint_trajectory_streamer.cpp

+        }
+        else if (reply_status.reply_.getResult() == MotionReplyResults::BUSY)
+        {
+          ROS_INFO("silently resending.");


BUSY can occur quite frequently. Changing this to a throttled log output might be good. Or perhaps even a debug.

gavanderhoorn · 2018-06-19T20:25:42Z

motoman_driver/src/joint_trajectory_streamer.cpp

+        }
+        if (reply_status.reply_.getResult() == MotionReplyResults::SUCCESS)
+        {
+          ROS_INFO("Point[%d] sent to controller", this->current_point_);


gavanderhoorn · 2018-06-20T11:57:05Z

motoman_driver/src/industrial_robot_client/joint_trajectory_streamer.cpp

+    trajectory_msgs::JointTrajectoryPoint rbt_pt, xform_pt;
+
+    // select / reorder joints for sending to robot
+    if (!select(msg->joint_names, msg->points[0], this->all_joint_names_, &rbt_pt))


Does select(..) also check that all required joints are present in the incoming msg?

If not, such a check would probably need to be added.

The function select(.) internally checks whether any element of msg->joint_names is empty or whether a matching "ROS element" can be found for the name of each joint in the message. If either of those two conditions are false, the function returns false. So I think that would be the check for all required joints being present.

The function select(.) internally checks whether any element of msg->joint_names is empty or whether a matching "ROS element" can be found for the name of each joint in the message. If either of those two conditions are false, the function returns false. So I think that would be the check for all required joints being present.

I agree with you on this. I think checking the return on select is adequate.

gavanderhoorn · 2018-06-20T11:58:19Z

motoman_driver/src/joint_trajectory_streamer.cpp

@@ -460,6 +460,74 @@ void MotomanJointTrajectoryStreamer::streamingThread()
        }
      }
      break;
+
+    case TransferStates::POINT_STREAMING:


@shaun-edwards: does this need to be added twice? Once here and once in the industrial_robot_client extension part of this package?

gavanderhoorn · 2018-06-20T11:58:49Z

motoman_driver/src/joint_trajectory_streamer.cpp

+      // otherwise, send point to robot.
+      tmpMsg = this->streaming_queue_.front();
+      msg.init(tmpMsg.getMessageType(), CommTypes::SERVICE_REQUEST,
+               ReplyTypes::INVALID, tmpMsg.getData());  // set commType=REQUEST


Should the comment be removed?

gavanderhoorn · 2018-06-20T12:01:53Z

motoman_driver/include/motoman_driver/joint_trajectory_streamer.h

@@ -142,6 +144,10 @@ class MotomanJointTrajectoryStreamer : public JointTrajectoryStreamer

  static bool VectorToJointData(const std::vector<double> &vec,
                                industrial::joint_data::JointData &joints);
+
+  double time_of_last;
+  double time_since_last;


Suggestion: rename these to include "of what" they are the "time of last". Right now these variable names say nothing about what this is used for, and without the context of them being used by the new point streaming code, it's rather difficult to understand what they are doing here.

There are also no comments associated with these lines.

gavanderhoorn · 2018-06-20T12:02:10Z

motoman_driver/include/motoman_driver/joint_trajectory_streamer.h

+
+  double time_of_last;
+  double time_since_last;
+  static const double point_streaming_timeout = 3.0;


Please add // seconds.

gavanderhoorn · 2018-06-20T12:02:56Z

motoman_driver/include/motoman_driver/industrial_robot_client/joint_trajectory_streamer.h

@@ -129,11 +132,13 @@ class JointTrajectoryStreamer : public JointTrajectoryInterface

  boost::thread* streaming_thread_;
  boost::mutex mutex_;
-  int current_point_;
+  int current_point_, streaming_sequence;


is this missing a trailing underscore?

Additional suggestion: what is this a sequence number of / for?

gavanderhoorn · 2018-06-20T12:03:59Z

I've added a few more comments (apologies, I should'v done a review instead).

Overall question: does this support multi-group systems?

tdl-ua · 2018-06-20T15:25:14Z

Thanks for all the revisions @gavanderhoorn. I will revise the code accordingly and add a commit.

As for multi-group support; I don't know. Am not too familiar with how the driver handles multiple manipulators. But @jim-rees comments the following in PR #88: "I've had a report that this does not work with dual arm robots, because they require motoman_msgs/DynamicJointTrajectory messages which this doesn't implement."

gavanderhoorn · 2018-06-20T15:30:00Z

Thanks for iterating on this @tdl-tbslab. Much appreciated.

re: multi-group: yes, the DynamicJointPoint would be needed. It should not be too hard to add, but we might not want to complicate things in this PR.

tdl-ua · 2018-06-20T17:39:49Z

I am assuming that multi-group refers to multi-arm setups such as the SDA5. Unfortunately, we do not have such a dual arm setup in our lab, so I would probably not be able to implement that feature.

gavanderhoorn · 2018-06-21T08:16:42Z

I am assuming that multi-group refers to multi-arm setups such as the SDA5.

not necessarily. Work cells with a single arm on a linear track, or an arm with a 2-axis positioner can also be setup as multi-group systems.

The SDA is just a clever way to use this to create a 'humanoid-like' torso + arms setup.

But as I wrote above: let's not complicate this PR.

@gavanderhoorn

Addressed most of @gavanderhoorn's comments, except for comments 3 and 4.

shaun-edwards · 2018-07-29T17:10:41Z

@gavanderhoorn, is this good to go?

tdl-ua · 2018-09-21T22:06:28Z

ping @gavanderhoorn

dambrosio · 2019-02-09T20:00:28Z

All,
I am willing to update this PR with all the latest kinetic-devel commits and test on our GP7. I did have a few additions that I think we should add (see comments in PR commits).

Thoughts on trying to get this tested, updated with latest kinetic_devel and the least confusing way to update this PR?

dambrosio · 2019-02-09T20:01:33Z

motoman_driver/include/motoman_driver/joint_trajectory_streamer.h

+  // variables for point streaming
+  double time_ptstreaming_last_point_;  // time at which the last point was received
+  double dt_ptstreaming_points_;        // elapsed time between two received points
+  static const double ptstreaming_timeout_ = 3.0; // seconds


Should we make this a parameter that defaults to 3.0 seconds?

We can but I guess it is already covered by "Force a transition to the IDLE state if we get a message with no-points" right?

dambrosio · 2019-02-09T20:03:42Z

motoman_driver/src/industrial_robot_client/joint_trajectory_streamer.cpp

+    this->state_ = TransferStates::POINT_STREAMING;
+    this->current_point_ = 0;
+    this->ptstreaming_seq_count_ = 0;
+    this->streaming_start_ = ros::Time::now();


Reset / clear the queue to prevent sending old trajectory points:

this->ptstreaming_queue_ = std::queue<SimpleMessage>();

dambrosio · 2019-02-09T20:06:33Z

motoman_driver/src/industrial_robot_client/joint_trajectory_streamer.cpp

+    if (msg->points.empty())
+    {
+      ROS_INFO("Empty point received, cancelling current trajectory");
+      return;


Force a transition to the IDLE state if we get a message with no-points, this functionality will allow users publishing to the joint_command topic to "instantly" stop POINT_STREAMING without waiting for timeout.

this->mutex_.lock(); trajectoryStop(); this->mutex_.unlock();

This would be a good addition I think.

tdl-ua · 2019-02-11T18:51:30Z

Hi @dambrosio, thanks for the reviews. I'll have a look at them later tonight and might have time to take care of this on Wednesday including the kinetic-devel update and incorporating the suggested modifications. I'll then add a commit. I think that should be the least messy way of adding your modifications to this PR.

dambrosio · 2019-02-11T20:05:22Z

Hi @dambrosio, thanks for the reviews. I'll have a look at them later tonight and might have time to take care of this on Wednesday including the kinetic-devel update and incorporating the suggested modifications. I'll then add a commit. I think that should be the least messy way of adding your modifications to this PR.

Sounds good. I will be testing this on a GP7 this afternoon and will let you know how things go.

dambrosio · 2019-02-12T14:35:55Z

Hi @dambrosio, thanks for the reviews. I'll have a look at them later tonight and might have time to take care of this on Wednesday including the kinetic-devel update and incorporating the suggested modifications. I'll then add a commit. I think that should be the least messy way of adding your modifications to this PR.

Sounds good. I will be testing this on a GP7 this afternoon and will let you know how things go.

Quick update: I tried these changes yesterday and ran into issues with MotoRos reply messages in response to sending a single trajectory point. Here is the ROS console output

[ WARN] [1549936100.819839659]: Motoman robot is now enabled and will accept motion commands.
[ INFO] [1549936105.338233468]: First joint point received. Starting on-the-fly streaming.
[ WARN] [1549936105.410549892]: Reply String: Invalid message, Sub string: Trajectory start position doesn't match current robot position
[ERROR] [1549936105.410620147]: Aborting point stream operation.  Failed to send point (#0): Invalid message (3) : Trajectory start position doesn't match current robot position (3011)

The trajectory_msgs/JointTrajectory message contains a single JointTrajectoryPoint that has different joint positions from those being reported on the joint_states topic.

I was thinking of updating the POINT_STREAMING mode to process two trajectory points in the jointCommandCB(), where the first is the current robot joint positions and the second is the desired joint positions. I realized that these suggested updates would almost replicate the functionality of what is provided in the STREAMING mode?

Update: Seems like the original PR was tested using this node, I'll try to run with it and see if I get similar errors to what I described above.

Update 2/14/19: Yesterday I managed to get point streaming to work on our GP7 manipulator. The jogger node used two very important (yet masked) steps in getting the streaming points successfully propagated through MotoROS.

The first trajectory point (let's call this the start point) must contain the current joint positions with all zero velocity values
Each following points must contain non-zero velocity values for each position and the time_from_start value must be greater than the previous point.

Item 2 is extremely important. I believe the start point should have a time_from_start equal to zero and each point should increase from there. For example: start pt - 0.0 [s], pt 1 - 0.02 [s], pt2 - 0.04 [s], etc. The time expressed in the example is just for reference and not actual values.

After minimal testing it was found that point streaming can successfully operate around 50 Hz max. If the user is not careful and sends joint_command messages faster than this the internal point streaming queue can grow without bounds. It is probably a good idea to check for a maximum queue size and either drop messages that would cause the queue to grow past this maximum or to error and transition back to IDLE.

@tdl-ua I have added checks to help the user follow these requirements when using point streaming mode, I will push to my forked branch and give you a link. If you would like to use any changes I have made please feel free.

dambrosio · 2019-02-12T14:50:54Z

motoman_driver/src/industrial_robot_client/joint_trajectory_interface.cpp

@@ -126,6 +126,8 @@ bool JointTrajectoryInterface::init(SmplMsgConnection* connection, const std::ve
  this->sub_cur_pos_ = this->node_.subscribe(
                         "joint_states", 1, &JointTrajectoryInterface::jointStateCB, this);

+  this->sub_joint_command_ = this->node_.subscribe("joint_command", 0, &JointTrajectoryInterface::jointCommandCB, this);


We should just double check that an infinite queue size makes sense for point streaming. Should we drop commands if the streaming node can cycle fast enough, or should we continue to fill up the command queue?

dambrosio · 2019-02-12T14:54:15Z

motoman_driver/src/industrial_robot_client/joint_trajectory_streamer.cpp

+  //If current state is idle, set to POINT_STREAMING
+  if (TransferStates::IDLE == state)
+  {
+    this->mutex_.lock();


I think we could get in an undesired state transition if we get a new msg with empty points and we are we are currently in IDLE. Maybe we should check if the msg->points.empty() and if true report a warning, and return.

This should be handled above on line 185.

tdl-ua · 2019-02-14T15:45:08Z

All,
I am willing to update this PR with all the latest kinetic-devel commits and test on our GP7. I did have a few additions that I think we should add (see comments in PR commits).

Thoughts on trying to get this tested, updated with latest kinetic_devel and the least confusing way to update this PR?

@dambrosio I've added you as a collaborator to the tbs-ualberta fork now so that you can push commits to the PR directly as I currently don't have time to work on this. Feel free to merge point-streaming with kinetic-devel and add your changes. Once I have the chance, I'll also test the updated PR on our SIA5.

dambrosio · 2019-02-14T16:05:40Z

All,
I am willing to update this PR with all the latest kinetic-devel commits and test on our GP7. I did have a few additions that I think we should add (see comments in PR commits).
Thoughts on trying to get this tested, updated with latest kinetic_devel and the least confusing way to update this PR?

@dambrosio I've added you as a collaborator to the tbs-ualberta fork now so that you can push commits to the PR directly as I currently don't have time to work on this. Feel free to merge point-streaming with kinetic-devel and add your changes. Once I have the chance, I'll also test the updated PR on our SIA5.

I'll be able to take a look at all of this next week. I will merge and add any of my changes for review. If I have time I will try and provide a mechanism you can use to do tool tip velocity control using the new POINT_STREAM state.

tdl-ua · 2019-02-14T18:04:07Z

All,
I am willing to update this PR with all the latest kinetic-devel commits and test on our GP7. I did have a few additions that I think we should add (see comments in PR commits).
Thoughts on trying to get this tested, updated with latest kinetic_devel and the least confusing way to update this PR?

@dambrosio I've added you as a collaborator to the tbs-ualberta fork now so that you can push commits to the PR directly as I currently don't have time to work on this. Feel free to merge point-streaming with kinetic-devel and add your changes. Once I have the chance, I'll also test the updated PR on our SIA5.

I'll be able to take a look at all of this next week. I will merge and add any of my changes for review. If I have time I will try and provide a mechanism you can use to do tool tip velocity control using the new POINT_STREAM state.

I've developed something in the past that does essentially velocity control in cartesian space (basically end-effector teleoperation): https://github.com/tbs-ualberta/manipulator_pose_following where I think I've addressed some of the issues you mention above.

The package is a work in progress as obtaining the IK still needs to be improved and the code needs to be made more manipulator-agnostic but it is working.

…_STREAMING state, removed updates to local data that didn't correspond to POINT_STREAMING state

dambrosio · 2019-02-19T14:07:20Z

@tdl-ua quick FYI, this branch should be good for testing.

tdl-ua · 2019-02-19T15:34:41Z

@tdl-ua quick FYI, this branch should be good for testing.

@dambrosio I'll see if I can test it tonight.

tdl-ua · 2019-02-24T00:38:19Z

@dambrosio I've finally had a chance to test your commits on our SIA5. I tried to jog the EEF in cartesian space and did not run into any issues.

I saw that you added checks on streaming queue bounds and time_from_start increase. I would agree that those checks are important to help the user follow the point streaming requirements. We can also calculate the frequency of incoming points from the difference between the time_from_start of two subsequent points and stop streaming with an error message if the frequency exceeds 50 Hz. The feedback to the user would then be much more informative.

jim-rees · 2019-03-27T20:01:58Z

Please see my comments about latency and jitter at #252 .

tall1 · 2023-06-27T11:29:30Z

Hi,
Following #555 - I want to use this PR as @gavanderhoorn suggested it might work.

I have an HC10DTP + YRC1000micro controller.
We're currently on ROS noetic (Ubuntu 20.04).

Will this work? What adjustments do I need to make in order to enable point streaming with my controller?

jim-rees and others added 4 commits April 22, 2016 14:50

merge branch boneil_point_streaming manually

374c382

original author: Brian O'Neil

Merge branch 'kinetic-devel' into point-streaming

23aef0a

Merge branch 'kinetic-devel' into point-streaming

5b3326e

Danfoa mentioned this pull request May 31, 2018

Trajectory start position doesnt match current robot position on CSDA10F dual arm robot #217

Closed

gavanderhoorn reviewed Jun 19, 2018

View reviewed changes

gavanderhoorn reviewed Jun 20, 2018

View reviewed changes

point-streaming: address @gavanderhoorn's comments

4440bd8

Addressed most of @gavanderhoorn's comments, except for comments 3 and 4.

tdl-ua force-pushed the point-streaming branch from e29cf33 to 4440bd8 Compare June 26, 2018 19:22

Merge branch 'kinetic-devel' into point-streaming

af87ce0

JSook mentioned this pull request Nov 1, 2018

Motion delay while point streaming for real-time control #252

Closed

dambrosio reviewed Feb 9, 2019

View reviewed changes

dambrosio reviewed Feb 12, 2019

View reviewed changes

dambrosio added 2 commits February 14, 2019 12:19

Merge branch 'kinetic-devel' into point-streaming

3f199c9

Added sequence checks in jointCommandCB to enable, stop and run POINT…

8ca5016

…_STREAMING state, removed updates to local data that didn't correspond to POINT_STREAMING state

jarvisschultz mentioned this pull request Apr 6, 2019

Implications of high Command - Feedback latency #219

Closed

1 task

gavanderhoorn mentioned this pull request Oct 6, 2019

Did the driver support joint velocity control or joint torque control? #297

Closed

harumo11 mentioned this pull request Jan 7, 2020

Motoman driver with point streaming gets error "Trajectory start position doesn't match current robot position (3011)" #312

Open

tanimfresh mentioned this pull request Apr 14, 2020

driver: persistent (and incorrect) "Trajectory start position doesn't match current robot position" #111

Open

ted-miller mentioned this pull request Apr 23, 2020

Error "Validation failed: Trajectory doesn't start at current position." when executing high speed back to back moves #324

Open

gavanderhoorn mentioned this pull request May 8, 2020

On-line joint motion control using VAL 3 driver ros-industrial/staubli_val3_driver#20

Closed

gavanderhoorn mentioned this pull request Feb 3, 2021

Choppy motion of ROS controlled yaskawa GP8 robot #386

Closed

RoboRiz mentioned this pull request Apr 13, 2021

Running Single Group on Dual-Arm Setup (Point Streaming) #401

Closed

gavanderhoorn mentioned this pull request Jun 7, 2021

Realtime arm servoing with Staubli TX2-90 ros-industrial/staubli_val3_driver#32

Open

gavanderhoorn mentioned this pull request Aug 23, 2021

Point streaming queue has reached max allowed elements #420

Closed

MaxorPaxor mentioned this pull request Jan 19, 2022

fail to build point_streaming #446

Closed

gavanderhoorn added the backlog Will be addressed at a later time label Feb 3, 2022

point-streaming: further development based on PR #88 #215

Are you sure you want to change the base?

point-streaming: further development based on PR #88 #215

Conversation

tdl-ua commented May 16, 2018 • edited

gavanderhoorn commented May 31, 2018

gavanderhoorn commented May 31, 2018

andreaskoepf commented Jun 1, 2018

shaun-edwards commented Jun 19, 2018

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

gavanderhoorn commented Jun 20, 2018

tdl-ua commented Jun 20, 2018

gavanderhoorn commented Jun 20, 2018

tdl-ua commented Jun 20, 2018 • edited

gavanderhoorn commented Jun 21, 2018 • edited

shaun-edwards commented Jul 29, 2018

tdl-ua commented Sep 21, 2018

dambrosio commented Feb 9, 2019

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

tdl-ua commented Feb 11, 2019

dambrosio commented Feb 11, 2019

dambrosio commented Feb 12, 2019 • edited

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

tdl-ua commented Feb 14, 2019

dambrosio commented Feb 14, 2019

tdl-ua commented Feb 14, 2019

dambrosio commented Feb 19, 2019

tdl-ua commented Feb 19, 2019

tdl-ua commented Feb 24, 2019

jim-rees commented Mar 27, 2019

tall1 commented Jun 27, 2023 • edited

tdl-ua commented May 16, 2018 •

edited

tdl-ua commented Jun 20, 2018 •

edited

gavanderhoorn commented Jun 21, 2018 •

edited

dambrosio commented Feb 12, 2019 •

edited

tall1 commented Jun 27, 2023 •

edited