The big result since the last status report is that more, very important safety systems have been implemented to protect the robot and its surroundings, and the robot now has more data about objects near to it as it moves so it can avoid running into those obstacles. And the new power and data control panel is finished, which makes it easier for me to disassemble the robot.
The last week and a half has seen a lot of work on Puck. Two of the time of flight sensors were replaced as the originals were either not sending distance measurements or the measurements were wildly wrong. While that layer of the physical robot case was opened to replace the sensors, I also color-coded all the cables for the time of flight and SONAR sensors, making it easier to trace the cables from the custom micro processor board (a.k.a the Teensy Monitor Board) to the physical sensors. The new power and data control panel was finished and all the new wiring is in place.
I also rewired the signal cable from the relay block to the emergency stop of the motor controller. This would allow the Teensy Monitor to immediately stop power going to the motors if some emergency situation were discovered.
A lot of work was done in the software of the Teensy Monitor. More diagnostic messages were added, which showed that there are still reliability issues with communication between the Teensy Monitor and the sensors, which is something I’ll have to solve eventually. I may just need to add better cable shielding.
The motor temperature sensors now are published to a topic, and the temperature values are now averaged as the sensors tend to be rather noisy. Code was added to detect an over-temperature situation for the motors. When the robot is driven for a long time, the temperature inside the motors can build up to the point that there is a risk of melting the wires inside.
Importantly, code to detect a runaway motor situation is now implemented. This happens especially when the encoder connections to the motor controller come loose. Remember that the robot vibrates a lot when it moves, and the connectors on the RoboClaw motor controller are not designed to stay well connected in the face of vibration.
The redundant motor current sensors were found to be useless, so the code that talked with those sensors is disabled for now. I’ll need to find some better sensors in the future. Meanwhile the motor current sensors built into the RoboClaw are working well enough. And code was added to detect an over-current situation, which can happen when the robot runs up against, say, a wall and the motor wheels stop turning or turn very slowly. Left in this situation, the wires in the motor would quickly overheat and literally melt.
With the three new fault detection code routines, the robot now get shut off via the emergency-stop signal to the RoboClaw if the motors get too hot, if the motors take off at high speed unexpectedly, or if the motors start drawing a lot of current, beyond normal operation. This is a three way protection to save the expensive motors and prevent the robot from running into something and just keep pushing forward.
With the now fully working SONAR and time of flight sensors, and the data being published at 25 to 30 frames per second, I finally figured out how to hook those sensors into the robot motion planning software. Now, if the robot is moving along and comes across an obstacle that only one of the proximity sensors can see (the LIDAR sees objects only at a specific height), the robot can make a plan to move around the object.
Outstanding work remains to discover why some of the sensor values are occasionally rejected by the planning software. It is likely that is because the data is too old, meaning that the data was more than a couple of milliseconds old by the time the planning software saw it. There may be a simple fix, but it needs to be investigated.
This is a complete replacement of a previous blog post. At the very end of this article you will find the small bit of XML code which defines the behavior tree described in this article.
I’m trying to develop a skill in using behavior trees as the logic to move Puck. As part of that learning, I have developed a behavior tree that attempts to move Puck so that one of its edges it is within 6 inches (0.1254 meters) from either a left or right wall in my hallway. The basic strategy will be, if Puck is not already near a wall, to turn left and drive up to some “maximum distance” looking for a wall ahead and, then turn right so that it is heading in the original direction. If a wall wasn’t found to the left, Puck will then turn right and search for up to twice that maximum distance for a wall and then turn left to be, again, facing in the original direction. The reason to travel up to twice the original maximum distance this second time is that the robot needs to move “maximum distance” just to get back to where the robot was when it first began searching to the left.
Here is my first attempt at such a behavior tree.
FindWall behavior tree
Assuming you are not well-versed in behavior trees, let me try to explain what the tree represents.
First, behavior trees consist of a tree of nodes. Interpretation (execution) of the behavior tree begins by “ticking” the top node in the tree and, depending on the kind of node, that node may “tick” one or more of the child nodes, recursively. A ticked node returns a status of either RUNNING, SUCCESS or FAILURE. The ticked status of a child node is used to decide what a particular node does next. I hope this will be clearer as I describe the above tree. Also, look at the link at the beginning for a more complete description of how behavior trees work.
Behavior tree nodes can communicate with each other via a “blackboard”, which is a simple key/value store. There are various ways to write the value of a key name into the blackboard, and nodes can get the value of any key name. Not delved into here, there is a way of remapping key/values between nodes so that nodes can be written as more generic pieces of code, taking parameters.
The robot will begin execution at the Root node which will then attempt to tick the IfThenElse node. This node begins by interrogating the status of the first child node and, if its status is SUCCESS, it will execute the second child node, else instead it executes the third child node. IfThenElse returns as its status the status of either the second or third child, whichever one was chosen.
The first node of the IfThenElse node is a FoundLeftOrRightWall node, a custom node I crafted for Puck. FoundLeftOrRightWall works by reading the various sensors of Puck to try to find the nearest distance from the edge of the robot to the left and right walls near to the robot. FoundLeftOrRightWall takes a parameter called “within_distance”, which was given a hard-coded value of 0.1254 meters (6 inches) for this node as shown. If either the left or right wall is less than or equal to 0.1254 meters away from the edge of the robot, FoundLeftOrRightWall will return SUCCESS else it returns FAILURE.
If Puck is already within 6 inches of the left or right wall, the second node, AlwaysSuccess is executed which, as you might guess, always return SUCCESS, which will be propagated to the IfThenElse node which itself propagates SUCCESS to the root node and we’re done. This whole behavior tree is attempting to position the robot within 6 inches of a left or right wall. If Puck is already there, nothing additional needs to be done.
If Puck is not within 6 inches of a wall to start, it needs to get there, so instead of the AlwaysSuccess being ticked, Sequence will be ticked.
A Sequence has any number of child nodes, they being RetryUntilSuccesful and IfThenElse in the above. A Sequence node begins by putting itself into the RUNNING state. It then ticks the first child node. If that node returns FAILURE, the Sequence stops and returns FAILURE itself, which is propagated to the parent node (IfThenElse in this case). If the child returns RUNNING, then Sequence will just keep ticking that child until either SUCCESS or FAILURE is returned. If SUCCESS is returned, Sequence will then go on to the next child node and continue until there are no more child nodes, at which point Sequence itself returns SUCCESS, because all of the child nodes were successful.
Sequence, above, will then begin by ticking the RetryUntilSuccesful node. In the picture above, “FindWallToTheLeft” is just a name I gave to the node so I have an idea what the node is trying to accomplish.
The way a RetryUntilSuccesful node works is it will attempt to execute the child node (an IfThenElse node here) up to “num_attempts” (hard coded as 3 attempts here) times. If the child node succeeds during any of those 3 attempts, RetryUntilSuccesful will stop ticking the child node and return SUCCESS to the parent node (Sequence in this case).
So the RetryUntilSuccesful is going to make up to 3 attempts to find the left wall. It makes an attempt by ticking the IfThenElse node which tests to see if FoundLeftOrRightWall sees a wall within 0.1254 meters. If so, AlwaysSuccess is returned else a subtree will be executed called FindWallInDirection. That subtree will be described below but basically it attempts to find a wall in the given direction by turning in the indicated direction, moving up to some maximum distance looking for a wall, and then turning back to the original direction.
You should get then that the RetryUntilSuccesful is going to make up to 3 attempts to find a wall to the left of the original orientation of Puck. If it finds a wall, it returns SUCCESS to Sequence, which will then go on to its second child, another IfThenElse.
And I can see now that his is wrong. Remember that the way Sequence works is that it will sequentially execute each child node as long as each child node returns SUCCESS. If RetryUntilSuccesful returns FAILURE because it didn’t find a wall to the left, then Sequence stops ticking the child nodes and itself returns FAILURE.
I won’t correct the tree in this post. But it shows how using the graphical representation of a tree, and trying to explain it to someone, can uncover errors in the behavior tree. Instead, the fix is to insert a node between Sequence and RetryUntilSuccesful which ignores the status of RetryUntilSuccesful and always returns a SUCCESS status.
Going on to the second child of Sequence, it is going to use IfThenElse to see if the first child found a wall to the left and, if not, will use a similar RetryUntilSuccesful child node to search to the right. Note that this second RetryUntilSuccesful node, that I named “FindWallToRight” will make up to 6 attempts to find a wall to the right whereas the first RetryUntilSuccesful only made up to 3 attempts. That is because we may need 3 RetryUntilSuccesful attempts just to get back to the original position of Puck when we started looking for a wall, then we make up to 3 more attempts to find a wall to the right.
There is no error here by not including a forced success parent node for the second RetryUntilSuccesful node. If we fail to find a wall to the right, the whole tree needs to return FAILURE.
Now I’ll describe the FindWallInDirection subtree which is reused in two places via the parent behavior tree. This allows function-like behavior without having to code a common bit of tree behavior in more than one place.
FindWallInDirection subtree
FindWallInDirection is going to execute a sequence of two child nodes. There is an error in this subtree, which I’ll explain as we go along.
The first child node of Sequence is an Switch2 node. This node examines a blackboard variable, “rotate_first_in_direction” and will chose one of the child nodes depending on which of the “case_1” or “case_2” values matches the “rotate_in_first_direction” value. If the value is “left”, the first node will be ticked, which is a custom node I created called Turn that will turn 90 degrees, which is a turn to the left. If the value is “right”, the second node will be ticked ,which will Turn -90 degrees, which is a turn to the right. If the value is neither “left” or “right”, the third child, AlwaysSuccess is ticked, so no turn is made at the start.
The purpose of FindWallInDirection is to make an initial turn, attempt to find a wall ahead of the robot within some maximum distance, and then reverse the turn so the robot is pointing back in the original orientation. What is missing in this subtree is the action that makes that restoring turn. Again, this shows the value of creating a graphical representation of the behavior tree and then trying to explain it to someone. I will not attempt to fix the tree in this blog post.
After the initial turn is made, the Sequence node executes the second child, a RetryUntilSuccesful node which is going to make up to 6 attempts to move forward a bit and see if a wall is found ahead of the robot. This does so by executing an IfThenElse node which starts by testing if a wall is found ahead of the robot via the custom node I created, FrontWallFound, which takes a “within_distance” parameter. If the front wall is near, rather than just returning SUCCESS, I chose to tick the SaySomething node to emit a message to the log, which will also return SUCCESS as a side effect. If the wall isn’t found ahead of the robot, the custom node I wrote, MoveForward is ticked to move the robot ahead 0.0762 meters (3 inches).
I have been doing experiments with Puck while it was sitting on the top of a desk. To help prevent it accidentally rolling about, I enabled the wheel locks for the two caster wheels in back. I was ready to try an experiment with an obstacle avoidance block of code I have been developing, so I put Puck on the floor in a hallway and brought up the software stack.
As is often the case, I also brought up a keyboard teleoperation node which allows me to drive the computer manually and remotely. I have a data visualizer on my desktop computer (rviz2) that lets me look at the LIDAR scans, the proximity sensors data, any of the camera videos, a mockup of the physical robot placed in the house, and so on, and the robot can be far away from me.
As I began to move the robot, it didn’t seem to be steering as I expected. It kept pulling to the right and I heard some unusual sounds coming from the hallway. But my mind was on the new feedback in the display which was showing me an idea of potential hazards along the way along with a recommendation of how to avoid the hazards. It was all fun until it wasn’t. Eventually there was a grinding sound coming from the hallway.
I went out to look and the picture shows what I saw. The right wheel and motor were twisted around and the wheel was grinding against the frame of the robot. I could barely imagine how this was even possible. My first thought was that there was a collision and the four bolts holding the motor to the frame had sheared off.
I put the robot back on the desktop and took off the top plate with the control panel and LIDAR, then the middle plate with the two computers and the camera array, and then I could look at the motor mounts. What had actually happened is that three of the four screws holding the motor had come out of the holes. How is that possible, you may ask?
The screws are 6-32 screws and after the initial robot construction I added a spacer between the motor and the chassis as the rear of the motor body is slightly wider that the front of the motor body and the right angle gear box. The screws fit into that gearbox. The spacers give room for the motor to lie completely parallel with the plane of the frame. When I added those spacers (just washers), it made is so the screws no longer reached completely through the gearbox housing. In fact, and I didn’t measure this when I did it, the screws barely screwed into the motor housing at all. And the torque from one locked caster wheel on the wheel-coupler-gearbox assembly was enough to really twist the motor so that three of those screws popped out.
The fix was easy enough—I replaced the screws with longer screws. But I still don’t have, for example, a sensor to detect if the caster wheels are locked or not before driving. That is still an open problem for human error for the robot. I could remove the wheel locks, but without a specialized frame to hold the robot off the floor when I run experiments where I want to motors to turn but the robot to not actually go anywhere, without that frame the wheel locks are my safety feature to prevent the robot from rolling off the desk.
It’s been a long slog trying to get reliable recordings of the important sensors and other bits of data from the robot.
I need these recordings so I can, say, drive around the house and record the proximity sensors, odometry and LIDAR as, together, they reflect what is around the robot as it moves. Then I can work on my software that tries to determine where the robot is in the house, where it is trying to get to, and computes a plan for getting there as the world changes around it.
It’s not practical to try to debug complex software by trying to repeatedly drive the robot around the house on the same path, there are too many variables that will never be quite the same from run to run. And the battery discharges over time and there are only so many recharges I can make to the expensive battery. But by playing back the gathered data from one run over and over, I can pause where I need to and look at why my software isn’t quite doing what I expected.
I’ve mentioned in the past that ROS2’s recording system (rosbag2) is unreliable, at least so far, and lacks features I want, at least so far. I’m pretty sure that ROS2 will eventually be “good enough”, but that’s months or years in the future. Meanwhile, as has been for my whole life, I’m limited by my mortality.
Unfortunately, the state of hobby robotics today, right now, is that you have to build everything yourself. Oh, you can start further up the ladder and spend money to buy a better base, better sensors, faster computers, a better starting position. But the problems are many and the existing solutions are few, specialized and brittle.
The way I tackle any complex problem is to try to reduce the problem to a sub problem that is just out of reach of what I can do today. Then I build what I know and understand, and then work on that little bit more that gets me a solution to that “just out of reach” problem. Then I go on to the next sub problem that is just out of reach but on the path towards being the real problem I need to solve.
Puck is intended to transport a robot gripper around the house, to find and fetch something, and move that something to a different place. Many people working on hobby robots call this the “fetch a can of beer from the refrigerator” problem, which is a fair goal for a hobby robot. Mind, for me it’s sufficient that the robot moves from some place reasonably far away from that “can of beer” which might actually be a colored block to start, maybe on a table to start with rather than in a refrigerator, recognize the can, grab the can, pick it up and move it to some other predefined place.
I don’t have a gripper yet. I’ve got ideas about how to build one, and friends who have offered to help me get something working. But gripping things isn’t the next “just out of reach problem” I need to solve just yet. Baby steps.
Over the past few years, as I’ve tried to learn robotics and build a real robot, I have been building ever more complex robots that get ever nearer that can-grabbing robot. But everything about robots is hard.
For example, my robot, as of today, pumps out 400 sonar samples a second and 800 laser, time-of-flight samples a second. Getting that information from the computer that reads those sensors to the computer that reacts to the data has been a real problem.
I was at the point where I was getting all that data successfully transferred, then I added “just one more” thing to the mix and only half the data was being transferred. Why? I don’t know yet. And when I tried to add in “just one more sensor” to the mix, the whole thing blew up. Why? I don’t know yet.
But I can capture some data and play it back. And it sort of works. Something is missing in the “transform tree” yet so the whole magic isn’t working yet, but I’ll get there. Slowly.
But it’s a long slog, because everything about robots is hard.
Puck is my current robot, as of this posting date. It is intended to be sufficient for autonomous navigation around my house and, eventually, to include a manipulator that will allow Puck to go to the refrigerator, open it, select a specific item off a shelf, grab it, close the refrigerator, and bring the item to me.
I typically name each generation of my robot with a name starting with the next letter of the alphabet. Over the last 3 or 4 years, I’ve gone from Arlene to Puck.
Puck weights in at about 56 pounds on the date of this posting.
The hardware changes all the time as I run experiments and learn what works and what doesn’t. The frame is constructed from 2020 T-slot aluminum extrusions and 1/8-inch thick, aluminum plates.
Motors, Shaft Encoders, Wheels
A motor, right angle coupler and wheel
The robot has a pair of motors beneath the bottom platform. The motors are model IG42 from Superdroid Robots (click on IG42 to see the specifications and on Superdroid to see the catalog item). The motors drive a right angle connector with a 1/49 gear ratio and run at a maximum of 122 RPM with 24 volt power, producing a maximum of 15 kg-cm torque. Note that 15 kg-cm = 1.47 N-m = 1.08 ft-lbs.
According to this site, a pair of motors with 1.5 N-m torque each should be able to propel a 27.22 kg (60 pound) robot with 0.1 meter (4 inch) radius wheels up a 3 degree incline at 1 meter per second (2.24 miles per hour) at an acceleration of 0.2 meters per second squared. This should suffice for my goals.
Each motor has attached a dual-channel, quadrature encoder to provide an estimate of how much rotation each wheel has made over time.
The wheels are 8 inches in diameter and come from RobotShop.
Electrical Power
The main power source is a 36 volt, 30 amp-hour LiFePo4 battery. This drives:
A 48-volt to 24-volt, 10-amp DC to DC converter. The 24 volts is supplied to a RoboClaw, dual 15-amp motor controller.
A 48-volt to 12-volt, 10-amp DC to DC converter. The 12 volts powers several components, including another DC to DC converter which supplies all the power to the main, AMD Ryzen 3700X processor and motherboard.
A 48-volt to 5-volt, 10-amp DC to DC converter. The 5 volts powers several components, including a custom monitor board and an 8-port Ethernet switch.
There is an emergency-off main power switch which feeds the battery voltage to a volt/amp/watt meter which then feeds the DC to DC converters. Through fuses, of course.
The battery terminals are brought out to a pair of Anderson Powerpole connectors which allow an external charger to charge the battery.
The Bottom Plate
The bottom plate
The bottom plate holds the motors, shaft encoders and motor temperature sensors underneath, and on top is:
The battery.
Three DC to DC power converters.
A custom PC board which monitors many sensors and the health of the system.
An 8-channel relay board which the monitor controls to power on or off various subsystems or to reset them.
The RoboClaw motor controller (mounted vertically just above the battery in the picture, not very visible).
A snubber circuit to prevent damage from back-EMF if the motors should be slowed down by external forces (also not very visible in the picture).
A pair of redundant current monitors for the motors (not very visible).
A terminal barrier strip which routes all the power wiring.
The Middle Plate
The middle plate
The middle plate holds a camera sensor array (not shown in the picture), the main computer (right in the picture) and a secondary computer (left in the picture). Below the aluminum plate is an 8-channel Ethernet switch, a bank of Anderson Powerpole connectors to distribute 12-volt power, and a bank of Anderson Powerpole connectors to distribute 5-volt power.
The main computer is an 8-core, AMD Ryzen 3700x processor with 32GB of RAM and 512GB of solid state (NVMe) disk. The motherboard is also holding a GeForce GT 1030 graphics card.
The secondary computer is an Nvidia Jetson Xavier NX computer, tasked with most of the artificial intelligence tasks for the robot.
The Top Plate
The top plate
The top plate currently hold a LIDAR from a Neato vacuum cleaner, and the control panel. The LIDAR will soon be replace with a different kind. Eventually, a gripper will sit on the top plate.
Partial view of the control panel
The control panel holds a pair of HDMI video/USB connectors (not shown in the picture) which feed the video/keyboard output from the two computers to external keyboards and monitors. Often I put the robot up on blocks and connect a keyboard and monitor to one or both computers for a better software development experience, as opposed to remote login to those systems over the network.
Shown in the picture is the red power-on/emergency shutdown switch (lower right), a voltage/current/wattage meter (lower left) and the control panel for the custom monitor (upper left). The custom monitor panel is a touch screen and can be used to control power to several subsystems or to reset some subsystems. It also shows the real-time readings of several sensors.
The control panel also holds the charging plug for the battery (not shown in the picture).
The Sensors
Mounted to the frame of the robot are 4 sonar sensors and 8 time-of-flight laser sensors. These 12 sensors are used to provide better proximity detection of obstacles near the computer. Readings further than about a meter from the periphery of the robot are ignored by these sensors and other sensors provide distance measuring out to at least 6 meters. Since each kind of sensor has its own special set of materials it can see or not see, I use two different kinds of proximity sensors.
The camera sensor array
In front of the robot, on the middle plate, is mounted a camera sensor array. There are a pair of Intel Realsense D435 stereo cameras which can provide color images, infrared images and depth images. The pair are mounted such that together they give almost a 180 degree view from the front edge of the robot.
Between the D435 cameras, facing upwards, is an Intel Realsense T265 camera which can provide infrared imagery, black and white imagery, but primarily visual odometry for the robot. Motor shaft encoders are poorly suited for robot odometry, although they are used to advise the T265 camera.
In the picture, below the cameras on a cross member you can see one of the sonar sensors.
Shown previously is the LIDAR sensor for 2-dimensional, distance visualization of the environment. The Realsense cameras provide 3-dimensional distance visualization.
The Network
Below the middle plate is mounted an 8-channel, gigabit Ethernet switch. Connected to the switch are the main and secondary computers, the monitor and sometimes a cable is routed from the household Ethernet network. The main computer provides a WiFi connection to the household network, so the robot talks to my other household computers as needed as it roams the house. Mostly this is so I can run visualization software on my laptop or desktop computer to see how the robot is performing.
The main computer also provides a Bluetooth network connection so that I can use a gaming controller to manually drive the robot around the house, if needed. This is especially useful when I bring the robot to offsite locations to show it off, as the robot is rather heavy to carry very far.
