One path I'm exploring with the K12 arm and its K11R controller is whether I can make the robot operate with ROS.
I'm completely new to the ROS world of Robotics. This puts me at least 8 years behind the learning curve. And as such, I might not explain things really accurately here.
ROS
The initial idea is that ROS is a bunch of middleware software that provides a common way for things to talk to each other.
There are "nodes", which conceptually could be things that have information. In our world, a joint could be a node. A set of configuration information could also be a node.
Then, you have publish-subscribe mechanisms. As an example, the robot arm could have joint location information. You could have something that publishes the arm's joint location on a regular basis. Then, you could have a visualizer that reads the joint information and renders it graphically.
There are also ways in which you can send actions, and so on.
This all came about with ROS way back in history. Since 2018, ROS has moved on to ROS2, and the underlying APIs for coding nodes and actions, and the ways in which things are launched, has changed.
ST-Robotics has published a git repo at https://github.com/st-robotics that provides software for ROS, but it's for ROS v1 and hasn't been updated to V2 yet. In fact, the last commit I could find was from September, 2018, and there hasn't been anything there since.
Nonetheless, the repository provides some insight into their intent.
ROS environment
In order to run ROS, I installed Ubuntu 24.04.4_LTS. I could grab the ros package as part of Ubuntu, but found that the ST-Robotics code was built based on the ROS melodic version.
To keep things orderly, I set up a Docker image, and then set up ROS melodic within that.
The next thing I ran into is that ROS provides a user interface in MoveIt and Rviz, but you have to do some extra steps in order for the outer system's X display to be accessible from the inner docker instance.
Then, I ran into issues with Python. Part of the implementation in the ST-Robotics code is using a version of python and its dependencies that are quite outdated. These days, too, it's recommended to run a virtual environment in python so as to avoid perturbing the global python libraries that everyone shares. This is done for proper security reasons.
Overall, I have gotten to the point where I have ROS set up, but I don't really know how to launch the right pieces in order for a configured arm to appear in Moveit and Rviz.
However, I found some useful bits in the python shell and in the r12_hardware_interface code.
The python shell
Within the git repo, there is a section call r12_python_shell.
This originated as a shell built by Adam Heins.
The way it works is that you can run the r12_python_shell/r12/r12-shell script as an executable within Linux. This depends on its shell.py, and arm.py.
shell.py is a subclass of the cmd.Cmd object, which basically provides you with terminal emulator behavior. Text comes in to its reader, and the words get compared against known values in self.commands. The parent object (cmd.Cmd) looks at strings that match commands, and redirects to functions of the form do_name() as they're found.
If a match for a string is not found in the list of do_name methods of the object, the default() method is called.
In this way, shell.py defines its own special set of commands (e.g., "connect"), and passes through other commands directly to the controller. (BTW, from a naming standpoint, I think "arm.py" could be "controller.py" so as to distinguish between a K11R controller, and the device it's controlling.)
Before it can do any work with the controller, though, the system has to connect via the USB-to-Serial interface. This is handled in arm.py in its search_for_port() method. It scans the known USB devices for one that is from vendor 0x0403 and has the product ID 0x6001.
In Linux, you can see this kind of information by using the lsusb command and other similar commands. However, you may need to be provided access in order for that to work. First, the outer shell has to start docker in a particular way in order for the docker instance to see the usb devices. For me, they're found at /dev/ttyusb0. Second, even if the docker instance can see the usb device, the user that you're logged in as, when in the docker instance, has to belong to the right permission group.
Once set up, a typical sequence that I run is this:
1. Start a terminal window in Linux
2. Start a docker instance, allowing the created docker instance access to the outer X display, and access to the usb devices.
$ cd
$ cd r12_python_shell
$ cd r12
$ ./r12-shell
This provides me with the cmd.Cmd prompt and awaits instructions. The first thing to do is connect to the controller
> connect
And then, typically, I'll start ROBOFORTH, get the system fired up, and get the arm calibrated.
> ROBOFORTH
> START
> CALIBRATE
Modifications to the shell
I haven't diffed the original Adam Heins code against the version in the r12_python_shell area. But I did try various operations and ran into strange delays at points.
One of the main problems was in getting the Teach pad to work. After calibrating, I would run
> TEACH
and then it would sit there for a long time, and come back with an odd "Speed? <n>" message.
If I would run
> TEACH
again, then it would turn the lights on on the Teach pad, and I would be able to move the arm in joint mode.
The cause of the delay is that the basic python shell sets up a 30 second serial read timeout. This is defined in arm.py. It's there because they wanted to allow the arm enough time to run a calibration sequence without timing out.
The reason this gets in the way of the TEACH operation is that TEACH actually immediately responds with a request for a motor movement speed setting. Normally, if you run this, the controller will respond as follows:
> TEACH
Speed? <16>
and then it waits for more input. But the response isn't of a form that the shell was expecting, so it sits there waiting for more. I think the second TEACH command I was sending was not being interpreted as an actual command, but as a response to the "Speed?" question.
The way I've gotten around the delay is to make the timeout amount much smaller (2 seconds), generally. If the command is CALIBRATE, it adjusts the timeout (for that call only) to the longer 30 seconds.
The next problem is that normally you should be able to run TEACH, get the Speed? <value> response, and just hit Enter to accept the default value. This is the reason that some of the documentation says "Type TEACH and hit Enter twice". What they're really saying is "Enter the TEACH command, and then accept the default speed setting."
The problem in the Python shell is that the default cmd.Cmd code treats an empty line as "repeat the last operation". Not wanting that, they overrode the command in shell.py's emptyline() method to just "pass", meaning it will not do anything.
Instead, we want an empty line to pass the empty line down to the controller. I changed the emptyline() method to just call self.default("").
That leads to a third problem. In its current form, if the default() method is given an empty string, it will not pass it along to the controller. There are a few lines at the start that check for all uppercase in the string. If it fails the check, it returns immediately. It is a faulty assumption that everything going to the controller is uppercase. In the case of TEACH, the response to the command is a numeric speed, or an empty line. So, I've disabled the uppercase "Unrecognized command" check.
With that in place, it now works to enter Teach mode (equivalent of the T icon in Robwin), move the arm around in Joint mode, and hit the check mark on the Teach pad (or in British English, the "tick") to exit teach mode. Then, I can run WHERE, or COMPUTE and POINT name, to capture the joint positions.
An interesting side-effect of this change is that lowercase commands are flowing through to the controller now. But, they get uppercased somewhere in the path, so they still work. "energise" is treated as "ENERGISE", and so on. The only thing that doesn't make it through to default() is a command that matches the special python shell set of commands, like "connect" or "exit". I've also found, interestingly, that uppercase "EXIT" behaves differently than lowercase "exit".
Cartesian mode with the Teach pad
I have not yet figured out how to get Cartesian movements of the Teach pad to work in the python shell. They always end up giving me an error. I think I have to observe what's going on with Robwin so I can know what has to be entered in the python shell.
Python shell vs. r12_hardware_interface
With just enough knowledge of the python shell's shell.py and arm.py code, it became clear that the same code is being used (with slight changes, probably) in the r12_hardware_interface arm.py code.
The r12_hardware_interface is a "ROS package providing a FollowJointTrajectoryAction interface to the ST Robotics r12 arm. Requires r12_moveit_config and my_r12_description packages. Based on Adam Heins' python shell for the r12."
It has two ROS things going on.
One is the joint_trajectory_action_server.py. I don't fully understand it yet, but I'm starting to get it.
The _execute_cb() is a callback that is provided as a parameter to the SimpleActionServer constructor at startup. Presumably, someone calls this to say "you, arm, go here!". Within the _execute_cb(), you can see that it sends FORTH commands like "FIND TRAJECTORY" to find the route named TRAJECTORY. It forgets it if it already existed (releases the variable and reserved memory). Then, it creates it anew by saying "ROUTE TRAJECTORY" (create a route of the name TRAJECTORY), and follows that up with "500 RESERVE" (allocates space for 500 movement positions in the route).
Then, it converts the provided set of coordinates to joint positions, sends them to the controller using a special $JL command, and moves the arm along the route by issuing a special $RUN command.
After the route execution has begun, it loops and calls _update_feedback(). That in turn calls arm.read_pos(). That calls another special ROBOFORTH command, $, which gets the 5 joint positions back as an array. (The serial response buffer may also have 'SPEED = nnnn' in the way, generated by the $RUN command, so the code has to ignore that on every arm.read_pos() call.)
NB: I think the code that parses the joint positions could be improved with a regular expression parser.
The action server code continues to loop until the arm position indicates that it's close enough to the intended movement goal.
Separately, the arm position is also being published via the joint_state_remapper.py(). I think this allows some other ROS node to subscribe to the arm position and update its understanding of the arm's position. I think this allows other subscribers (e.g., a UI visualizer like RViz) to see what the arm is doing.
$JL, $RUN, and $ commands
One big roadblock I face is that my controller does not understand the $, $JL, or $RUN commands. Without these, the current ROS code won't work, and I can't consider upgrading the ROS v1 code to be based on ROS2. It's like a fundamental dependency isn't being met.
The $JL command lets this external system (MoveIt, or any other caller from Python land) construct a set of ROUTE points without actually moving the robot arm. Recall that in the originally intended operation from ST-Robotics, a user would move the joints with the Teach pad, or lead the arm "by the nose", and capture joint locations. Those locations would be stored as positions within a ROUTE.
As it stands, it is not clear where ROUTE memory is stored. It's also not obvious if there's some other way to hack values into a ROUTE without actually moving the arm. The only way I know to add a point to a route is by using the LEARN operation, and that pulls the joint positions from the encoders. $JL is like a "poke" or "store" operation that allows programmatic assignment of the route position without reading the encoders.
The $RUN command appears to be similar to a normal RUN operation. But, perhaps it's non-blocking. I'm not sure. I think ideally it returns immediately (whereas I think RUN is a blocking call) and that allows the rest of the system to continue, checking for positional changes until complete or interrupted.
The $ command is terse, and provides a way to get arm joint position information while the $RUN is in process. Its output differs from WHERE in few ways. First, it seems to always report position information in joint form, i.e., as motor step positions. It does not report data in Cartesian form. Second, it's terse. The command is a single character, reducing transmission speed, and the response is just a set of five integers. By comparison, the WHERE command returns a header line and three lines of data, and it may return Cartesian values if you don't ensure you're in JOINT mode first.
The github repository states that the $JL and $RUN commands are required, and if you can't run them, you should contact ST-Robotics support. I've tried that, and have not heard back.
Other findings
I'm a little confused why the python shell (and subsequently the arm.py code in r12_hardware_interface) establishes serial communication with two stop bits. Other parts of the documentation mention how you can change the baud rate, and I believe it says to use one stop bit. The existing shell code works with two bits, so I'm not wholly inclined to try to change that to one. But it may be an opportunity for a very small speed-up. I think it may be doable to improve overall communication speed by just bumping up the baud rate. That might not work in high (electrical) noise environments.
One neat thing I found in r12_hardware_interface / joint_state_remapper.py is conversion ratios. There is this chunk of code:
RATIOS = [3640, 8400, 6000, 4000, 4500] #Joint motor counts corresponding to 90 degree rotation
This defines the number of steps required on each axis in order to move 90 degrees. While it's not clearly commented in the code, the order of values is waist, shoulder, elbow, hand, and wrist.
So, you can say
> connect
> ROBOFORTH
> START
> CALIBRATE
> TELL WAIST
> 1820 MOVE
> TELL ELBOW
> 6000 MOVE
and you should have the waist rotated 45 degrees (half of 3640) and the elbow-to-hand section level to the floor.
These values will be important if I'm to do any of my own route computations.
No comments:
Post a Comment