Archives For Robots

VR Drone

April 5, 2016 — Leave a comment
drone2

VR Drone based on DJI S900 hexacopter. The camera is attached at the bottom.

 

I made this prototype in 2015 while exploring if it is possible to fuse Virtual Reality headsets and drones to make flying them easier. This is a drone (hexacopter to be precise) that has a custom made wide-angle stereo camera. It has two calibrated fisheye cameras in parallel setup with human like baseline (calibrating fisheye was a bit tricky). The camera images form two stereo hemispheres for left and right eyes. The cameras are 185 degrees and the stereo effect is good in 160 degrees range. There are some convergence issues on the sides, but it’s ok for peripheral vision.

StereoCamera

Stereo Camera With Tegra TK1

 

The camera streams real-time H264 stereo video feed at 30 fps over WiFi or 4G. The streaming and image processing is done on NVIDIA Tegra TK1 board. Big Thanks to NVIDIA for making such an awesome mobile computer! Tegra TX1 is even better now (in 2016).

The ground station receives the video feed and renders it in Oculus Rift DK2 at 75 fps with upsampling. The higher frame rate rendering is important for reducing VR sickness. In theory, one needs to stream 90 fps from the camera to avoid VR sickness, but in practice it consumes too much network bandwidth and is not practical (as of 20015-2016). Wireless streaming from a moving drone with existing protocols (over WiFi or 4G) proved to be a challenge. In addition to streaming issues, I had one nasty flyaway and one crash due to radio interferences between GPS and other antennas. I ended up adding some RF shielding (photos below). I guess, aluminum foil is critical tech in airspace.

TegraShieldedInTheBox

RF shielding

A drone operator has a 180 degrees immersive view with digital panning in the Virtual Reality headset and can fly the drone by controlling it. This project uses digital panning and not mechanical gimbals like other projects I looked at. Digital panning reduces VR sickness due to very little latency in viewing rendering (latency is <10ms when looking around). In some way, user experience is similar to a cinema where the video is 24fps, but looking around has much higher fps (human like fps).

Here is the flight video (with Oculus footage):

 

I tested this system with about 15 people. Test pilots could take off, fly and land with no issues while in VR. Users reported the sensation of flying and out of body experiences. About 70% of users did not feel issues with the motion/VR sickness due to the rendering/control schema used in this project. Usually, people experience less sickness when standing as opposed to seated flying.

 

 

 

 

 

 

 

Finally, I decided to do some inverse kinematics and control my assembled Bioloid humanoid “type A” from PC:

Bioloid Humanoid

Bioloid Humanoid Type A

Unfortunately, it turned out to be not so easy as I thought initially…

I read some online stuff from Robotis (their site lacks good documentation by the way) and expected that I can use USB2Dynamixel connector to connect my Bioloid to PC and then use Dynamixel SDK from Robotis to control dynamixels programmatically from a C++ program. After some trial and error and looking for answers online I found to my huge surprise that you cannot use Dynamixel SDK when the kit is connected to PC via CM-510 or CM-5 controller.

This  setup (which I think should be fairly popular): PC<->USB2Dynamixel<->CM-510<->Dynamixels  is NOT supported by Dynamixel SDK 😦   You can only use this SDK if you hook Dynamixels directly to USB2Dynamixel and connect them to the battery/power. I know that many “advanced” people do it this way, but I don’t like it when the most obvious setup is not supported out of the box.

Fortunately, after some searching I found how to do it yourself (no soldering required, only programming). It is a non-documented solution and it is sort of easy to do once you know how, but it is not easy to find good explanation about it and working code. So I decided to document it here step by step and provide a working C++ code sample for Windows (see below).

BIG thanks to Aprendiendo whos site contains the solution, but it is not complete for C++ if you run on Windows. So I re-wrote some of that code to make it a complete C++ code sample that you can download and run on Windows. This could save you some time if you want to do the same 🙂

So, to control your Bioloid from PC you need to:

1) Use USB2Dynamixel or other USB to Serial connector to connect CM-510 (or CM-5) controller via a serial cable to PC’s USB port. If you have USB2Dynamixel, then you have to switch it to RS232 mode (position #3)

2) Usually, USB2Dynamixel appears as COM3 port on your PC. On Windows, you have to open this port’s properties (in devices) and go to “Advanced” settings and set “Latency Timer” to 1 (msec) as described here

3) Power up your CM-510 (or CM-5). The “manage” mode indicator should start blinking. Press the “Start” button to start the “managed mode”. The indicator next to “manage” title should light up. If you skip this step, Bioloid’s controller is not going to be in the right state for controlling from PC.

4) Now, the most important part – the Bioloid controller (CM-510 or CM-5) needs to be switched to the “Toss mode“. In this mode, CM-510 firmware works as a bridge and forwards all commands it receives from PC to Dynamixels connected to it and also forwards all Dynamixel responses back to PC. This mode is not documented by Robotis by some reason. To switch to it you need to send “t\r” (‘t’ + 0x0D sequence) from your C/C++ program via a serial port to CM-510. See SendTossModeCommand() function that does it in the C++ code below.

5) After that, you can create Dynamixel protocol commands and send them to CM-510 via the serial port. Make sure that the program initializes the COM port communication speed at 57600, since this is the max speed that CM-5 and CM-510 support. See C++ code below for details.

NOTE 1: I found that on some PCs this process was not enough. After powering your Bioloid you need to run RoboPlus Manager from Robotis and connect the app to your Bioloid (press “connect” button) before controlling it from your PC program. The RoboPlus Manager does tons of obfuscated non documented communication and by some reason the “Toss mode” can be initialized only after that!

NOTE 2: “Toss mode” cannot be turned OFF (at least I don’t know how at the moment) and to turn it off you need to turn your Bioloid power OFF.

NOTE 3: I am still not sure if I can read sensors connected to CM-510 without custom firmware. See Aprendiendo website for more information about custom firmware to do this.

This BioloidAPI.ZIP file contains the C++ project for Windows (VS 2010 project) that demonstrates how to switch to the “toss mode” and how to send Dynamixel commands to move servos. I was trying to make it easy to understand. Here is most of the C++ code:


#include
#include
#include

class SerialPort
{
public:
    SerialPort();
    virtual ~SerialPort();

    HRESULT Open(const wchar_t* szPortName, DWORD baudRate);
    void Close();
    void Clear();

    HRESULT SendData(BYTE* pBuffer, unsigned long* pSize);
    HRESULT ReceiveData(BYTE* pBuffer, unsigned long* pSize);

private:
    HANDLE serialPortHandle;
};

SerialPort::SerialPort() :
serialPortHandle(INVALID_HANDLE_VALUE)
{
}

SerialPort::~SerialPort()
{
    Close();
}

HRESULT SerialPort::Open(const wchar_t* szPortName, DWORD baudRate)
{
    HRESULT hrResult = S_OK;
    DCB dcb;

    memset( &dcb, 0, sizeof(dcb) );

    dcb.DCBlength = sizeof(dcb);
    dcb.BaudRate = baudRate;
    dcb.Parity = NOPARITY;
    dcb.fParity = 0;
    dcb.StopBits = ONESTOPBIT;
    dcb.ByteSize = 8;

    serialPortHandle = CreateFile(szPortName, GENERIC_READ | GENERIC_WRITE, 0, NULL, OPEN_EXISTING, NULL, NULL);
    if ( serialPortHandle!=INVALID_HANDLE_VALUE )
    {
        if( !SetCommState(serialPortHandle, &dcb) )
        {
            hrResult = E_INVALIDARG;
            Close();
        }
    }
    else
    {
        hrResult = ERROR_OPEN_FAILED;
    }

    return hrResult;
}

void SerialPort::Close()
{
    if (serialPortHandle!=INVALID_HANDLE_VALUE || serialPortHandle!=NULL)
    {
        PurgeComm(serialPortHandle, PURGE_RXCLEAR | PURGE_TXCLEAR);
        CloseHandle(serialPortHandle);
    }
    serialPortHandle = INVALID_HANDLE_VALUE;
}

void SerialPort::Clear()
{
    if (serialPortHandle!=INVALID_HANDLE_VALUE || serialPortHandle!=NULL)
    {
        PurgeComm(serialPortHandle, PURGE_RXCLEAR | PURGE_TXCLEAR);
    }
}

HRESULT SerialPort::SendData(BYTE* pBuffer, unsigned long* pSize)
{
    HRESULT hrResult = ERROR_WRITE_FAULT;

    if (serialPortHandle!=INVALID_HANDLE_VALUE && serialPortHandle!=NULL)
    {
        if( WriteFile(serialPortHandle, pBuffer, *pSize, pSize, NULL) &&
            FlushFileBuffers(serialPortHandle)
            )
        {
            hrResult = S_OK;
        }
    }

    return hrResult;
}

HRESULT SerialPort::ReceiveData(BYTE* pBuffer, unsigned long* pSize)
{
    HRESULT hrResult = ERROR_READ_FAULT;

    if (serialPortHandle!=INVALID_HANDLE_VALUE && serialPortHandle!=NULL)
    {
        if( ReadFile(serialPortHandle, pBuffer, *pSize, pSize, NULL) )
        {
            hrResult = S_OK;
        }
    }

    return hrResult;
}

bool CreateAX12SetPositionCommand(BYTE id, short goal, BYTE* pBuffer, DWORD* pSize)
{
    const unsigned int packetSize = 9;

    if(*pSize < packetSize)     {         return false;     }     // PACKET STRUCTURE: OXFF 0XFF ID LENGTH INSTRUCTION PARAMETER_1 …PARAMETER_N CHECKSUM     *pSize = packetSize;     pBuffer[0] = 0xFF;     pBuffer[1] = 0xFF;     pBuffer[2] = id;     pBuffer[3] = 2 /* number of parameters */ + 3;	// packet body length     pBuffer[4] = 3;						// instruction id = write data     // Parameters     pBuffer[5] = 30;					// start address of position goal setting     pBuffer[6] = BYTE(goal);			// goal low byte (to address 30)     pBuffer[7] = BYTE(goal>>8);			// goal high byte (to address 31)

    // Checksum
    DWORD packetSum = 0;
    for(size_t i=2; iSendData(buffer, &size);
    if(FAILED(hr))
    {
        printf("Failed to send set dynamixel position command\n");
        return false;
    }
    Sleep(10);

    memset(buffer, 0, sizeof(buffer));
    size = sizeof(buffer);
    pSerialPort->ReceiveData(buffer, &size);

    if (size>4 && buffer[4] == 0)
    {
        printf("id=%d set to position=%d\n", id, position);
    }
    else
    {
        printf("Error while setting id=%d position=%d, error:%d\n", id, position, buffer[4]);
        return false;
    }

    return true;
}

bool SendTossModeCommand(SerialPort* pSerialPort)
{
    BYTE buffer[1024];
    buffer[0]='t';
    buffer[1]='\r';
    DWORD size = 2;

    HRESULT hr = pSerialPort->SendData(buffer, &size);
    if(FAILED(hr))
    {
        printf("Failed to send TOSS model command\n");
        return false;
    }
    Sleep(100);

    size = sizeof(buffer);
    pSerialPort->ReceiveData(buffer, &size);

    return true;
}

int _tmain(int argc, _TCHAR* argv[])
{
    DWORD baudRate = 57600;
    SerialPort comPort;

    HRESULT hr = comPort.Open(L"COM3", baudRate);
    if(FAILED(hr))
    {
        printf("Cannot open COM3 port\n");
        return 0;
    }

    SendTossModeCommand(&comPort);

    while(1)
    {
        printf( "Enter dynamixel ID and goal position:\n" );

        int id = 0;
        int position = 0;
        scanf("%d %d", &id, &position);

        SetDynamixelPosition(&comPort, id, position);

        printf("Press ESC to terminate, otherwise press any other key to continue\n");
        if(getch() == 0x1b)
        {
            break;
        }
    }

    comPort.Close();

    return 0;
}

Here is the walking quadruped robot that I made some time ago –

It turned out that 4 legged motion is pretty hard to get right. It took me 2 weeks to fix all the balancing and motion issues, so it can walk well. Turning left and right was especially challenging since it does not have enough levels of freedom in some directions. It uses 12 servos, but still can work pretty long time. It seems like Bioloid servos are pretty efficient. Makes lots of noise when walking though. Biolod coding language also does not scale well when you try to implement complex logic. I guess the best approach is to control the robot from a PC (or netbook) and use onboard controller only for low level motion control since it is pretty limited.

    Recently, I have been playing with a robotic kit – Bioloid from a Korean company Robotis (this kit has been around for quite some time, so this post may repeat well known facts). If you are interested in robotics, but do not want to deal with low level details, this kit might be for you. It is very easy to create robots with it – you only need a screwdriver and a PC. At the same time it is very flexible and allows creating a variety of robots – from simple manipulators to humanoids.

    Here is how this kit looks like when you buy it (this is a “comprehensive” version of the kit, the humanoid robot does not come pre-assembled):

Bioloid Comprehensive Kit

    It includes: 1 main controller, 18 servos, 1 sensor, wires and a variety of frames. Robotis calls their servos and sensors dynamixels. They all have the same unified casing (even sensors) and serve as basic construction blocks for robots. Dynamixels are wired together (and to the main controller) in a daisy chain fashion (see picture). This minimizes wiring and simplifies assembly.

Daisy Chain Link

Each dynamixel has a unique ID and when they are wired together they form a “chain of responsibility”, i.e. each dynamixel reacts only to commands that are sent to it even though they all listen to all commands on the wire. In addition to controlling servos, the main controller can read their parameters, e.g. current position, temperature, applied force, etc. Bioloid servos, sensors and frames are connected together with small nuts and bolts, which makes assembling a bit tricky.

     Bioloid comes with great software support. There are 2 main tools for programming a Bioloid robot – Motion Editor and Behavior Control Programmer. It is also possible to program in C if you want to have greater control.

    The Motion Editor allows recording basic joint movements. This process looks like making a cartoon animation – you move joints by hand and record snapshots in sequence. Then this sequence is given an ID and saved in the main controller, so it can be invoked later by a higher level behavioral program.

      Motion Editor 

    The Behavior Control Programmer allows writing higher level programs that either use pre-recorded motions or control servos directly. Bioloid has its own language that looks like BASIC. If you are familiar with basic programming you will have no problems coding in this language. Programs are event driven – they respond to events from sensors and launch different motions or procedures according to current state.

         Behavior Control Programmer

 

    Here is the video of a manipulator that I made with this kit. This simple robot detects objects in front of its sensor and tries to grab them. The claw and the sensor have some slack, so objects may be placed in a small area and not only in one spot:

It took me about 2 hours to assemble and to program it. I spent most of the time assembling it mechanically, because the kit has rather small nuts and bolts (at least for my hands) and it is pretty tricky to put nuts into their slots in dynamixels/frames. Programming was very easy and straightforward – I recorded several basic motions with the motion editor and then used them in the Behavior Control Programmer tool. It required very basic scripting skills and I think anyone can do it, but I am a software developer, so it could be easier for me than for others.

    Here are pros and cons of Bioloid kit that I can think of after playing with it for some time:

Pros:

  • Very flexible. Allows creating a variety of robots
  • Very easy to assemble and program robots
  • Motion Editor is great – it takes just a few minutes to create a very complex motion

Cons:

  • It includes only 1 sensor. It is infrared and very inaccurate. It would be great to have a set of different sensors including ultrasonic distance sensor.
  • Assembling/disassembling robots can be tricky due to very small nuts/bolts and not precise slots in frames.
  • It is easy to code a small program in the Behavior Control Programmer, but this tool and the Bioloid language does not scale well in terms of complexity – when you want to create something more complex, it becomes very hard to maintain and change control programs. It is almost impossible to code any AI algorithm of medium complexity (to be fair – you can program Bioloid in C, but in this case, the complexity goes through the roof and this kit loses its attractiveness)

Overall, I think it is a great educational kit/toy for people interested in robotics