Add support for FS100

[gavanderhoorn]

tasks:

• [x] add support for FS100 controllers to build system for M+ libmicroros
• [ ] add endianness config for Micro-CDR (here) and Micro-XRCE-DDS (here)
  • ran into an issue: micro-ROS/micro-ROS-Agent#197
  • and another/follow-up one: https://github.com/micro-ROS/micro-ROS-Agent/issues/197#issuecomment-1692789200
  • work around / hack in place: https://github.com/Yaskawa-Global/motoros2/issues/19#issuecomment-1930072900
• [x] add FS100 to VS project configurations and related support files (.mps)
  • Humble only (for now)
• [x] add FS100 version of MotoROS_PlatformLib binary
• [x] add FS100 version of ParameterExtraction binary
• [x] allow building MotoROS2 for FS100
• [x] resolve missing mpCtrlGrpNo2GrpId(..) (see https://github.com/Yaskawa-Global/motoros2/pull/49/commits/06e7eb2fffdb0a8962ee461c4f4c5cec5b8da412 and https://github.com/Yaskawa-Global/motoros2/pull/49/commits/032bcb43219e370cd0211a280899d3156ec33a99)
• [x] need to read config file from USB instead of SRAM (https://github.com/Yaskawa-Global/motoros2/pull/49#issuecomment-1584786335)
  • turns out FS100 does support SRAM, so just reuse YRC1 code for this
• [x] work-around/remove use of mpGetEncoderTemp (not available on FS100: https://github.com/Yaskawa-Global/motoros2/pull/49#discussion_r1225131674)
• [x] need to read robot calibration data (https://github.com/Yaskawa-Global/motoros2/pull/49/commits/e28fc7b57ca615c1ece58a76a9381ee4fb47b6c9, https://github.com/Yaskawa-Global/motoros2/pull/49/commits/4f31af5782491e5d1bcce40481eb1ec8e251a20a)
• [x] don't report application info (see MotoROS1: here
• [x] FS100 supports max 4 groups (here fi)
• [x] no (direct) support for retrieving system date-time (mpGetCalendar(..))
• [ ] update documentation
  • [ ] 'supported' badge
  • [ ] installation instructions
  • [ ] troubleshooting
  • [ ] faq

[gavanderhoorn]

Added this to the Untargeted milestone, as this is low-priority for now.

[gavanderhoorn]

As an update: I've started working on this.

YEU provided me with an FS100 -- thank you @yeu-buchholz and colleagues :+1: :100: :robot: :beers:

No guarantees -- the FS100 is an old platform and in some ways more limited than the YRC1 and DX2. I've got the binary starting up successfully, but am running into issues which seem to point to problems with the RMW and/or memory management.

I'll open a (draft) PR if/when it makes sense.

[gavanderhoorn]

@destogl FYI also (we discussed this at the RIC23).

[gavanderhoorn]

Update: seems there are some more endianness issues to resolve: https://github.com/micro-ROS/micro-ROS-Agent/issues/197#issuecomment-1692789200.

[gavanderhoorn]

Hi @saahu27.

I want to understand, contribute and make use of fs100 controller using ros2.

great. We're always looking for more community participation.

Can we please get in touch so that i can explain more about my project and work and discuss how can i contribute.

unless you feel what you're doing is (highly) sensitive, I'd suggest starting a discussion on the Discussion forum.

At the moment I'm debugging some endianess issues, as you can read in my previous https://github.com/Yaskawa-Global/motoros2/issues/19#issuecomment-1693305876.

I expect basic MotoROS2 to work on FS100 once those get fixed (ie: publications and IO services, no motion yet). But no guarantees.

[saahu27]

I have learnt a lot on how to use ros from your previous contributions for universal robots and i feel very happy to have interacted with you.Thank you for your response. I was not able to express myself clearly earlier. i wanted to request to drop your email so i can get in touch with you.

Nothing that i do is classified i wanted some help on how to go about controlling the motoman 100fs using ROS 2 and Move it2. So, i am currently working on porting motoman ros1 drivers using ros1 to ros2 bridge. i came across the new ros2 drivers you were contributing. Been trying your work on ROS1 Drivers and just wanted to ask some pointers on how to understand your new drivers, what kind of control can we expect a point to point control/Cartesian? Trajectory control? with what baud rate so i can decide ros2 controller update frequency etc which I will open a discussion in the page you mentioned.

[gavanderhoorn]

i wanted to request to drop your email so i can get in touch with you.

nothing personal, but I try to avoid discussing ROS topics in private emails if at all possible.

Most of my work is open-source, and I'd like to be able to reuse diagnostic sessions and discussions as much as possible as well.

I will open a discussion in the page you mentioned.

great, looking forward to it.

[gavanderhoorn]

Coming back to this (after a long time): I believe I ~~solved~~ hacked around the last endianness issue.

In addition to topics, services now also seem to work, but I've yet to test something more complex than example_interfaces/srv/add_two_ints.

If it works, it would be the last major hurdle for a bare-bones FS100 version of MotoROS2, although we'd most likely not be able to use the FollowJointTrajectoryAction server due to memory constraints.

---

Edit: the approach only works with a little-endian Agent. As most ROS 2 users would be using amd64 systems, I believe that's an acceptable limitation.

If we'd want/need more flexibility -- like runtime detection and configuration -- we'd probably need a more fundamental solution.

---

Edit 2: Just tested write_single_io and read_single_io, they both work.

---

Edit 3: diff showing the changes that appear to be working: https://github.com/micro-ROS/rmw_microxrcedds/compare/humble...gavanderhoorn:rmw-microxrcedds:le_temp_test.

(reminder: rebase this on-top of https://github.com/micro-ROS/rmw_microxrcedds/compare/3.0.0...3.0.2)

[gavanderhoorn]

I've got quite some parts of MotoROS2 disabled for this FS100 test and it shows: /joint_states reaches about 180 Hz. Probably related to what is discussed in #9.

[gavanderhoorn]

Turns out the FS100 does support SRAM (at least according to 162002-1CD and my own tests), so I'm going to update these lines:

https://github.com/Yaskawa-Global/motoros2/blob/10d8f00317c5d4ef7960627962337bf72e2ba817/src/ConfigFile.c#L788-L791

and some others to no longer treat the FS100 like the DX200.

[gavanderhoorn]

Current state: https://github.com/Yaskawa-Global/motoros2/compare/main...gavanderhoorn:motoros2:fs100_support.

Works like the YRC1 version, including updating config file from USB and storing it in SRAM.

Number of patches needed is actually quite limited. About half of them add bits and pieces to the build scripts and project configuration for FS100 support. And the other half #ifdefs a bunch of stuff to set FS100 specific values.

Main difference: FollowJointTrajectoryAction server is not enabled currently.

I'm also seeing the JointState topic publishing at approx 180 Hz, even though controller_status_monitor_period is set to 10 (which should result in 100 Hz IIUC).

[gavanderhoorn]

Test build: <outdated_binary>.

NOTE: this is not production ready and for testing purposes only.

[ted-miller]

we'd most likely not be able to use the FollowJointTrajectoryAction server due to memory constraints.

Can you elaborate on this? (Perhaps you already discussed in another thread, but I can't remember.)

I know the M+ docs explicitly state there are limits to static memory. But we've blatantly ignored that for DX2 and YRC1, and everything works. Have you experienced issues on the FS?

[gavanderhoorn]

I seem to remember to have tried keeping the static buffer at its 'normal' size and seeing loading errors. But that was a long time ago when there were multiple other issues.

I just re-enabled 'everything' and the node still comes up. I can start the point queue mode, PP shows servo power is on (virtual, as I have no real hw) and the service returns success. Haven't tried moving the virtual robot yet.

Need to try with the buffer at normal capacity and see if that still loads.

Controller does have enough memory free btw (about 16 MB if I interpret the stats correctly), just not in the M+ partition.

[ted-miller]

I've got a guy who should be able to do some testing the week after next. He's completely new to all this, so it'll take a bit to get up to speed. But I'm hoping to have him start helping with the issues-list. (I know I've said that like 4 or 5 times now. So, who knows if it'll actually happen.)

[gavanderhoorn]

@ted-miller: did you / your colleague get a chance to test the alpha I posted?

[ted-miller]

did you / your colleague get a chance to test the alpha I posted?

No.

I know I've said that like 4 or 5 times now. So, who knows if it'll actually happen

... yeeeaaahhhh....

[ted-miller]

@gavanderhoorn I've got power on my FS100. Can you provide a binary (or the supporting libraries) that has 'everything' re-enabled and also has DUMMY_SERVO_MODE defined?

[gavanderhoorn]

I'll have to build that specifically, as my FS100 here only has a virtual robot 'connected'.

Probably tomorrow.

[ted-miller]

my FS100 here only has a virtual robot 'connected'

So does that mean you already have DUMMY SERVO MODE defined in the alpha-build above? I also don't have a physical robot arm on my FS.

[gavanderhoorn]

No, there just isn't any manipulator connected. I can't check right now, but I guess it's configured for virtual hw (rotary).

The build in https://github.com/Yaskawa-Global/motoros2/issues/19#issuecomment-1934143832 does not have DUMMY_SERVO_MODE enabled.

It also doesn't have 'everything enabled'. I did that after I posted that build.

[gavanderhoorn]

@ted-miller: <outdated_binary>.

I'm not near my FS100 right now, so can't test, but I believe this should at least start and let you interact with it.

Things missing are: FJT server, start_traj_mode and select_motion_tool.

---

Edit: and a version with the FJT and start_traj_mode (re)enabled. No select_motion_tool though: <outdated_binary>.

This also has the static buffer at 2000000 bytes, so not sure that will load.

---

Edit 2: and a version of -alpha41 which uses heap memory for the FJT buffer instead of a static buffer. Just as a test. I don't expect this to work, but you never know.

<outdated_binary>

[ted-miller]

I'm getting immediate deserialization errors. But I think my agent might be old. I'll update in the morning.

[gavanderhoorn]

Hm. How old is your Agent? I use microros/micro-ros-agent:humble from 7 months ago.

Just tested alpha40 and alpha41 (the one with the full 2MB static buffer ..). They both load and I can call services and ros2 topic echo /joint_states.

What sort of PC are you using to run the Agent? I'm making some assumptions about endianness, word size, etc which may not be true on your side.

---

alpha42 loads, starts up but then fails to complete init as heap allocation fails (somewhat expected). Ignore that one.

---

Edit: might be that alpha41 corrupts the stack with the large static buffer. It loads, no errors/warnings anywhere, but I can't call /start_traj_mode. It just hangs.

---

Edit 2: ok, so alpha41 causes a 1020[6] when installed on the controller in the normal way.

Only alpha40 loads and starts normally, but doesn't have the action server enabled. It also probably doesn't init the static buffer properly, which might mean point queueing would not work either, but I haven't tested it and it should be fixable.

---

Edit 3: reducing the static buffer to 1MB makes alpha41 load. No more 1020[6].

Still can't call /start_traj_mode, but /start_point_queue_mode (still) works.

[gavanderhoorn]

@ted-miller: please try this build: <outdated_binary>.

Has a reduced static buffer (1MB), but everything is enabled (except select_motion_tool).

Loads successfully on my FS100 here. No 1020[6].

I can also call /start_traj_mode.

Have not tested executing any trajectories yet.

@gavanderhoorn wrote:

Still can't call /start_traj_mode, but /start_point_queue_mode (still) works.

I forgot to (re)add /start_traj_mode to the executor .. 🤦

[ted-miller]

Turns out my agent was 18 months old. It worked fine after an update.

For the most part, I think you've got it working well. There's a few issues though.

1. As mentioned before, installing the .out file in maintenance mode is really slow. I had alpha40 loaded from yesterday. The first time that I tried to load alpha44, I got this: I'm assuming this is a timeout due to the excessive time it takes to install the binary. So binary size might be an issue.

I was able to reboot and try again. It worked the second time.

2. For joint_states, I'm seeing this:

effort:
- 5.17293035e-316
- 2.124100552e-314
- 7.70200109095887e-310
- 1.69759663277e-313
- 5.172925e-316
- 1.3051418519317825e-278

I verified we are zeroing out the array before calling the API. So perhaps the API doesn't like dummy mode.

3. joint_states doesn't update when I move the robot. This is possibly due to dummy mode. The only FS100 that I have with a physical arm is a pair of deltas (MPP3H). I might be able to use those for testing next week. But this system has another M+ app installed and I don't know what kind of resources it consumes. I'm not sure if I can safely remove this app and get a copy to reload later.

4. tf doesn't update when I move the robot either. Same comment as 3.

I didn't have issues with any of the other services.

[gavanderhoorn]

Turns out my agent was 18 months old. It worked fine after an update.

:+1:

1. As mentioned before, installing the .out file in maintenance mode is really slow. [..] I'm assuming this is a timeout due to the excessive time it takes to install the binary. So binary size might be an issue.

The flash on the FS100 is just really slow. Loading/running directly from USB (fi) is much, much faster.

(I'm not claiming 'this is fine', just identifying the most likely cause here)

There aren't many options for reducing the binary size I haven't considered I believe, but I do have one or two more ideas. It wouldn't be too pretty though.

I was able to reboot and try again. It worked the second time.

2. For joint_states, I'm seeing this:

effort:
- 5.17293035e-316
- 2.124100552e-314
- 7.70200109095887e-310
- 1.69759663277e-313
- 5.172925e-316
- 1.3051418519317825e-278

I verified we are zeroing out the array before calling the API. So perhaps the API doesn't like dummy mode.

O, right. DUMMY_SERVO_MODE.

That might not actually be enabled in this build ..

re: effort values: those are some really small values. I've seen it as well here, and I believe I've even seen it with the YRC1 build.

I'll check again whether there is something not right with the (de)serialisation. I am doing some not-exactly-as-they-should-be-done things.

3. joint_states doesn't update when I move the robot. This is possibly due to dummy mode.

hm. Not sure what that would be.

As I wrote above: this isn't dummy servo mode.

The only FS100 that I have with a physical arm is a pair of deltas (MPP3H). I might be able to use those for testing next week. But this system has another M+ app installed and I don't know what kind of resources it consumes. I'm not sure if I can safely remove this app and get a copy to reload later.

Couldn't you put the FS100 you're testing with now in 'virtual hw' mode?

MotoROS2 would not know the difference in that case. Mine is configured like that, and it all just behaves as-if there is an actual manipulator connected.

4. tf doesn't update when I move the robot either. Same comment as 3.

Might be the same cause.

Could you perhaps make a wireshark capture?

[ted-miller]

As I wrote above: this isn't dummy servo mode Couldn't you put the FS100 you're testing with now in 'virtual hw' mode?

I'm not sure we're on the same page.

My current FS100 is in 'virtual' (dummy) mode. I don't have an arm attached.

So, when MR2 attempts to read the feedback position, it will always return 0. I'm saying that I need to test again on a physical arm to see if that's indeed the problem.

The DUMMY_SERVO_MODE flag in MR2 tells it to read command-position instead of feedback-position.

[gavanderhoorn]

Ah, right. I conflated a couple of things.

Ok: <outdated_binary>.

DUMMY_SERVO_MODE=1 now and I had this still disabled:

https://github.com/Yaskawa-Global/motoros2/blob/5f8c80011c64dc9152a4c72b2eb138760a9d649e/src/MotionControl.c#L1053

I suspect/hope this now makes things 'move' again.

[ted-miller]

/tf is static:

wireshark.zip

EDIT: To clarify, this is with alpha44