search

ros-industrial / motoman

ROS-Industrial Motoman support (http://wiki.ros.org/motoman)
145 stars 192 forks source link

Joint feedback - Velocity and Acceleration #174

Closed ted-miller closed 6 years ago

ted-miller commented 7 years ago

It just came to my attention that the Joint Feedback messages have fields for velocity and acceleration. It would appear that this data gets populated into the velocity/effort fields of [rostopic echo /joint_states].

However, the MotoROS driver doesn't actually populate these fields. The [validFields] mask is set to 2 for position only.

Is this ok? Should I be populating the velocity/acc fields? Is this data useful for anything?

gavanderhoorn commented 7 years ago

If the controller makes that data available, then yes please, populate it. Same goes for the effort fields (Ros_CtrlGroup_GetTorque(..)?).

As to the usefulness: I would say it's always useful to have as much state information available as possible, even if right now use-cases might not be apparent.

Personally I would however not derive those values (by dx/dt fi).

ted-miller commented 7 years ago

I can get the feedback-speed and torque for each axis on FS100, DX200, and YRC1000 (not DX100).

However, as best I can tell from looking at MotoROS and the Industrial Core, the joint feedback message isn't designed to accept effort/torque. Are you suggesting that I overwrite the acceleration field with the torque? Or that I add a new member to the feedback-message? If I increase the size of the feedback message, will that break anything in the Industrial Core? (I haven't delved into it in great detail yet)

shaun-edwards commented 7 years ago

The ROS side and MotoPlus size message definitions have to match. We could make that happen, but I'm more worried about sending even more data through the controller. Does the controller have a max packet size for socket comms?

ted-miller commented 7 years ago

Probably... but I expect that the TCP stack on the controller should accommodate the normal MTU for ethernet, which is 1500 bytes (need to test, of course). If I add a torque field, it would put a SmBodyJointFeedbackEx packet at 692 bytes.

gavanderhoorn commented 7 years ago

@ted-miller wrote:

However, as best I can tell from looking at MotoROS and the Industrial Core, the joint feedback message isn't designed to accept effort/torque.

O right, I misremembered JOINT_FEEDBACK_EX having the effort fields as well. It is only available in IRCv2.

Are you suggesting that I overwrite the acceleration field with the torque? Or that I add a new member to the feedback-message? If I increase the size of the feedback message, will that break anything in the Industrial Core? (I haven't delved into it in great detail yet)

Well, we could, but that would mean an update to the ROS side as well. Not sure that is worth it. Might as well do that when/if motoros migrates to IRCv2.

gavanderhoorn commented 7 years ago

@shaun-edwards wrote:

I'm more worried about sending even more data through the controller. Does the controller have a max packet size for socket comms?

I'm not too worried about this. The TCP/IP stack on the Motoman controllers is pretty sane (after all, it's vxworks), and MotoPlus applications have (almost) direct access to that.

gavanderhoorn commented 6 years ago

@ted-miller: would adding the velocity data to the JOINT_FEEDBACK_EX msgs be something you could implement?

For some things we're starting to look at here it might come in handy to have that data available.

ted-miller commented 6 years ago

Sure.

gavanderhoorn commented 6 years ago

And a question on ROS Answers about this: Motoman FS100: Missing velocity and effort in joint_states.

(note that velocity and effort aren't missing, I'm pretty sure the OP misremembered).

ted-miller commented 6 years ago

@gavanderhoorn See https://github.com/ted-miller/velocity_feedback I haven't actually tested this yet beyond compiling.

gavanderhoorn commented 6 years ago

Thanks.

Did you remove all pkgs except motoman_driver in that commit as well?

For the future; could you please commit such large changes in a separate commit? The additions for the velocity feedback are now burried in a commit that touches 351 files ..

ted-miller commented 6 years ago

Yeah... I realized that after I committed. Let me know if you need me to redo this repo.

I've attached a comparison report that shows the files that were actually changed.

fileschanged

gavanderhoorn commented 6 years ago

It would be really nice if you could push the same changes, but then either not delete the pkgs, or do everything in two commits.

You could also do this in a separate branch on your regular motoman fork, if that would be more convenient.

ted-miller commented 6 years ago

I just reverted and re-pushed the changes.

gavanderhoorn commented 6 years ago

Just tested this.

Haven't validated the output (ie: correspondence between commanded and reported velocities), but MotoROS seems to function as it should and I get sane data on the /joint_states topics.

Is there a rate limit on the velocity feedback that MotoPlus makes available? It appears to lag a bit and seems a bit discretised as well.

s_pos_vel

ted-miller commented 6 years ago

I'm not sure. I've never actually used this API before, but I expected it to have the same behavior as the feedback-position API.

I'll need to run some additional tests.

ted-miller commented 6 years ago

I have run some additional testing and got similar results to yours. I compared the values to the [Servo Monitor] function on the pendant and they are identical (including the aggregation/discretization). So, I believe that this result is the best I can obtain using the MotoPlus API for feedback speed. I will prepare a report and send to HQ in Japan... but this will take a long a time to discuss and potentially improve.

There is one other way to obtain the feedback speed without additional hardware. I ran a quick test and found that this does provide data with finer granularity. But there are couple drawbacks:

While I'm at it, is this a good time to implement feedback torque? If there were some kind of version-check message implemented in motoman-driver, I could use it to maintain backward compatibility of the MotoROS portion.

gavanderhoorn commented 6 years ago

Thanks for the update.

I suspect the delay for the feedback will be greater because the data has to get routed through a few layers in the controller before I can access it in MotoROS. But, given that it's a more accurate representation of the instantaneous speed, maybe this is a moot point.

Delay is undesirable. But discretisation is also undesirable. I can't really decide whether one is worse than the other. In most cases delayed data is unusable, whether it's discretised or not (too old == too old). And for applications where info about velocity is interesting, I believe that delay would always be undesirable.

There are a bunch of protected robot parameters which must be enabled to use this function. Since we just got away from requiring yaskawa-level parameters, I don't want to go back to it. I have an idea of how it could possibly be automated in MotoROS... but that piece could not be published open-source. I would need to put it in a pre-compiled library.

That would not be different from how the 'parameter extraction' works right now, correct?

This would consume a LOT of robot M registers. If someone starts to combine MotoROS with other robot options, it's very possible there could be a resource conflict.

That could definitely be a concern. Personally I think it's nice to have velocity feedback available, but not at all costs.

While I'm at it, is this a good time to implement feedback torque? If there were some kind of version-check message implemented in motoman-driver, I could use it to maintain backward compatibility of the MotoROS portion.

There is no versioning in the protocol unfortunately, so that is not something we can exploit.

The MSG_MOTO_JOINT_FEEDBACK_EX does not include effort, only acceleration. If we want to add this, we'd either have to repurpose the acceleration field or introduce a new message (IRCv2 does actually include effort in its 'Dynamic Joint State' message).

mbharatheesha commented 6 years ago

Have been a long time lurker here. It would certainly be nice to see information beyond positions on the robot state feedback when one starts thinking about realizing dynamic behaviours (I mean, those that make use of the robot dynamics).

Delay is undesirable. But discretisation is also undesirable. I can't really decide whether one is worse than the other. In most cases delayed data is unusable, whether it's discretised or not (too old == too old). And for applications where info about velocity is interesting, I believe that delay would always be undesirable.

With regard to having to pick between delayed or discretised information, the latter is perhaps preferable imo. As already pointed out, delays are definitely undesirable and require quite some attention from the user to compensate for them before acting on the information. However, with discretised information, there is at least a possibility to perform a naïve linear extrapolation of the current value based on the sampling interval (which would take relatively less time). Of course, such (a) workaround(s) would be with an assumption that the information (velocity in this context) is not drastically changing within one sampling interval.

ted-miller commented 6 years ago

I have performed testing to compare the MotoPlus API vs the M-Register method to read feedback speed. It turns out I was wrong about the delay. The M-Register method is definitely superior. FeedbackTester Results.xlsx So, I need to investigate the viability of deploying the M-Register method and automating it inside of the param-extraction library. (I'm not even sure if this is an "official" controller feature.)

gavanderhoorn commented 6 years ago

That's certainly an improvement @ted-miller:

plot

(ignore the sign on the second part of the plot, it's incorrect).

Both the delay as well as the discretisation of the MotoPlus API feedback is clearly visible here.

ted-miller commented 6 years ago

@gavanderhoorn See https://github.com/ted-miller/velocity_feedback/commit/c1cec328cc2caae792301ca6062b78cc7cb617ef I have implemented the change to the Param Extraction library to automatically enable all the parameters for high-speed Register Speed Feedback. It turns out that these are all user-level parameters (not protected)... but there's a bunch of them and this method helps to eliminate confusion/error. At startup, the library will attempt to read the param values. If they are not set, it will automatically enable them. This will require a reboot the first time it is done, so I raise an alarm, "8001 [10] Speed FB enabled, reboot now". After that, velocity data will be included with the Joint Feedback messages.

The high-speed register method is only available for DX200 and newer. So, for DX100 and FS100, I am disabling velocity feedback entirely. I figure that MotoPlus-API method (above) is pretty useless. Thoughts?

If this gets merged into the main branch, I plan to add some wiki pages with these details.

ted-miller commented 6 years ago

Are there any known conflicts or issues if I merge these changes into the main branch and submit a PR? I'm assuming that the existing ROS-I code can handle velocity data being transmitted, even if it's not used by the final application.

gavanderhoorn commented 6 years ago

I would suggest creating a new branch on your regular motoman_driver fork. Commit the changes to that fork, and open a PR from that fork against ros-industrial/motoman.

That way even if a review / testing round takes some more time, you won't be 'blocked', as you can just work in a different branch.

I can't test this - as I only have an FS100 - but perhaps we can find someone with a DX200 or YRC1000 that could take a quick look.

I'll take a look at the ROS side to make sure that the deserialisation results in proper JointState msgs with the velocity fields populated. I'm assuming it's all ok, as it already worked with what you posted in https://github.com/ros-industrial/motoman/issues/174#issuecomment-347301240, but it's good to check in any case.

ted-miller commented 6 years ago

I'll ask Erc Marcil to validate on my end.

ted-miller commented 6 years ago

@gavanderhoorn If you need this feature on the FS100, you could try the API mpSvsGetVelTrqFb. This gets speed and torque feedback at a good rate. (I only recently learned of this API.)

gavanderhoorn commented 6 years ago

Thanks. That is good to know.

Would you accept a PR adding that to the FS100 code paths in motoros? :)

ted-miller commented 6 years ago

Yeah. I can probably get around to it on Monday. Or, feel free to submit it sooner.

gavanderhoorn commented 6 years ago

Joint Velocity feedback was added in #191 (MotoRos v1.7.0, DX200 and YRC1000) and #205 (MotoRos v1.7.2, FS100).

Effort is available on the controller, but due to limitations in the simple message msgs currently used by the driver, cannot be reported back to ROS.

gavanderhoorn commented 6 years ago

Closing this, as for now this has been addressed/implemented as far as is possible.

We'll revisit adding effort to the state reporting msgs at a later time.

nexus1203 commented 2 years ago

@gavanderhoorn See ted-miller/velocity_feedback@c1cec32 I have implemented the change to the Param Extraction library to automatically enable all the parameters for high-speed Register Speed Feedback. It turns out that these are all user-level parameters (not protected)... but there's a bunch of them and this method helps to eliminate confusion/error. At startup, the library will attempt to read the param values. If they are not set, it will automatically enable them. This will require a reboot the first time it is done, so I raise an alarm, "8001 [10] Speed FB enabled, reboot now". After that, velocity data will be included with the Joint Feedback messages.

The high-speed register method is only available for DX200 and newer. So, for DX100 and FS100, I am disabling velocity feedback entirely. I figure that MotoPlus-API method (above) is pretty useless. Thoughts?

If this gets merged into the main branch, I plan to add some wiki pages with these details.

Can someone shed some light on "high-speed register method"? Is it a built-in standard function on DX200 controllers like "high-speed Ethernet server"? Or something else. I am trying to acquire torque data on MH12 robot on DX200. So far I have tried "high speed Ethernet server" but data rate is limited to 20ms per request and response cycle. I haven't been able to achieve torque read through ROS either. Is it possible to reach higher readout frequency like the above velocity readout shown above?

ted-miller commented 2 years ago

@nexus1203, The "register method" is a feature of the robot controller. When enabled, the robot controller will automatically populate M-Registers with the percentage of max torque. (Not an absolute Nm.) You would then need to move this data out of the robot using High Speed Ethernet Server, or MotoPlus, or a fieldbus connection like Ethernet/IP.

If you are using ROS, then there is a function in the code to read the feedback torque (Nm). https://github.com/ros-industrial/motoman/blob/7860ff545106d98ed6a3fa0121f2fb60891f69bd/motoman_driver/MotoPlus/CtrlGroup.c#L454-L477

But, this function isn't actually called by anything, nor is the data linked to any feedback messages from MotoROS > motoman_driver. So, you could add this functionality with some modifications to both MotoROS and motoman_driver.

Steps on the MotoROS side: • Add a new command ID and message format for SimpleMessage, which is the communication between robot controller and PC. • Add code to the StateServer to generate this feedback message and send it to the PC. I recommend adding this in the same spot that sends position feedback.

Alternatively, you could modify the format of the existing feedback message to include the torque data.

Either option require modifications to the motoman_driver code on the PC. It needs to either accept the newly created command, or accept the modified format of the existing feedback command.

nexus1203 commented 2 years ago

@ted-miller Thank you very much for your explanation. May you please point me to a documentation for this method? I went through the manuals of "high speed Ethernet server" and "Ethernet IP server" functions but I can't seem to find a reference to how to enable this function. I do know I can read M variables using these variables but I am trying to figure out how to actually move data to M variable so that I can read it through high speed Ethernet server method or Ethernet IP.

ted-miller commented 2 years ago

I don't believe that any formal documentation exists for this feature. I think it is technically not an "officially supported" feature. But, I did just verify that it works on my YRC1000.

S1CxG122: Enable/Disable. Each bit corresponds with an axis. 

- Ex: Bit 0 = [Enable S Axis], Bit 1 = [Enable L Axis], etc...

DX200: S1CxG488 ~ 495: Register number for each axis' value YRC1000: S1CxG910 ~ 919: Register number for each axis' value

- Ex: [S1CxG 488/910 = 50]: M050 will contain the torque percentage for S axis, if [S1CxG122:Bit0 == 1]. The value is a signed value, based on the direction of rotation.
nexus1203 commented 2 years ago

@ted-miller After trying for over a week and so. I am still not able to read M-registers on DX200. I am using high-speed ethernet function and generally M register should be available at 0x79 (Register data reading / writing command). However when reading these registers, the value is always returned as 0. In your previous comment you mentioned:

When enabled, the robot controller will automatically populate M-Registers with the percentage of max torque.

Is it function need to be enabled manually? On the controller side, high-speed ethernet function is enabled, all the other commands such as 0x76 and 0x77 works fine. But I am really hitting a wall with M register method. For the context, What I am trying to achieve here is a reliable way to measure torque reading (preferably >= 200Hz). However, I am stuck with 25ms update rate on ROS, and 10ms request/response time on high speed ethernet function. From my understanding High speed ethernet function has fixed request/response time and any faster than 10ms communication is not possible (I am really hoping that I am wrong).

ted-miller commented 2 years ago

Yes, this function needs to enabled manually, using the robot parameters listed above.

Then you can validate the function on your teach pendant by touching In/Out > Register. Navigate to M050 - M055 and watch these values as you jog the robot arm.

If the high-speed-ethernet commands are still returning zero, then please use Wireshark to record the network traffic between your PC and the robot controller.

From my understanding High speed ethernet function has fixed request/response time and any faster than 10ms communication is not possible (I am really hoping that I am wrong).

It is not actually a fixed time. But, it is a relatively slow protocol. I expect a single command to read an M-Register would take between 5-10 ms. But you need to send 6 commands to read all the axes.

There are two ways to achieve 200Hz (or greater). Both of these would require paid options for the robot.

  1. Use the MotoPlus SDK. You could either modify MotoROS to speed up the feedback publisher. Or, you could create your own custom application specifically for reading torque.
  2. Using the M-Register method above, you could modify the robot's CIO Ladder Program to automatically move the Register data into External Outputs. (This is updated at a fixed 2 ms interval.) From there, you could utilize some fieldbus interface (such as Ethernet/IP or Profinet) to send the data to external device. I know there are open-source implementations of Ethernet/IP available on GitHub. You would have to integrate that with you application to read the data in real time.
nexus1203 commented 2 years ago

@ted-miller Thank you very much for the setup tutorial. I will test this the first this tomorrow.

There are two ways to achieve 200Hz (or greater). Both of these would require paid options for the robot.

I will contact my local dealer to get quotations on these. Again, thank you for your guidance.

gavanderhoorn commented 2 years ago

@nexus1203 (and @ted-miller also ;) ): could you please start a new discussion if, after testing, it still doesn't work for you?

Your questions are off-topic for this issue.

We should try to keep issues on-topic, and using HSES to access M-registers is rather different from what was being discussed in this issue originally.