Realtime Interface (Joint and Cartesian) #449
There are no files selected for viewing
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,150 @@ | ||
| <!-- | ||
| SPDX-FileCopyrightText: 2025, Yaskawa America, Inc. | ||
| SPDX-FileCopyrightText: 2025, Delft University of Technology | ||
|
|
||
| SPDX-License-Identifier: CC-BY-SA-4.0 | ||
| --> | ||
|
|
||
| # R/T Motion Control | ||
|
|
||
| The real-time motion control server is intended to be used in a closed loop system. | ||
| It allows the user to command incremental offsets at the rate of the robot controller's interpolation clock. | ||
|
Collaborator
There was a problem hiding this comment. Not a native speaker, but wouldn't offsets always be incremental? Or does this emphasise the fact it's relative to the previous state/increment?
Collaborator
Author
There was a problem hiding this comment. Yes, my intent was to emphasize that it's relative to the previous state |
||
| This control mode minimizes overhead as much as possible by routing the user commands directly to the MotoPlus motion API, mpExRcsIncrementMove. | ||
|
|
||
| ## Activation | ||
|
|
||
| This control mode is activated using the [start_rt_mode](ros_api.md#start_rt_mode) service. | ||
| The user must specify the `control_mode` to indicate whether the increments will be joint offsets (radians) or cartesian TCP offsets (meters / radians). | ||
|
|
||
| If this service is successful, it will return a `result_code` of `Ready (1)`. | ||
| Otherwise, please examine the `result_code` and `message` files in the response for more information. | ||
|
|
||
| The service will also return a `period` in milliseconds. | ||
| This indicates the rate at which increment commands will be expected by the robot. | ||
|
Collaborator
There was a problem hiding this comment. Are expected or must be provided? I'd expect the latter.
Collaborator
Author
There was a problem hiding this comment. It's really an expectation. You will get really bad performance if you don't maintain that rate. But there is no hard requirement to do it. |
||
| The default period for a single manipulator is 4 milliseconds. | ||
| However, that value will increase as additional axes or manipulators are added to the system. | ||
|
|
||
| ## Usage | ||
|
|
||
| ### Command Flow | ||
|
|
||
| Once activated, a UDP server will listen on port `22000` (default). | ||
| The user then sends the first increment with a the `sequenceId` field set to `0`. | ||
| After that, the user must wait until the robot replies before sending the next increment. | ||
| Each subsequent command must increment the `sequenceId`. Additionally, each subsequent command must not be sent until the robot replies to the previous command. | ||
|
Collaborator
There was a problem hiding this comment. Saying that "each subsequent command must not be sent until the robot replies to the previous command" seems incompatible with the I think that we should consider removing
Collaborator
Author
There was a problem hiding this comment. I disagree. But I was struggling to articulate why. So here's Gemini :)
Collaborator
There was a problem hiding this comment. I believe I would at least partly agree with @jimmy-mcelwain -- and I disagree with Gemini. Real-time command interfaces are supposed to be deterministic; allowing out-of-order packets, or dropped packets breaks that determinism, or at least weakens it. re: "sensitive to network conditions": we're essentially using UDP here as a stand-in for a deterministic network protocol, but without changing that transport layer or any of the network layers (fi ethernet), which is non-deterministic itself by default (collision handling fi). To make it work well enough, we can reduce the chances of non-deterministic behaviour occurring by essentially making a two-host network segment where both take turns to use the medium. That means: direct cable, no switches, and only RT traffic on the connection. Allowing for lost datagrams improves (developer) UX, but if too many datagrams go missing, control performance suffers. I would argue users interested in using a real-time external motion interface care about control performance and are likely willing go through the hassle of setting up sufficiently performant network infrastructure.
it's never made explicit, but I believe this assumes a shared connection. Otherwise there is very little chance of packet loss -- unless there is some network failure besides the RT code.
Collaborator
Author
There was a problem hiding this comment.
I agree with you in principle. But I don't think this is realistic in a true industrial environment. I have seen a lot of "inadvisable" (stupid) configurations. I'm fine with lowering |
||
| This will occur at the rate of the `period` from the [start_rt_mode](ros_api.md#start_rt_mode) service. | ||
|
|
||
| If a command is not received with 30 seconds (default), then the session times out and is dropped. | ||
ted-miller marked this conversation as resolved.
Show resolved
Hide resolved
|
||
| At that point, the server must be reactivated by calling `stop_traj_mode` and `start_rt_mode`. | ||
| A "keep-alive" can be used by sending a command with zero increments. | ||
|
|
||
| Additionally, if the client does not receive a reply packet within this amount of time, then it should be assumed that the session is dead. | ||
|
|
||
| <img src="img/RtFlow.png" alt="Command Flow" /> | ||
|
|
||
| ### Data format (command) | ||
|
Collaborator
There was a problem hiding this comment. Commenting here, as it's about both the command as well as the reply. This is not an exhaustive list, but I would recommend to add:
Optional, but recommended:
For the state packet specifically:
For the command packet:
For both (motion related packets) again:
My main motivation for these suggestions would be to make it possible to interpret data in command and state packets completely independently of the state of MotoROS2, another server and/or a client implementation. That makes both implementing MotoROS2, that other (hypothetical) server and clients easier to implement, as well as things like protocol dissectors and (de)serialisation libraries.
Collaborator
Author
There was a problem hiding this comment. @gavanderhoorn What do you think about this? struct RtPacket
{
UINT16 version;
UINT16 packet_type; //see enum xxxx
UINT32 sequenceId;
double delta[MAX_GROUPS][MAX_AXES]; //[8][8]
int toolIndex[MAX_GROUPS]; //[8]
char reserved[32]; //for future expansion
}struct RtReply
{
UINT32 sequenceEcho;
double feedbackPositionJoints[MAX_GROUPS][MAX_JOINTS]; //[8][8]
double feedbackPositionCartesian[MAX_GROUPS][MAX_JOINTS]; //[8][8]
double previousCommandPositionJoints[MAX_GROUPS][MAX_AXES]; //[8][8]
double previousCommandPositionCartesian[MAX_GROUPS][MAX_AXES]; //[8][8]
RobotStatus status; //literal clone of the /robot_status structure
bool fsuInterferenceDetected;
}
Personally, I don't see why that's necessary. Since this is meant to be used in real time, why do I care about the stamp? All I really care about is the sequence identifier. I'm expecting that the user has done any time parameterization.
I've already got the
These could be part of the |
||
|
|
||
| The command packet is a *packed* `RtPacket` structure. | ||
|
|
||
| ```c | ||
| //########################################################################## | ||
| // !All data is little-endian! | ||
| //########################################################################## | ||
| struct RtPacket | ||
|
Collaborator
There was a problem hiding this comment. should this perhaps also include a bitmask/integer array indicating to which groups increments are being commanded? That would make it an even thinner wrapper around
Collaborator
Author
There was a problem hiding this comment. I don't want to give the impression that a user can send one packet to control R1 and another packet to control R2. The current implementation makes it very clear that the whole system must be controlled by a single command packet. |
||
| { | ||
| int version; | ||
|
|
||
| UINT32 sequenceId; | ||
ted-miller marked this conversation as resolved.
Show resolved
Hide resolved
|
||
| double delta[MAX_GROUPS][MAX_AXES]; //[8][8] | ||
| int toolIndex[MAX_GROUPS]; //[8] | ||
| } | ||
| ``` | ||
|
|
||
| The `version` must match the version number expected by the server. | ||
| If it does not match the expected value, the packet will be rejected and the connection will be dropped. | ||
| The current version is `1`. | ||
|
|
||
| This contains a sequence ID, the increments for each control group, and the tool number to use for each control group. | ||
|
|
||
| #### Joints | ||
|
|
||
| When the `control_mode` is `JOINT_ANGLES (1)`, the order of the joints in the `delta` array must be in the order of `S L U R B T E 8`. | ||
| Please note that for seven axis robots, the `E` joint is phyically mounted in the middle of the arm. | ||
| But it must be sent at the end of the joint array. | ||
|
|
||
| See `JointIndices` enum. | ||
|
|
||
| ```c | ||
| enum JointIndices | ||
| { | ||
| Joint_S = 0, //radians | ||
| Joint_L, | ||
| Joint_U, | ||
| Joint_R, | ||
| Joint_B, | ||
| Joint_T, | ||
| Joint_E, | ||
| Joint_8, | ||
|
|
||
| MAX_JOINTS | ||
| } | ||
| ``` | ||
|
|
||
| #### Cartesian | ||
|
|
||
| When the `control_mode` is `CARTESIAN (2)`, the order of the joints in the `delta` array must be in order of `X Y Z Rx Ry Rz Re 8`. | ||
|
|
||
| See `CartesianIndices` enum. | ||
|
|
||
| ```c | ||
| enum CartesianIndices | ||
| { | ||
| TCP_X = 0, //meters | ||
| TCP_Y, | ||
| TCP_Z, | ||
|
|
||
| TCP_Rx, //radians | ||
| TCP_Ry, | ||
| TCP_Rz, | ||
| TCP_Re, | ||
|
|
||
| TCP_8, //pulse | ||
|
|
||
| MAX_AXES | ||
| } | ||
| ``` | ||
|
|
||
| Please note that rotations are applied in the order of `Z Y X`. | ||
|
Collaborator
There was a problem hiding this comment. Just thinking out loud: what about supporting quaternion input? Would remove the burden of figuring out the correct rotation order from clients.
Collaborator
Author
There was a problem hiding this comment. My brain doesn't like quaternions. My first instinct is, "no". But... quats would be consistent with our
Collaborator
Author
There was a problem hiding this comment. Ehhh... I really prefer yaw, pitch, roll. Since the idea is to be a minimal wrapper around the motion API, I think the data should be as close as possible to the 'final form'. @jimmy-mcelwain Do you have an opinion?
Collaborator
Author
There was a problem hiding this comment. I just remembered that jimmy is out all week.
Collaborator
There was a problem hiding this comment.
I think that I would agree with this, but does the statement:
Make sense in this context? If we are talking about pose, yes it is important that we are saying that the rotations are applied in the order of
Collaborator
Author
There was a problem hiding this comment. So... it turns out that position is reported by the robot using So a quaternion may make this easier on the user. What do you guys think? We can hide the rotational order in the backend. Additionally, I found that rotations get complicated when commanding such small rapid increments.
So I think that becomes hard for a user to calculate rotations. |
||
|
|
||
| ### Data format (reply) | ||
|
|
||
| The command packet is a *packed* `RtReply` structure. | ||
| This will echo the sequence ID, provide feedback position, and provide commanded position. | ||
|
|
||
| Additionally, there is a flag to indicate if the Functional Safety Unit (FSU) reduced the speed of the **previous** command cycle. | ||
| This indicates that the robot did not complete the full increment as commanded. | ||
|
|
||
| ```c | ||
| //########################################################################## | ||
| // !All data is little-endian! | ||
| //########################################################################## | ||
| struct RtReply | ||
| { | ||
| UINT32 sequenceEcho; | ||
|
|
||
| double feedbackPositionJoints[MAX_GROUPS][MAX_JOINTS]; //[8][8] | ||
| double feedbackPositionCartesian[MAX_GROUPS][MAX_JOINTS]; //[8][8] | ||
|
|
||
| double previousCommandPositionJoints[MAX_GROUPS][MAX_AXES]; //[8][8] | ||
| double previousCommandPositionCartesian[MAX_GROUPS][MAX_AXES]; //[8][8] | ||
|
|
||
| bool fsuInterferenceDetected; | ||
| } | ||
| ``` | ||
|
|
||
| ## Deactivation | ||
|
|
||
| Other motion modes may not be used at the same time as the real-time motion control server. | ||
ted-miller marked this conversation as resolved.
Show resolved
Hide resolved
|
||
| The service to start those modes will fail when invoked. | ||
| By calling `stop_traj_mode`, the R/T server will be disposed. | ||
| At that time, another motion mode may be used. | ||
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
If/when we merge the related PR, this should be replaced by a permalink.