Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

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

Open
wants to merge 8 commits into
base: kinetic-devel
Choose a base branch
from

Conversation

tdl-ua
Copy link
Contributor

@tdl-ua tdl-ua commented May 16, 2018

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.

jim-rees and others added 4 commits April 22, 2016 14:50
original author: Brian O'Neil
…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
Copy link
Member

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
Copy link
Member

@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
Copy link
Contributor

@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
Copy link
Member

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.

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

Choose a reason for hiding this comment

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

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

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

Choose a reason for hiding this comment

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

Same here.

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))
Copy link
Member

Choose a reason for hiding this comment

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

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.

Copy link
Contributor Author

Choose a reason for hiding this comment

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

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.

Copy link
Contributor

Choose a reason for hiding this comment

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

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.

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

case TransferStates::POINT_STREAMING:
Copy link
Member

Choose a reason for hiding this comment

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

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

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

Choose a reason for hiding this comment

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

Should the comment be removed?

@@ -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;
Copy link
Member

Choose a reason for hiding this comment

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

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.


double time_of_last;
double time_since_last;
static const double point_streaming_timeout = 3.0;
Copy link
Member

Choose a reason for hiding this comment

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

Please add // seconds.

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

boost::thread* streaming_thread_;
boost::mutex mutex_;
int current_point_;
int current_point_, streaming_sequence;
Copy link
Member

Choose a reason for hiding this comment

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

is this missing a trailing underscore?

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

@gavanderhoorn
Copy link
Member

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
Copy link
Contributor Author

tdl-ua commented Jun 20, 2018

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
Copy link
Member

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
Copy link
Contributor Author

tdl-ua commented Jun 20, 2018

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
Copy link
Member

gavanderhoorn commented Jun 21, 2018

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.

Addressed most of @gavanderhoorn's comments, except for comments 3 and 4.
@shaun-edwards
Copy link
Member

@gavanderhoorn, is this good to go?

@tdl-ua
Copy link
Contributor Author

tdl-ua commented Sep 21, 2018

ping @gavanderhoorn

@dambrosio
Copy link
Contributor

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?

// 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
Copy link
Contributor

Choose a reason for hiding this comment

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

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

Copy link
Contributor Author

Choose a reason for hiding this comment

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

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?

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

Choose a reason for hiding this comment

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

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

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

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

Choose a reason for hiding this comment

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

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();

Copy link
Contributor Author

Choose a reason for hiding this comment

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

This would be a good addition I think.

@tdl-ua
Copy link
Contributor Author

tdl-ua commented Feb 11, 2019

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
Copy link
Contributor

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
Copy link
Contributor

dambrosio commented Feb 12, 2019

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.

  1. The first trajectory point (let's call this the start point) must contain the current joint positions with all zero velocity values
  2. 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.

@@ -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);
Copy link
Contributor

Choose a reason for hiding this comment

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

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?

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

Choose a reason for hiding this comment

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

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.

Copy link
Contributor

Choose a reason for hiding this comment

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

This should be handled above on line 185.

@tdl-ua
Copy link
Contributor Author

tdl-ua commented Feb 14, 2019

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
Copy link
Contributor

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
Copy link
Contributor Author

tdl-ua commented Feb 14, 2019

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
Copy link
Contributor

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

@tdl-ua
Copy link
Contributor Author

tdl-ua commented Feb 19, 2019

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

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

@tdl-ua
Copy link
Contributor Author

tdl-ua commented Feb 24, 2019

@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
Copy link

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

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

tall1 commented Jun 27, 2023

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?

@DanManN
Copy link

DanManN commented Jun 25, 2024

At Rutgers we have a sda10f robot (one of the dual arm manipulators) and I maintain a custom fork of this repo, mostly containing custom configurations for our experimental setup. Based on this pull request, however, I was able to recently implement the point streaming functionality. I don't think my code modifications are production ready by any means but I wanted to advertise them here for anyone who was having trouble getting point streaming working for their system. I would also love to help merge this functionality upstream but I'm not sure how to test code compatibility with other controllers and robot systems.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
backlog Will be addressed at a later time
Development

Successfully merging this pull request may close these issues.

8 participants