AXONS - Homebrew firmware for AX12+ and CM510

[Fraser]

phpBB [media]

[Fraser]

I just solved a problem I was having with my setup, some times when I issued PWM command to servo 15 other servos (7,8,9,10,12) on the bus went haywire with random PWM values. I changed 15 to 13 and vice versa by reprogramming firmware, servo 15 (now in knee) still caused the problem. Oh noooo, I thought, my firmware is buggy.

Anyway it turns out that servos 7,8,9,10,12 had corrupt firmware. This was caused by me accidentally initiating all 18 bootloaders at once on the same bus some days ago. This causes chaos as the characters from servos cause others to do random stuff, some clear eeprom some dump etc. At the time I realised that some servos went dead so I reprogrammed them and thought the others were fine, turns out they were slightly corrupt.

Moral of the story:
When uploading new firmware to servos in situ on the robot make sure and be careful that each servo is isolated from all others, if not then you will need to reprogram the whole lot because some may be corrupted without showing obvious signs.

Next stage for me is to add different flavors of PC to Robot PWM packets so you can choose to receive just position data or position data and gyro, IMU etc.

Now I will investigate the notoriously noisy robotis gyro, I wonder if wrapping the analog lines around small ferrite toroids will help...

[billyzelsnack]

(Don't be discouraged by the lack of responses to your work. I know I'm interested in all your posts on this research. Keep it up!)

[Fraser]

Cheers Billy, I will keep on hacking away at this.

[Fraser]

Thanks to Saschas info on tweaking the USB2Dynamixel FTDI driver in windows I have managed to get the total comms loop time down to 11ms for 18 servos on windows7.

The actual ping time is down to around 7ms, the extra 4ms is the total time length of the transaction packets for 18 servos, about 2ms for PWM and 2ms for Position.

I'm pretty happy with it, I set the right side of the robot to mirror and slave the left side: The left side is open loop (no PWM drive) and the right side is closed loop. So when I move the left arm the right arm mirrors the actions etc. Also tried servo springiness which works nicely.

I am about to play with parsing Robotis motion files so I can finally get my bot walking again, hoping to tweak joint interpolation, add bias, temporal offsets etc. realtime.

Oh yeah, 2 nights back I got my first bot injury, was holding the bot by its back and a code error caused the arms to go crazy, left index finger got pinched between shoulder bracket and chest cover, had an L shaped flap cut far enough to bleed. Glad it was only an AX12!

I'll post a vid when I get him walking with Axons.

[billyzelsnack]

You're in luck. The mtn file is text and trivial to parse.

I quickly hacked this together for robocup last year. It's a mess, but it might be useful to someone.

Code: Select all

static bool loadit( FILE* fp, Motion& inout_motion )
{
   unsigned char header[3];
   //if( fscanf_s( fp, "%c%c%c", &header[0], 1, &header[1], 1, &header[2], 1 )!=3 ){ return false; }
   if( fscanf( fp, "%c%c%c", &header[0], &header[1], &header[2] )!=3 ){ return false; }
   printf("header %d %d %d\n", header[0], header[1], header[2] );

   char line[256];
   char* linePtr=0L;

   linePtr=fgets( line, sizeof(line), fp );
   if( !accept( linePtr, "type=motion" ) ){ return false; }

   linePtr=fgets( line, sizeof(line), fp );
   if( !accept( linePtr, "version=1.01" ) ){ return false; }

   linePtr=fgets( line, sizeof(line), fp );
   if( !accept( linePtr, "enable=" ) ){ return false; }

   linePtr=fgets( line, sizeof(line), fp );
   if( !accept( linePtr, "motor_type=" ) ){ return false; }

   while( !feof(fp) )
   {
      linePtr=fgets( line, sizeof(line), fp );

      if( accept( linePtr, "page_begin" ) )
      {
         Page page;

         linePtr=fgets( line, sizeof(line), fp );
         if( !accept( linePtr, "name=" ) ){ return false; }
         char name[64]={0};
         sgets( name, sizeof(name), linePtr );
         page._name=strdup( name );
         //printf( "name [%s]\n", page._name );

         linePtr=fgets( line, sizeof(line), fp );
         if( !accept( linePtr, "compliance=" ) ){ return false; }

         linePtr=fgets( line, sizeof(line), fp );
         if( !accept( linePtr, "play_param=" ) ){ return false; }

         while( !feof(fp) )
         {
            linePtr=fgets( line, sizeof(line), fp );
            if( accept( linePtr, "page_end" ) )
            {
               inout_motion._pages.push_back( page );
               break;
            }

            if( !accept( linePtr, "step=" ) ){ return false; }

            Frame frame;
            frame._valid=true;
            frame._pause=0;
            frame._time=0;

            std::vector<float> positions;
            //char* nextToken;
            //char* token=strtok_s( linePtr, " \r\n", &nextToken );
            char* token=strtok( linePtr, " \r\n" );
            while( token!=0L )
            {
               float value=0;
               //if( sscanf_s( token, "%f", &value )!=1 ){ return false; }
               if( sscanf( token, "%f", &value )!=1 ){ return false; }
               positions.push_back(value);
               //printf( "[%f]\n", value );
               //token=strtok_s( 0L, " \r\n", &nextToken );
               token=strtok( 0L, " \r\n" );
            }

            int numSteps=positions.size();
            for( int ii=0; ii<numSteps; ii++ )
            {
               if( ii==(numSteps-2) ){ frame._pause=positions[ii]; continue; }
               if( ii==(numSteps-1) ){ frame._time=positions[ii]; continue; }
            
               ServoFrame servoFrame;

               servoFrame._id=ii;
               servoFrame._position=(int)positions[ii];
               
               frame._servoFrames.push_back( servoFrame );
            }
               
            page._frames.push_back( frame );
         }
      }
      else
      {
         unsigned char trailer[3];
         //if( sscanf_s( linePtr, "%c%c%c", &trailer[0], &trailer[1], &trailer[2] )==3 )
         if( sscanf( linePtr, "%c%c%c", &trailer[0], &trailer[1], &trailer[2] )==3 )
         {
            printf("trailer %c %c %c\n", trailer[0], trailer[1], trailer[2] );
         }
         break;
      }

   }

   printf("werd\n");

   return true;
}


[Fraser]

DAMN! That would have saved me a shed of time if I'd seen it earlier

It's a lot more compact using sscanf than mine using cstring.find:

Code: Select all

///////////////////////////////////////////////////////////////////////////////////////////////////
//      Allow user to browse for a .mtn file
//   Split the whole file up into separate motions
void OBJ_RobotisMotion::ParseMtnFile(void)
{
   CString sMtnFile;
   CFile F;
   CFileException E;
   ULONGLONG TotalBytesRead = 0;
   if(F.Open(v.sMtnFilePath, CFile::modeRead|CFile::typeBinary, &E))
   {
      int iLength = (int)F.GetLength();
      char* pBuf = sMtnFile.GetBufferSetLength(iLength);
      TotalBytesRead = F.Read(pBuf, (UINT)iLength);
      F.Close();
   }
   else
      return;

   v.iNumMotions = 0;
   v.iNumNoNames = 0;
   bool bFinished = false;
   int iFind1 = 0;
   int iFind2 = 0;

   //*//////////////////////////////////////////////////////////////////////////////////////////////////
   //enable=
   iFind1 = sMtnFile.Find("enable=", iFind1);
   if(iFind1 == -1)
   {
      return;
   }
   else
   {   //got "enable="
      iFind1 += strlen("enable=");
      //store compliances
      for(int i = 0; i < NUM_SERVOS; i++)
      {
         if(i < (NUM_SERVOS-1))
            iFind2 = sMtnFile.Find(0x20, iFind1);
         else
            iFind2 = sMtnFile.Find(0x0A, iFind1);
         if(iFind2 == -1)
         {
            return;
         }
         else
         {   //got 0x20 or 0x0A
            CString sVal = sMtnFile.Mid(iFind1, iFind2-iFind1);
            iFind1 = iFind2 + 1;
            if(sVal == "1")
               v.bServoEnabled[i] = true;
            else
               v.bServoEnabled[i] = false;
         }
      }
   }//*/
   while(!bFinished)
   {
      iFind1 = sMtnFile.Find("page_begin", iFind1);
      if(iFind1 == -1)
      {
         bFinished = true;
      }
      else
      {   //got "page_begin"
         iFind1 += strlen("page_begin");
         iFind2 = sMtnFile.Find("page_end", iFind1);
         if(iFind2 == -1)
         {
            bFinished = true;
         }
         else
         { //got "page_end"
            CString sMtn = sMtnFile.Mid(iFind1, iFind2-iFind1);
            iFind2 += strlen("page_end");
            ParseMtn(sMtn);
         }
      }
   }
}
///////////////////////////////////////////////////////////////////////////////////////////////////
// Parse a single motion
void OBJ_RobotisMotion::ParseMtn(CString sMtn)
{
   int iFind1 = 0;
   int iFind2 = 0;

   //*/////////////////////////////////////////////////////////////////////////////////////////////////
   //name=
   iFind1 = sMtn.Find("name=", iFind1);
   if(iFind1 == -1)
   {
      return;
   }
   else
   {   //got "name="
      iFind1 += strlen("name=");
      iFind2 = sMtn.Find(0x0A, iFind1);
      if(iFind2 == -1)
      {
         return;
      }
      else
      {   //got 0x0A
         v.aMotion[v.iNumMotions].sName = sMtn.Mid(iFind1, iFind2-iFind1);
         if(v.aMotion[v.iNumMotions].sName.IsEmpty())
         {
            return;
            v.aMotion[v.iNumMotions].sName.Format("NoName %03d", ++v.iNumNoNames);
         }
         iFind1 = iFind2 + 1;
      }
   }//*/
   //*//////////////////////////////////////////////////////////////////////////////////////////////////
   //compliance=
   iFind1 = sMtn.Find("compliance=", iFind1);
   if(iFind1 == -1)
   {
      return;
   }
   else
   {   //got "compliance="
      iFind1 += strlen("compliance=");
      //store compliances
      for(int i = 0; i < NUM_SERVOS; i++)
      {
         if(i<(NUM_SERVOS-1))
            iFind2 = sMtn.Find(0x20, iFind1);
         else
            iFind2 = sMtn.Find(0x0A, iFind1);
         if(iFind2 == -1)
         {
            return;
         }
         else
         {   //got 0x20 or 0x0A
            CString sVal = sMtn.Mid(iFind1, iFind2-iFind1);
            iFind1 = iFind2 + 1;
            v.aMotion[v.iNumMotions].aucCompliance[i] = atoi(sVal);
         }
      }
   }//*/
   //*//////////////////////////////////////////////////////////////////////////////////////////////////
   //play_param=
   iFind1 = sMtn.Find("play_param=", iFind1);
   if(iFind1 == -1)
   {
      return;
   }
   else
   {   //got "play_param="
      iFind1 += strlen("play_param=");
      //store play params
      const int NUM_PLAY_PARAMS = 5;
      for(int i = 0; i < NUM_PLAY_PARAMS; i++)
      {
         if(i<NUM_PLAY_PARAMS> -1)
      {   //got "step="
         iFind1 += strlen("step=");
         //store vals
         for(int i = 0; i < NUM_VALS; i++)
         {
            if(i<(NUM_VALS-1))
               iFind2 = sMtn.Find(0x20, iFind1);
            else
               iFind2 = sMtn.Find(0x0A, iFind1);
            if(iFind2 == -1)
            {
               return;
            }
            else
            {   //got 0x20 or 0x0A
               CString sVal = sMtn.Mid(iFind1, iFind2-iFind1);
               iFind1 = iFind2 + 1;
               if(i < NUM_SERVOS)
               {
                  v.aMotion[v.iNumMotions].aPose[v.aMotion[v.iNumMotions].ucNumPoses].ausJointTarget[i] = atoi(sVal);
               }
               else if(i == NUM_SERVOS)
               {
                  v.aMotion[v.iNumMotions].aPose[v.aMotion[v.iNumMotions].ucNumPoses].usPauseMillis = (unsigned short)(((float)atof(sVal))*1000);
               }
               else if(i == NUM_SERVOS+1)
               {
                  v.aMotion[v.iNumMotions].aPose[v.aMotion[v.iNumMotions].ucNumPoses].usTimeMillis = (unsigned short)(((float)atof(sVal))*1000);
                  v.aMotion[v.iNumMotions].ucNumPoses++;
               }
            }
         }
      }
      else
      {
         v.iNumMotions++;
         return;
      }
   }//*/
}


[MOHIT JINDAL]

Did anyone of you tried to control Dynamixels with pc using Usb2Dynamixel in visual Basic or Visual c++

[Fraser]

Hi,

I control my servos with visual c++ through the USB2Dynamixel. But I am using my own custom firmware, more info here:

http://www.ledset.com/bioloid/axons/

Be careful with axons though, it is a raw low level interface to the servo motor H-bridges and position sensors, there are no torque safety limits in place yet, a small code error can cause complete joint crashes and you can't use it with existing robotis tools. So you may want to stick with the standard Robotis firmware and use the dynamixel sdk from robotis to send commands to your servos, the SDK should have instructions for using it included:

http://support.robotis.com/en/software/dynamixel_sdk/usb2dynamixel/usb2dxl_windows.htm

[siempre.aprendiendo]

Did anyone of you tried to control Dynamixels with pc using Usb2Dynamixel in visual Basic or Visual c++

MOHIT, have you visited http://support.robotis.com/en ?

Yo have a lot of info about using usb2dynamixel with visual c++ (well, really it's more c than c++) at

http://support.robotis.com/en/software/ ... al_c++.htm

[Fraser]

Well I got my .mtn file parser running into the closed loop code. There simply isn't enough joint stifness in the ankles and knees to allow most of the moves to operate correctly. I could probably correct for the sag using offsets into those joints but I will back burner that idea for a while.

The stable moves like clap, bravo, rap chest etc all work fine and with the fairly low latency they are smooth without jitter or visible steps. I am interpolating into 10ms steps, so a 1 second move gets broken down into 100 small moves etc.

So without position hold in the servo itself it may be impossible to get sufficient stiffness to run the stock walking gait .mtn. I have tried turning the gain up in my control code on the PC and also truncating the PWM dead zones to remove springiness but too much of this causes oscillation about the hold angle.

I'm actually having a hard time finding the stock walk motion, I think maybe they are split up into cryptically named motions within the file: bio_prm_humanoidtypea_en.mtn ?

I do however reckon it will be possible to evolve some kind of walking gait using sinusoids with correct amplitude, phase, offset, global frequency (and possibly top truncation) to the different servos. Of course I would only need to adjust one side of the humanoid and the other side can be derived from it fairly easily so I may give it a shot using a GUI to control 9 servos down one side, that means I have to tweak at least 27+1 slider controls until it walks, hopefully the exploration of this 28 dimension space will be fruitful. And then I might have a programmatic walking gait as a foundation for further experimentation.

At first i thought I could get away with just the legs but I'm just realising how inportant arms are for faster walking gaits, they give you a small correction envelope (action reaction) swing an arm one way and the bot swings a little the other (common knowledge to most of you guys already).

P.S.
Does anyone have a real stable and quite slow walking gait for humanoidA in the form of a .mtn file that I could feed into this? I would youtube a vid showing how it handles it.

[Fraser]

with the standard bioloid premium gyro unit i am getting 7 bits of useful data.

So this is like 64 being middle (no rocking), with 0 and 128 as extreme rockings.

robot stationary gives noise only on the lowest bit. so 6 bits are noise free.

what are others experience with the gyro, better, worse?

[migueluli]

Hi Fraser, first of all congratulations for your nice work.

I'd like to ask you about the role of the CM-510 in your control scheme. I just bought a Bioloid Beginner to Comprehensive Upgrade Kit because I didn't really want to use the CM-5 controller and I found the price/servo_number ratio to be the best I could find. I'm planning on controlling the servos directly from the PC with USB2Dynamixel (probably using ROS on Ubuntu) and trying to 'hack' some kind of speed control, so I can work on differential IK methods. What I've seen so far about your work seems like a good starting point to achieve my goals, despite not being there yet.

Do you think I can come close to the rates you're obtaining with your custom firmwares but without using the intermediate CM controller? I'm guessing that your CM controller firmware is probably in charge of splitting/joining the packets from the PC/servos to the servos/PC respectively, thus reducing latency from multiple USB write/reads on the PC. Am I right?

I must admit that I haven't even been able to try the standard firmware yet since I still have to buy a power supply for using the USB2Dynamixel. Maybe USB2Dynemixel SDK gives me high enough loop frequency to achieve some reasonable speed-control-like scheme (integrating speed into appropriate position commands) for the 14 servos. Do you think I should try this method?

Also, I was thinking of adding some controller in the future, something like a RoBoard or similar, which I guess will improve the communication latency when compared to using a desktop PC and USB port. Would you suggest going for this solution right from the beginning?

I might be talking nonsense here, but this low level communication stuff is not really one of my strengths. I'm more of a math guy

[Fraser]

Hi Miguel, Thanks.

Hi Fraser, first of all congratulations for your nice work.

I'd like to ask you about the role of the CM-510 in your control scheme. I just bought a Bioloid Beginner to Comprehensive Upgrade Kit because I didn't really want to use the CM-5 controller and I found the price/servo_number ratio to be the best I could find. I'm planning on controlling the servos directly from the PC with USB2Dynamixel (probably using ROS on Ubuntu) and trying to 'hack' some kind of speed control, so I can work on differential IK methods. What I've seen so far about your work seems like a good starting point to achieve my goals, despite not being there yet.

Buying AX12 servos was a good choice I think. They have good torque and the build quality seems very good for the price. I think you will be able to achieve what you wish with them.

Do you think I can come close to the rates you're obtaining with your custom firmwares but without using the intermediate CM controller? I'm guessing that your CM controller firmware is probably in charge of splitting/joining the packets from the PC/servos to the servos/PC respectively, thus reducing latency from multiple USB write/reads on the PC. Am I right?

The CM510 controller is only in the loop for me because I am working on having IMU data at the tail of the reply packet. As far as comms are concerned it is acting as a fairly transparent bridge.

You will be pleased to know that my firmware (Axons) will work fine using just a string of AX12 servos and no controller, they work together to make a seamless reply packet containing all the position datas. You should be able to easily exceed my current rate of 100Hz over 250kbps serial. Using ROS you will probably get well over 150Hz with lower latency.

I must admit that I haven't even been able to try the standard firmware yet since I still have to buy a power supply for using the USB2Dynamixel. Maybe USB2Dynemixel SDK gives me high enough loop frequency to achieve some reasonable speed-control-like scheme (integrating speed into appropriate position commands) for the 14 servos. Do you think I should try this method?

For servo only operation I use my CM510 to power the bus, I have a special lead with the TTL signal wire cut. You have no CM510 yet so you'll need a PSU like you say. There is no harm in trying robotis firmware. You can get some pretty smooth IK stuff running by issuing synch write commands. But you will enjoy the freedom of closing the loop on a main controller, unlimited freedom to explore any control scheme you can imagine.

Also, I was thinking of adding some controller in the future, something like a RoBoard or similar, which I guess will improve the communication latency when compared to using a desktop PC and USB port. Would you suggest going for this solution right from the beginning?

As you already have a USB2Dynamixel I would suggest getting a string of AX12s hooked up to it and playing with your desktop computer first. If you wish then I can post links to C++ classes for interfacing to my firmware, they are nicely object orientated with simple interfaces exposed to the programmer. But they will need a little porting work for linux, specifically you will have to change the serialread write commands to some standard linux stuff. Then when you upgrade to a Roboard etc you can quickly slide all your existing code over.

I might be talking nonsense here, but this low level communication stuff is not really one of my strengths. I'm more of a math guy

No nosense received at my end lol, all sound questions. Low level comms are a pain to debug for anybody on a multidrop bus system like this, my perserverence was stretched to the limits in the latter stages of debug, every small code change required rebuilding and programming all 18 servos with the new code.

If you know some C then with your math knowledge you will have great fun playing with Axons, the servos will be under your full command and with some fancy math could be made to do some impressive stuff.

If you need any info on installing Axons into your servos then just let me know, The servo firmware as it stands seems very stable, I have since improved the CM510 firmware and will release a better version soon, but the AX12 firmwares will probably be left as is for a while. Also it is quite painless to change firmware from and to robotis original as you require without risk of bricking your servos.

P.S.
You can try axons using just one servo first, do not put physical limit on the servo to start with (just use a single free standing servo).

[migueluli]

Thanks for your thorough reply Fraser.

I think I'll first try woking with the custom firmware for a while as you suggest, and see its limitations.

I also liked the idea of trying the Axon fw on just one servo first to see how it behaves. In this respect it'd be nice to have your PC interface classes to help me start. If I finally end up using your fw I'll probably code a ROS node to interface with the servos, which I'll be glad to contribute back to the community.

Unfortunately my other duties (job, studies) won't let me progress very fast so don't expect great advances soon

I'd still like to know your opinion on one more thing. If I understood well, when the Axon firmware is being used on a servo, the command the servo receives is the voltage level (PWM) to be applied to the motor inside, which should be proportional to speed under a constant load. However on long kinematic chains (e.g. robot arm) the load is far from being constant and a feedback loop must be closed somewhere if speed control is to be achieved. If this is done on the PC with a 100Hz rate for the PWM loop, then the speed control loop should work at somewhat slower rates, say 10-20Hz. In that case, wouldn't it be better to run the PWM control loop directly on the servo board at higher than 100Hz rates and then be able to control the speed at 100Hz? I'm assuming here that the bottleneck for the 100Hz rate is the communication bus and that the electronics on the servo are able to run much faster, which might not be true at all.