Tag Archives: python

Primitive gait balancing – 1D

I’m working to improve the walking gait of the mjbots quad A1. In this iteration, I wanted to tackle an incremental step towards a more fully dynamic gait, but one that will still greatly increase the capability of the machine. As mentioned last time, the current walking gait cycles between all four legs, and then alternative opposing corner legs in order to move laterally. I’d like to keep that same basic structure, but be a bit smarter about what happens during the swing phase.

Setup

First, the obvious: When the robot has legs on the ground, they support it. If all four legs are on the ground, they form a support polygon. If the center of mass of the robot is within this polygon when projected onto the ground, the robot will happily sit there forever without tipping over.

During the flight phase of the gait, only two legs are in contact with the ground. Now, we no longer have a support polygon, but a support line. The center of mass of the robot is either on one side of it or on the other. Well, I suppose it could be exactly over, but then the slightest breeze would push it to one side or the other. Whichever side the robot is on, gravity will exert a torque and cause it to continue to tip over in that direction.

The current gait sequencer is oblivious to this fact. It will happily pick up the legs when the robot is far away from the support polygon, and then move the center of mass even further away. Needless to say, this results in the robot tipping over rather rapidly.

The “Idea”

Clearly the robot can’t move while keeping itself exactly over the support line, but can we arrange the gait such that it spends equal amounts of time on both sides (to be more precise, the integral of the distance from the support line over the time of the swing cycle is 0)? If we could, that would result in the minimum possible angular rotation during the swing phase, which would hopefully make things be a lot better without a whole lot more work.

In this post, I’ll consider the problem in 1 dimension, along a line as a simplification. Given that the robot can only move in one direction at a time, we should be able to generalize the 1 dimensional case to the 2 dimensional case later.

For a constant velocity and a fixed swing time, the answer is pretty clearly yes. If we start the swing phase right when the center of mass is 1/2 of the swing time from the opposing support, then the swing will be completed when it is on the other side spending exactly the same amount of time on each side of the support line.

In fact, this also holds for a more general case, as long as we don’t attempt to accelerate during the swing phase itself. All that is necessary is to start the swing when the center of mass is half a swing times away from the support point at the current velocity and not accelerate until the foot is down again.

Granted, in addition to the one dimensional simplification, in this model the feet are allowed to be infinitely far away from the center of mass. However, if we just apply a maximum time spent with all legs in stance, that places a limit on how far the legs can get away with only a slight decrease in performance.

The model also breaks down when the deceleration causes the robot to change directions. A simple heuristic is to change the leg order in that case.

Summary

Well, that looks pretty good in 1 dimension. Next up I’ll extend it to 2 dimensions, and try it out on the robot.

C++20 coroutines and moteus_tool

I’ve had a confusing mismash of development tools for the moteus servos for a while now.  My original development tool was in python, which worked just fine.  Coroutines allowed me to express complex asynchronous logic succinctly, the program itself was rather simple, and I could easily integrate it with matplotlib for plotting.  However, when looking to run this on the raspberry pi, I needed a newer python version than came with raspbian, which turned out to be a royal pain to get installed in a repeatable manner.  Thus I rewrote a portion of the moteus_tool in C++ and just used my normal cross-compiling toolchain to generate the binaries.  What I didn’t do was port the calibration logic, as the state machine required with standard boost::asio would have bloated the logic size by 5x, and I didn’t really need to calibrate servos from the raspberry pi ever.

Still, I’ve wanted to consolidate these tools for a while now, and while working towards other telemetry and development tool goals, I decided to make another pass at removing the duplicity.  I figured it was time to try using the new C++20 coroutines to implement the asynchronous logic in C++ and see if I could get rid of the python tool.  Since I’m currently vendoring all the compilers and libraries for the C++ applications, I am already running clang-9.0 with libc++ and boost 1.72 for all the host side tools which theoretically should all support coroutines just fine.

Why coroutines?

To recap, typical callback based boost::asio code looks something like this:

void DoSomething() {
  boost::asio::async_write(
    *stream_,
    boost::asio::buffer("command to send"),
    std::bind(&Class::DoneWriting, this, pl::_1));
}

void DoneWriting(boost::system::error_code ec) {
  FailIf(ec);
  boost::asio::async_read_until(
    *stream_,
    response_,
    "\n",
    std::bind(&Class::HandleRead, this, pl::_1));
}

void HandleRead(boost::system::error_code ec) {
  FailIf(ec);
  // Do something with the result.
}

This gets even worse if you have embedded control flow.  Typically the best you can do is construct an object to hold the state, and bind it around to keep track of things:

struct Context {
  int iteration_count = 0;
};

void Start() {
  auto ctx = std::make_shared<Context>();
  Write(ctx);
}

void Write(std::shared_ptr<Context> ctx) {
  message_ = fmt::format("{}", ctx->iteration_count);
  boost::asio::async_write(
    *stream_,
    boost::asio::buffer(message_),
    std::bind(&Class::HandleWrite, this, pl::_1, ctx));
}

void HandleWrite(boost::system::error_code ec,
                 std::shared_ptr<Context> ctx) {
  FailIf(ec);
  ctx->iteration_count++;
  if (ctx->iteration_count >= kLoopCount) {
    Done();
  } else {
    Write(ctx);
  }
}

Now, when coroutines are used, you can relinquish control with a simple “co_await”, and all the logic can be written out as if it were purely procedural:

boost::asio::awaitable<void> Start() {
  for (int i = 0; i < kLoopCount; i++) {
    auto message = fmt::format("{}", i);
    co_await boost::asio::async_write(
      *stream_,
      boost::asio::buffer(message),
      boost::asio::use_awaitable);
  }
}

Not only is this significantly shorter, it more directly expresses the intent of the operation, and lifetime of the various values is easier to manage.  No longer do we have to keep around the buffer as an instance variable or a shared ptr to ensure it lives beyond the write operation.  It stays in scope until it is no longer needed like any other automatic variable.

moteus_tool

Switching moteus_tool to coroutines was straightforward, and resulted in a big reduction in the verbosity required.

My first non-Fusion generated g-code

While working to build a weight reduced moteus servo mk2, I reworked my outer housing CAM to do all the machining on the Pocket NC v2-50.  For this part I didn’t necessarily need any challenging workholding and since I could get the stock in tube form, there wasn’t an inordinate amount of material to remove either.

The one challenge is that when mounted in the Sherline Chuck, the mill can’t actually reach all the way to the edge of the part without hitting the X travel limit (which is why most of the other 100mm diameter parts I do are fixtured slightly off-center).  In this case I tackled the problem in two iterations.

Iteration 1

For the first iteration, I just used an adaptive clear from 8 different directions to get most of the material out of the way, then used a multi-axis “flow” to finish the outer diameter causing the B axis to rotate while the end-mill remained roughly in place.  Then a subsequent pass came in from the top to clean up all the stuff that was left behind.

20191216-outer_housing_3_plus_1.png
Coming at the problem from all sides

20191216-outer_housing_od_finish
A “flow” toolpath to do the outside

This worked, but had a couple of problems.  First, it was slow.  A full cycle time was something like 10 hours, largely because all the adaptive clears spent a lot of time not removing much material and rapiding around.  Second, it left a non-ideal surface finish on the outer diameter.  The “flow” toolpath for some reason seemed to jiggle the mill around in X and Y for no great reason, and occasionally sped the B axis up by like 3 times the normal rate for a quarter revolution for no apparent reason.

Iteration 2

I figured this would be a lot easier if I could just have more control over the mill while spinning the B axis.  I could take all the extra material off the top using the side of the cutter, and produce a nicer surface finish on the outside.  Since that wasn’t possible within Fusion 360, I figured this wouldn’t be a terrible time to try doing some “manual” g-code for the first time.  The “manual” is in quotes only because I ended up writing a python script to do the actual generation.

To begin with, I started with the g-code from a Fusion 360 generated toolpath so that I could get the tool setup, probing and such configured in a way that I knew the Pocket NC would accept.  Then my python script had two options, the first generated the g-code to turn down all the material on the top of the stock to the final length.  It moved the mill into position, spun the B axis by 340 degrees, then gradually moved down a Y step while moving 20 degrees, then spun another 340 again until reaching the end.  This worked out just great, used much more of the cutting length of my Datron 4mm mill, and got done in something like 20 minutes.

The second option in the script was for turning down the OD to the final size.  This used the same basic approach, but instead of setting the Z past the inside of the tube, set it to exactly the OD.  The the Y stepped down in the same manner as before, just over a different range (the top of the finished part to slightly past the bottom).

This got me to where I could start on the internal features in only about 40 minutes of Pocket NC v2-50 time, which is a big improvement over a trip to Artisan’s Asylum and an hour on the lathe in order to get it set up, turning the part, and then cleaning up.

 

ICFP 2019 Programming Contest

For many years now on and off I’ve played in the programming contest associated with the International Conference on Function Programming.  Each time it has been fun, exhilarating, and definitely humbling!  www.icfpcontest.org

This year, the competition runs from Friday June 21st through Monday June 24th.  Once again we’ll be competing as “Side Effects May Include…”.  Look for us on the leaderboard and cheer on!

 

legtool now on github

legtool is the library and graphical application I’ve developed for inverse kinematics and gait generation to use with Super Mega Microbot. I’ve pushed a public version of it to github, in the hopes that it may be useful for other developers of legged robots.

I’ve also put together a short demonstration video, showing how to download, install, and use legtool with a gazebo simulated quadruped.

 

In lieu of actual documentation (or possibly to start generating the text of some), I’ve written some text describing what legtool does and how to to use it below.

legtool overview and installation

legtool is a python library and PySide based graphical application useful for pose generation, inverse kinematics, and gait generation for legged robots. It consists of a set of python libraries, and a graphical debugging application which uses those libraries to allow a developer to explore legged locomotion. It has primarily been tested on Ubuntu linux, but may be portable to other operating systems.

legtool is not yet packaged for installation, so all you need to do is clone the github repository and install the dependencies as documented in the README.

git clone https://github.com/jpieper/legtool.git

Legtool by default creates a configuration file in ~/.config, however, you can specify an alternate configuration on the command line using the -c option. If you have multiple robots, or a simulation and an actual robot, you will want to keep multiple configurations around.

Servo tab

The first tab which is active, the “servo” tab, contains controls to select the servos to control and configure them, along with controls to maintain a list of named poses, where each pose consists of a set of servo positions.

20140713-legtool-servo-tab.png
legtool servo tab

 

Here you can move servos individually to verify their operation, and compose poses. The only use for poses currently is in the later inverse kinematics tabs, so there is no need to define more than idle, minimum, and maximum.

Inverse kinematics tab

The second tab, the IK tab, lets you configure which servos are in which leg, what sign they have, and also the geometry of the legs. The lower right corner visualizes the achievable region of operation in 2D. You can select alternate projections, and change the centerpoint of the test using the IK Offset group. Clicking on the rendering will command that leg to that position.

20140713-legtool-ik-tab.png
legtool IK tab

Gait tab

The third, and currently final tab, contains controls to allow configuring the placement of all shoulder joints, marking the center of gravity, and configuring the gait engine. It provides several debugging facilities to allow visualizing the results of the gait engine.

20140713-legtool-gait-tab.png
legtool gait tab

The geometry configuration is in the upper left. The shoulder position can be set individually for each leg. The center of gravity and idle position are common to all legs. The gait configuration area in the lower left selects which gait to use, and then exposes a number of configuration options for that gait.

The remainder of the tab is devoted to interactive tools for debugging the gaits:

  • Gait graph: The gait graph in the upper middle shows for the duration of a single cycle, when each of the legs is in the air.
  • Geometry view: Visible in the upper right, this shows a 2D rendering of where the shoulders, legs, body, center of gravity, and support polygon are located.
  • Graphical command view: Visible in the lower right, the graphical command view shows which commands are feasible given the current geometry and gait configuration. Any two command axes can be selected, after which the feasible region is shown in green, regions where the speed is limited are in yellow, and totally infeasible regions are in red.
  • Playback: The playback scrubber allows you to slide back and forward through a gait cycle, visualizing the results in the geometry view or on the simulated or real robot.
  • Textual command area: All of the 9 degrees of command freedom are settable here, in addition to the two available in the graphical command view.

pygazebo 3.0.0-2014.1

Yesterday I pushed the release of the newest version of pygazebo to github, 3.0.0-2014.1. This version has two big changes, the first is that I’ve updated all the message bindings for gazebo 3.0.0, which gets two new messages, CameraCmd and SphericalCoordinates. The second, and larger change, is a switch from eventlet to trollius for asynchronous coroutine operation.

trollius is an implementation of the python3 asyncio/tulip library for python2. It is largely API compatible, with the biggest exception being that as a library, trollius can’t support the new python3 “yield from” syntax. pygazebo internally uses an API compatible subset.

The biggest reasons to switch were:

  • Explicit control flow: In the asyncio model, the only ways of yielding control are by returning from a function, or “yield”ing from a coroutine. This means that you can reason locally about whether synchronization is appropriate, even when calling other methods.
  • Integration with other event loops: While eventlet was nominally capable of being integrated with other event loops, in practice it wasn’t very easy and no one did it. The best integration strategy was usually to poll at a high frequency in another event loop and pump the eventlet loop. asyncio already has a draft event loop to interoperate with GLib (gbulb), and writing a basic one to interoperate with QT was not challenging.
  • Unit test coverage: Eventlet (and other greenlet based systems) had a long-standing issue where most python unit test systems could not accurately track test coverage. trollius and asyncio seem to work just fine here.

The new release is on github and pypi, so a newer pygazebo is only a “pip install pygazebo” away.

pygazebo – First release

I managed to clean up the python bindings to Gazebo I wrote when testing mech gaits in simulation, and have released them publicly as pygazebo.

All you have to do is:

pip install pygazebo

From the README, a simple example:

import eventlet
from pygazebo import Manager

manager = Manager()
publisher = manager.advertise('/gazebo/default/model/joint_cmd',
                              'gazebo.msgs.JointCmd')

message = pygazebo.msg.joint_cmd_pb2.JointCmd()
message.axis = 0
message.force = 1.0

while True:
    publisher.publish(message)
    eventlet.sleep(1.0)

and you’re off and ready to code!