-
Notifications
You must be signed in to change notification settings - Fork 7
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
Make position direct mode usable #198
Comments
Related? "[TechnosoftIpos] Cannot change control mode some/many times #170" |
Can't really tell unless we determine the underlying cause. Even though it's not stated explicitly in the title, #170 originated when trying to perform |
I've doing different tests, changing the mode in different occasions, and executing the position direct example (2ebc388) that I developed. The result has been this: Screen output: teo@teo-oliver:~$ yarp rpc /teo/rightArm/rpc:i
>>set pos 3 0
Response: [ok]
>>set pos 3 0
Response: [ok]
>>set icmd cmds (pos pos pos pos pos pos pos)
Response: [ok]
>>get icmd cmds
Response: [is] [cmds] ([pos] [pos] [pos] [pos] [pos] [pos] [pos]) [tsta] 10 1552306524.43345 [ok]
>>set pos 3 0
Response: [ok]
>>set pos 0 5
Response: [ok]
>>set pos 0 5
Response: [ok]
>>set pos 1 -5
Response: [ok]
>>set pos 1 -5
Response: [ok]
>>set pos 1 0
Response: [ok]
>>set pos 3 0
Response: [ok]
Steps in the execution:
|
See iPOS driver manual, 9.1.1. Internal States. |
Yes, that totally makes sense! |
Done at 6dae264, seems to work now. |
I'll upload the output of launchManipulation for this issue:
|
I'm really baffled that only CAN IDs 21 (left arm) and 27 (head) are responding in the CBW2 state thread, almost no other IDs are shown there (just a few PDO-related lines). Perhaps the acceptance filters are misbehaving? By the way, I noticed that the status word (6041h) transitions from 8637h to 9237h upon issuing a Edit: might be a coincidence (well, it is), but the following three calls issued by
I mean:
|
The same test done now at 12:10 with left wrist (ID26)
>>get encs
Response: [is] encs (20.003027) [tsta] 3 1553080045.375092 [ok]
>>get icmd cmds
Response: [is] [cmds] ([posd]) [tsta] 4 1553080049.250982 [ok]
>>set icmd cmod 0 pos
Response: [ok]
>>get icmd cmds
Response: [is] [cmds] ([pos]) [tsta] 5 1553080057.332439 [ok]
>>set pos 0 0 (not working)
Response: [ok]
>>set pos 0 0 (working)
Response: [ok] |
Control word test: |
Remarks:
|
Done at 2616482. The "10. Reset the set point." command has been replicated and now is sent prior to setting the target position in |
Still broken. Previous (successful) tests did not cover a full arm, rather a single joint. This time, on the second attempt (the first one is always successful, as noted above), we observe that the commanded joint rotates a bit in the beginning, then stops responding to new target points. |
Detected the change that produces the error in the example. |
Test summary:
|
Last test, prints current pending reads in CBW2: (updated to OneCanBusOneWrapper) |
We've learned that the This is highly undesirable and leads to undefined behavior, to say the least. I would rethink the whole buffer full/empty handling mechanism. See roboticslab-uc3m/teo-bimanipulation#3 (comment). |
Attaching the oldest code I've found: locomotion.zip (from private repo which has some interesting stuff) |
Regarding yarp-devices/libraries/YarpPlugins/TechnosoftIpos/ICanBusSharerImpl.cpp Lines 255 to 266 in a023145
and 9.2.3. Object 2072h: Interpolated position mode status:
Shouldn't the "pt buffer empty" check for value 80h, instead? That is, when bit 15 is set: "Buffer is empty – there is no point in the buffer." Edit: this actually might explain why we keep seeing "pt buffer empty" messages next to a "pt buffer full", which makes little sense (the buffer shouldn never deplete so fast). Also, I believe the PT examples in the iPOS manual are wrong regarding the buffer low warning, note that we never enable bit 7 in 2074h (section 9.2.5), the example neither. This would imply that: 1. perhaps buffer low's default is 00h; 2. the MSB in 2072h is almost always zero (does it make sense, shouldn't that produce constant buffer-empty messages?). |
This code set buffer-low signaling value to 4, buffer-empty was never checked against. There was an attempt to change the default buffer size (full) to 10792 (2A28h), which was ultimately commented out. Considering that the buffer-empty state might not be currently reached in normal PT-mode operation (see previous comment), we don't know whether intermitent depletion of the buffer could entail undesirable effects (@jgvictores). |
Test with |
Well, I can confirm the buffer is definitely not empty until the very end of the motion sequence. The buffer-low bit is set, too, in this case. After the last |
Even if the reference speed is updated while in external reference position mode, its new values are ignored by the drive upon trying to reach the next targets. Also, this mode is sometimes unresponsive upon pos->posd->pos->posd, not so often when running the examplePositionDirect app. Considering to switch back to PT mode for online trajectories, leaving PVT aside for offline stuff. |
Implemented and tested. I'm working on this along with #208 since this solution uses the I noticed that device routing in the ControlBoardWrapper layer is kinda messy regarding how IRemoteVariables methods are interpreting incoming/outgoing bottles, see source code. For instance, we'd need to build a two-element bottle if we ever want to send a remote variable to the head part, one element per CanBusControlboard instance. This won't affect both arms and legs since there is a one-to-one mapping (controlboardwrapper2-to-CanBusControlboard), but we are still forced to wrap any data sent in an additional bottle (e.g. In addition, this behavior introduces an assymetry in that said bottle would need to be constructed in a slightly different manner in case the CanBusControlboard device is not attached to a network wrapper. |
Recent tests showed that online trajectories performed in the new position direct mode, (re)implemented as PT interpolation, suddenly stop roughly at the 150th-200th commanded target. The drives had turned unresponsive after that point: no lines received in the CAN read stream ( Everything worked like a charm after I commented out the bits that set the PT buffer length to its maximum value as detailed in #198 (comment). This buffer has never been modified in that way before, even if we thought it was modified by us to store 15 points by the following lines: yarp-devices/libraries/YarpPlugins/TechnosoftIpos/IControlModeRawImpl.cpp Lines 211 to 215 in e26db48
This has been fixed in a recent revision of the iPOS CAN user manual: should have read object 2073h (Interpolated position buffer length) instead of 2074h (Interpolated position buffer configuration). |
Reminder: [debug] IRemoteVariablesImpl.cpp:50 setRemoteVariable(): ptModeMs
[debug] IRemoteVariablesRawImpl.cpp:37 setRemoteVariableRaw(): ptModeMs
[warning] IRemoteVariablesRawImpl.cpp:45 setRemoteVariableRaw(): Illegal state, unable to change variable when currently used.
[warning] IRemoteVariablesImpl.cpp:58 setRemoteVariable(): Unable to set remote variable on node 0. |
Could you upload the latest code? I don't see the changes we had to introduce in this method. There is a possibility the CAN network has nothing to do here, perhaps a state variable is not being correctly cleared on the iPOS-YARP device side, or some tweaks need to be made in your app. |
|
Please run your applicacion twice, make sure this error happens on the second run, and paste here the complete logs of launchManipulation. |
Here you have the output information of launchManipulation: And the output of my app: output.txt. |
Identified at #211 (comment). Suggested workaround: edit launchManipulation.ini, remove head joints. |
Just tried another implementation which does not need to care about synchronizing send rates on remote client and CAN master. Each TechnosoftIpos node starts a separate thread given a fixed period (50 ms). By the way, I'm giving another try to PVT submode, therefore the internal buffer (shrinked to hold a single target) must never deplete. ASBJ (as suggested by @jgvictores), the thread mimicks a LIFO stack in that only the last received target is forwarded to the buffer via CAN. In other words, high-frequency (<50 ms) trajectories get downsampled, whereas the last commanded target is placed in the buffer if no new point was received within 50 ms. Checklist:
|
Follow-ups to my previous post:
By now, PT is far better (i.e. produces a smooth movement) than PVT. Even more, smaller periods favor PT submode, whereas PVT is prone to induce oscillations.
OK with smaller rates, loses smoothness at higher values (as expected).
If the PT/PVT buffer is full, new points are always dismissed.
Can't reproduce.
Implemented, see first point: I like PT far more than PVT in this regard. |
Updated ID 9 (left leg) to F508M (latest one in F508 series) and checked supported drive operation modes thanks to 1aef421. Still no CSP mode available, although I had to check other manufacturer's description pertaining to this CAN object since the iPOS user manual does not cover CSP at all in 6502h.
|
Change mode, set first PT point, don't send start signal:
Once (iff) start signal is sent, the buffer is being processed by the linear interpolator:
Shortly thereafter:
Conclusion: buffer empty state is triggered when there are no more points in the buffer AND the last point has been already processed. If the buffer low signal is configured for buffer size zero, it is sent to the CAN master a bit earlier, that is, when that last point is being consumed (thus producing motion). |
The position direct control mode has been rewritten (again), this time it should be final. See #220 for implementation details. To sum up, we tried four different setups: old PT mode
external reference position mode
PT mode with synchronized client-master period
PT/PVT modes with downsamplingLearn more at #220. |
Regarding CSP mode... Just discovered object 208Eh, Auxiliary Settings Register:
Default is 1, but this object does not exist in earlier versions of the iPOS user manual. |
Current (undesired/faulty) behavior:
IPositionControl -> IPositionDirect -> IPositionControl
and try topositionMove()
: need to send this command twice, no response at first try.IPositionControl -> IPositionDirect -> IPositionControl -> IPositionDirect
.ptPointCounter
is never set to zero: ref.The text was updated successfully, but these errors were encountered: