Code for Recording and Replaying Gestures Very Smoothly

Technology
1

I thought I would share some of my work here to help anyone on the forums looking to make something similar. I create an extremely smooth way of recording and replaying movements down to the smallest possible resolutions in time. For example, you can record 5 seconds of movements by physically moving a non-compliant poppy by taking position snapshots every 200ms.

The trick is that when you replay the positions, to make it really smooth, you need to dynamically calculate the required moving speed from each position every 200ms. Otherwise, the robot will move at a fixed speed from each recorded snapshot to the other.

So this is brought to you mainly by this function:

int calculateMXMotorSpeedBasedonRotationandTime(int stepsToMoveinDynamixelUnits , int amountofTimeToReachDesiredPositioninMilliSeconds)
{
    /*
    Dynamixel MX28AT and MX64AT has set position 0 to 4095 (0xFFF) is available.The unit is 0.088 degree.
    This allows us to calculate the desired speed  if we know the currect position, and the new position to go to.
    This will allow for smoother movements.
    Moving speed can be 0~1023 (0X3FF) can be used, and the unit is about 0.114rpm = 0.0000019 rpms
    meaning at 1023, we have around 116.622rpm
    speed = distance / time(simulationResolutionInMilliSeconds)
    speed here will be in revolutions per millisecond
    To translate this to  0~1023 speed value for the dynamixel motors we just divide that speed with rpms
    speed = ((((position_to_go - previous_position)*0.088)/360) / simulationResolutionInMilliSeconds)) /  0.0000019 rpms)
    Note: dynamixel reports 0.088 but this is not correct. becuase it should be more like 360 / 4095 = 0.08791

    note we are using abs() to negate any negative number since the position difference we receive can be positive or negative depending if cc or ccw desired rotation
    */
    return abs((((stepsToMoveinDynamixelUnits*0.08791) / 360) / amountofTimeToReachDesiredPositioninMilliSeconds) / 0.0000019);
}

int calculateAXMotorSpeedBasedonRotationandTime(int stepsToMoveinDynamixelUnits, int amountofTimeToReachDesiredPositioninMilliSeconds)
{
    /*
    Dynamixel AX12 has set position 0 to 1023 (0x3FF) is available.  The unit is 0.29 degree. Also the maximum rotation is 300 degree
    0~1023 (0X3FF) can be used, and the unit is about 0.111rpm = 0.00000185 rpms
    Formula will be
    speed = ((((position_to_go - previous_position)*0.29)/300) / simulationResolutionInMilliSeconds)) /  0.00000185 rpms)

    Note: dynamixel reports 0.29 but this is not correct. becuase it should be more like 300 / 1023 = 0.2932
    */

    return abs((((stepsToMoveinDynamixelUnits*0.2932) / 300) / amountofTimeToReachDesiredPositioninMilliSeconds) / 0.00000185);
}

Here is my code in c++ to record movements:

bool RobotStructure::recordPositionSequence(int timeToRecordInMilliSeconds, int recordingResolutionInMilliSeconds, string fileNameToSave, string forMotors)
{
ofstream fileToWrite(fileNameToSave);
if (fileToWrite.is_open())
{
fileToWrite << “Recording Time in ms:\n”;
fileToWrite << timeToRecordInMilliSeconds;
fileToWrite << “\n”;
fileToWrite << “Recording Resolution in ms:\n”;
fileToWrite << recordingResolutionInMilliSeconds ;
fileToWrite << “\n”;
}
else
{
return false;
}

std::vector<string> vect = getSubStrings(forMotors, ',');

if (vect.size() < 1)
    return false;

steady_clock::time_point startTimePoint = steady_clock::now();

while ((duration_cast<std::chrono::milliseconds>(steady_clock::now() - startTimePoint).count())< timeToRecordInMilliSeconds)
{
    steady_clock::time_point TimeToSetCurrentParams = steady_clock::now();

    setCurrentParameters(); 

    fileToWrite << "****SIMULATION RUN*****" << "\n";
    setCurrentParameters();
    for (int i = 0; i < vect.size(); i++)
    {
        cout << "STRING PROCESSING****" << endl;
        cout << vect.at(i) << endl;

        if (vect.at(i) == "HeadY")
        {
            fileToWrite << "HeadY" << "\n";
            fileToWrite << Neck->headY->position << "\n";
        }
        else if (vect.at(i) == "HeadZ")
        {
            fileToWrite << "HeadZ" << "\n";
            fileToWrite << Neck->headZ->position << "\n";
        }
        else if (vect.at(i) == "RightShoulderY")
        {
            fileToWrite << "RightShoulderY" << "\n";
            fileToWrite << RightArm->ShoulderY->position << "\n";
        }
        else if (vect.at(i) == "RightShoulderX")
        {
            fileToWrite << "RightShoulderX" << "\n";
            fileToWrite << RightArm->ShoulderX->position << "\n";
        }
        else if (vect.at(i) == "RightArmZ")
        {
            fileToWrite << "RightArmZ" << "\n";
            fileToWrite << RightArm->ArmZ->position << "\n";
        }
        else if (vect.at(i) == "RightElbowY")
        {
            fileToWrite << "RightElbowY" << "\n";
            fileToWrite << RightArm->ElbowY->position << "\n";
        }
        else if (vect.at(i) == "LeftShoulderY")
        {
            fileToWrite << "LeftShoulderY" << "\n";
            fileToWrite << LeftArm->ShoulderY->position << "\n";
        }
        else if (vect.at(i) == "LeftShoulderX")
        {
            fileToWrite << "LeftShoulderX" << "\n";
            fileToWrite << LeftArm->ShoulderX->position << "\n";
        }
        else if (vect.at(i) == "LeftArmZ")
        {
            fileToWrite << "LeftArmZ" << "\n";
            fileToWrite << LeftArm->ArmZ->position << "\n";
        }
        else if (vect.at(i) == "LeftElbowY")
        {
            fileToWrite << "LeftElbowY" << "\n";
            fileToWrite << LeftArm->ElbowY->position << "\n";
        }
        else if (vect.at(i) == "BustX")
        {
            fileToWrite << "BustX" << "\n";
            fileToWrite << Torso->bustX->position << "\n";
        }
        else if (vect.at(i) == "BustY")
        {
            fileToWrite << "BustY" << "\n";
            fileToWrite << Torso->bustY->position << "\n";
        }
        else if (vect.at(i) == "AbsZ")
        {
            fileToWrite << "AbsZ" << "\n";
            fileToWrite << Torso->absZ->position << "\n";
        }
        else if (vect.at(i) == "AbsX")
        {
            fileToWrite << "AbsX" << "\n";
            fileToWrite << Torso->absX->position << "\n";
        }
        else if (vect.at(i) == "AbsY")
        {
            fileToWrite << "AbsY" << "\n";
            fileToWrite << Torso->absY->position << "\n";
        }
        else if (vect.at(i) == "RightHipX")
        {
            fileToWrite << "RightHipX" << "\n";
            fileToWrite << RightLeg->hipX->position << "\n";
        }
        else if (vect.at(i) == "RightHipZ")
        {
            fileToWrite << "RightHipZ" << "\n";
            fileToWrite << RightLeg->hipZ->position << "\n";
        }
        else if (vect.at(i) == "RightHipY")
        {
            fileToWrite << "RightHipY" << "\n";
            fileToWrite << RightLeg->hipY->position << "\n";
        }
        else if (vect.at(i) == "RightKneeY")
        {
            fileToWrite << "RightKneeY" << "\n";
            fileToWrite << RightLeg->kneeY->position << "\n";
        }
        else if (vect.at(i) == "RightAnkleY")
        {
            fileToWrite << "RightAnkleY" << "\n";
            fileToWrite << RightLeg->ankleY->position << "\n";
        }
        else if (vect.at(i) == "LeftHipX")
        {
            fileToWrite << "LeftHipX" << "\n";
            fileToWrite << LeftLeg->hipX->position << "\n";
        }
        else if (vect.at(i) == "LeftHipZ")
        {
            fileToWrite << "LeftHipZ" << "\n";
            fileToWrite << LeftLeg->hipZ->position << "\n";
        }
        else if (vect.at(i) == "LeftHipY")
        {
            fileToWrite << "LeftHipY" << "\n";
            fileToWrite << LeftLeg->hipY->position << "\n";
        }
        else if (vect.at(i) == "LeftKneeY")
        {
            fileToWrite << "LeftKneeY" << "\n";
            fileToWrite << LeftLeg->kneeY->position << "\n";
        }
        else if (vect.at(i) == "LeftAnkleY")
        {
            fileToWrite << "LeftAnkleY" << "\n";
            fileToWrite << LeftLeg->ankleY->position << "\n";
        }
    }

    int durationToSetCurrentParamsandWriteToFile = duration_cast<std::chrono::milliseconds>(steady_clock::now() - TimeToSetCurrentParams).count();
    cout << "Simulation Run In: " << durationToSetCurrentParamsandWriteToFile << endl;

    int timeToSleep = recordingResolutionInMilliSeconds - durationToSetCurrentParamsandWriteToFile;
    cout << "sleeping for: " << timeToSleep << endl;
    std::this_thread::sleep_for(std::chrono::milliseconds(timeToSleep));
}
fileToWrite.close();
return true;

}
Here is my code to replay the movements, notice that you need to store the previous position and calculate the required speed to be moved on each recorded resolution based on that.

bool RobotStructure::playPositionSequence(string fileNameToRead, bool includeHead)
{
setCurrentParameters();
int previousPosition_headY = Neck->headY->position;
int previousPosition_headZ = Neck->headZ->position;
int previousPosition_RightShoulderY = RightArm->ShoulderY->position;
int previousPosition_RightShoulderX = RightArm->ShoulderX->position;
int previousPosition_RightArmZ = RightArm->ArmZ->position;
int previousPosition_RightElbowY = RightArm->ElbowY->position;
int previousPosition_LeftShoulderY = LeftArm->ShoulderY->position;
int previousPosition_LeftShoulderX = LeftArm->ShoulderX->position;
int previousPosition_LeftArmZ = LeftArm->ArmZ->position;
int previousPosition_LeftElbowY = LeftArm->ElbowY->position;
int previousPosition_bustX = Torso->bustX->position;
int previousPosition_bustY = Torso->bustY->position;
int previousPosition_absZ = Torso->absZ->position;
int previousPosition_absX = Torso->absX->position;
int previousPosition_absY = Torso->absY->position;
int previousPosition_RighthipX = RightLeg->hipX->position;
int previousPosition_RighthipZ = RightLeg->hipZ->position;
int previousPosition_RighthipY = RightLeg->hipY->position;
int previousPosition_RightkneeY = RightLeg->kneeY->position;
int previousPosition_RightankleY = RightLeg->ankleY->position;
int previousPosition_LefthipX = LeftLeg->hipX->position;
int previousPosition_LefthipZ = LeftLeg->hipZ->position;
int previousPosition_LefthipY = LeftLeg->hipY->position;
int previousPosition_LeftkneeY = LeftLeg->kneeY->position;
int previousPosition_LeftankleY = LeftLeg->ankleY->position;

int timeToRunSimulationInMilliSeconds;
int simulationResolutionInMilliSeconds;

string readLine;
ifstream fileToRead(fileNameToRead);

if (fileToRead.is_open())
{
    getline(fileToRead, readLine);
    getline(fileToRead, readLine);
    timeToRunSimulationInMilliSeconds = stoi(readLine);
    getline(fileToRead, readLine);
    getline(fileToRead, readLine);
    simulationResolutionInMilliSeconds = stoi(readLine);
    getline(fileToRead, readLine); //this is to skip the first "****SIMULATION RUN*****"
}
else
{
    return false;
}

steady_clock::time_point startTimePoint = steady_clock::now();
steady_clock::time_point TimeToRunSimulation = steady_clock::now();

while ((duration_cast<std::chrono::milliseconds>(steady_clock::now() - startTimePoint).count()) < timeToRunSimulationInMilliSeconds)
{

    if (getline(fileToRead, readLine))
    {
        cout << "READING.." << readLine << endl;

        if (readLine == "****SIMULATION RUN*****")
        {
            int durationToSetCurrentParamsandWriteToFile = duration_cast<std::chrono::milliseconds>(steady_clock::now() - TimeToRunSimulation).count();
            cout << "Simulation Run In: " << durationToSetCurrentParamsandWriteToFile << endl;

            int timeToSleep = simulationResolutionInMilliSeconds - durationToSetCurrentParamsandWriteToFile;
            std::this_thread::sleep_for(std::chrono::milliseconds(timeToSleep));

            TimeToRunSimulation = steady_clock::now();
        }
        else if (readLine == "HeadY" && includeHead)
        {
            getline(fileToRead, readLine);
            Neck->headY->setPosition(stoi(readLine), calculateAXMotorSpeedBasedonRotationandTime((stoi(readLine) - previousPosition_headY), simulationResolutionInMilliSeconds));
            previousPosition_headY = stoi(readLine);
        }
        else if (readLine == "HeadZ" && includeHead)
        {
            getline(fileToRead, readLine);
            Neck->headZ->setPosition(stoi(readLine), calculateAXMotorSpeedBasedonRotationandTime((stoi(readLine) - previousPosition_headZ), simulationResolutionInMilliSeconds));
            previousPosition_headZ = stoi(readLine);
        }
        else if (readLine == "RightShoulderY")
        {
            getline(fileToRead, readLine);

            RightArm->ShoulderY->setPosition(stoi(readLine), calculateMXMotorSpeedBasedonRotationandTime((stoi(readLine) - previousPosition_RightShoulderY), simulationResolutionInMilliSeconds));
            previousPosition_RightShoulderY = stoi(readLine);
        }
        else if (readLine == "RightShoulderX")
        {
            getline(fileToRead, readLine);
            RightArm->ShoulderX->setPosition(stoi(readLine),  calculateMXMotorSpeedBasedonRotationandTime((stoi(readLine) - previousPosition_RightShoulderX),simulationResolutionInMilliSeconds));
            previousPosition_RightShoulderX = stoi(readLine);
        }
        else if (readLine == "RightArmZ")
        {
            getline(fileToRead, readLine);
            RightArm->ArmZ->setPosition(stoi(readLine), calculateMXMotorSpeedBasedonRotationandTime((stoi(readLine) - previousPosition_RightArmZ), simulationResolutionInMilliSeconds));
            previousPosition_RightArmZ = stoi(readLine);
        }
        else if (readLine == "RightElbowY")
        {
            getline(fileToRead, readLine);
            RightArm->ElbowY->setPosition(stoi(readLine), calculateMXMotorSpeedBasedonRotationandTime((stoi(readLine) - previousPosition_RightElbowY), simulationResolutionInMilliSeconds));
            previousPosition_RightElbowY = stoi(readLine);
        }
        else if (readLine == "LeftShoulderY")
        {
            getline(fileToRead, readLine);
            LeftArm->ShoulderY->setPosition(stoi(readLine), calculateMXMotorSpeedBasedonRotationandTime((stoi(readLine) - previousPosition_LeftShoulderY), simulationResolutionInMilliSeconds));
            previousPosition_LeftShoulderY = stoi(readLine);
        }
        else if (readLine == "LeftShoulderX")
        {
            getline(fileToRead, readLine);
            LeftArm->ShoulderX->setPosition(stoi(readLine), calculateMXMotorSpeedBasedonRotationandTime((stoi(readLine) - previousPosition_LeftShoulderX), simulationResolutionInMilliSeconds));
            previousPosition_LeftShoulderX = stoi(readLine);
        }
        else if (readLine == "LeftArmZ")
        {
            getline(fileToRead, readLine);
            LeftArm->ArmZ->setPosition(stoi(readLine), calculateMXMotorSpeedBasedonRotationandTime((stoi(readLine) - previousPosition_LeftArmZ), simulationResolutionInMilliSeconds));
            previousPosition_LeftArmZ = stoi(readLine);
        }
        else if (readLine == "LeftElbowY")
        {
            getline(fileToRead, readLine);
            LeftArm->ElbowY->setPosition(stoi(readLine), calculateMXMotorSpeedBasedonRotationandTime((stoi(readLine) - previousPosition_LeftElbowY), simulationResolutionInMilliSeconds));
            previousPosition_LeftElbowY = stoi(readLine);
        }
        else if (readLine == "BustX")
        {
            getline(fileToRead, readLine);
            Torso->bustX->setPosition(stoi(readLine), calculateMXMotorSpeedBasedonRotationandTime((stoi(readLine) - previousPosition_bustX), simulationResolutionInMilliSeconds));
            previousPosition_bustX = stoi(readLine);
        }
        else if (readLine == "BustY")
        {
            getline(fileToRead, readLine);
            Torso->bustY->setPosition(stoi(readLine), calculateMXMotorSpeedBasedonRotationandTime((stoi(readLine) - previousPosition_bustY), simulationResolutionInMilliSeconds));
            previousPosition_bustY = stoi(readLine);
        }
        else if (readLine == "AbsZ")
        {
            getline(fileToRead, readLine);
            Torso->absZ->setPosition(stoi(readLine), calculateMXMotorSpeedBasedonRotationandTime((stoi(readLine) - previousPosition_absZ), simulationResolutionInMilliSeconds));
            previousPosition_absZ = stoi(readLine);
        }
        else if (readLine == "AbsX")
        {
            getline(fileToRead, readLine);
            Torso->absX->setPosition(stoi(readLine), calculateMXMotorSpeedBasedonRotationandTime((stoi(readLine) - previousPosition_absX), simulationResolutionInMilliSeconds));
            previousPosition_absX = stoi(readLine);
        }
        else if (readLine == "AbsY")
        {
            getline(fileToRead, readLine);
            Torso->absY->setPosition(stoi(readLine), calculateMXMotorSpeedBasedonRotationandTime((stoi(readLine) - previousPosition_absY), simulationResolutionInMilliSeconds));
            previousPosition_absY = stoi(readLine);
        }
        else if (readLine == "RightHipX")
        {
            getline(fileToRead, readLine);
            RightLeg->hipX->setPosition(stoi(readLine), calculateMXMotorSpeedBasedonRotationandTime((stoi(readLine) - previousPosition_RighthipX), simulationResolutionInMilliSeconds));
            previousPosition_RighthipX = stoi(readLine);
        }
        else if (readLine == "RightHipZ")
        {
            getline(fileToRead, readLine);
            RightLeg->hipZ->setPosition(stoi(readLine), calculateMXMotorSpeedBasedonRotationandTime((stoi(readLine) - previousPosition_RighthipZ), simulationResolutionInMilliSeconds));
            previousPosition_RighthipZ = stoi(readLine);
        }
        else if (readLine == "RightHipY")
        {
            getline(fileToRead, readLine);
            RightLeg->hipY->setPosition(stoi(readLine), calculateMXMotorSpeedBasedonRotationandTime((stoi(readLine) - previousPosition_RighthipY), simulationResolutionInMilliSeconds));
            previousPosition_RighthipY = stoi(readLine);
        }
        else if (readLine == "RightKneeY")
        {
            getline(fileToRead, readLine);
            RightLeg->kneeY->setPosition(stoi(readLine), calculateMXMotorSpeedBasedonRotationandTime((stoi(readLine) - previousPosition_RightkneeY), simulationResolutionInMilliSeconds));
            previousPosition_RightkneeY = stoi(readLine);
        }
        else if (readLine == "RightAnkleY")
        {
            getline(fileToRead, readLine);
            RightLeg->ankleY->setPosition(stoi(readLine), calculateMXMotorSpeedBasedonRotationandTime((stoi(readLine) - previousPosition_RightankleY), simulationResolutionInMilliSeconds));
            previousPosition_RightankleY = stoi(readLine);
        }
        else if (readLine == "LeftHipX")
        {
            getline(fileToRead, readLine);
            LeftLeg->hipX->setPosition(stoi(readLine), calculateMXMotorSpeedBasedonRotationandTime((stoi(readLine) - previousPosition_LefthipX), simulationResolutionInMilliSeconds));
            previousPosition_LefthipX = stoi(readLine);
        }
        else if (readLine == "LeftHipZ")
        {
            getline(fileToRead, readLine);
            LeftLeg->hipZ->setPosition(stoi(readLine), calculateMXMotorSpeedBasedonRotationandTime((stoi(readLine) - previousPosition_LefthipZ), simulationResolutionInMilliSeconds));
            previousPosition_LefthipZ = stoi(readLine);
        }
        else if (readLine == "LeftHipY")
        {
            getline(fileToRead, readLine);
            LeftLeg->hipY->setPosition(stoi(readLine), calculateMXMotorSpeedBasedonRotationandTime((stoi(readLine) - previousPosition_LefthipY), simulationResolutionInMilliSeconds));
            previousPosition_LefthipY = stoi(readLine);
        }
        else if (readLine == "LeftKneeY")
        {
            getline(fileToRead, readLine);
            LeftLeg->kneeY->setPosition(stoi(readLine), calculateMXMotorSpeedBasedonRotationandTime((stoi(readLine) - previousPosition_LeftkneeY), simulationResolutionInMilliSeconds));
            previousPosition_LeftkneeY = stoi(readLine);
        }
        else if (readLine == "LeftAnkleY")
        {
            getline(fileToRead, readLine);
            LeftLeg->ankleY->setPosition(stoi(readLine), calculateMXMotorSpeedBasedonRotationandTime((stoi(readLine) - previousPosition_LeftankleY), simulationResolutionInMilliSeconds));
            previousPosition_LeftankleY = stoi(readLine);
        }

    }
    else
    {
        fileToRead.close();
        return false;
    }
}
    
fileToRead.close();

send2WayData("127.0.0.1", 27005, "FINISHED_MOVING");
return true;

}

The result of all this, is extremely smooth movement that 100% replicates what you did when you physically created the gestures.

Here is a sample video that shows such smooth movements:

https://www.youtube.com/watch?v=uEac8mBLBhg

3 Likes
2

Thanks a lot Hadi for sharing your code with us. Indeed I think the smoothness you got is impressive !
However, why don’t you directly record the speed returned by the servomotor (and combine it in a frame including positions and speeds) rather to compute it at each motion steps ?

Extending your work to primitives motion, do you think that speed control of MX28 could be accurate enough to expect making the a plurality of motors working synchronously (I mean performing a uniform motion, each motor playing at the proper time) ? To be more clear, I try to get a perfectly linear motion with 5 MX28 and I have to admitt that I didn’t fully succeed to it. Speed control is (at least in my hands) not very accurate due to the lack of speed controller in the firmware of Dynamixels servos.
Your advise would be intresting.

3

Hi Bertrand,

Thanks for the reply. Sorry it took me a while to respond.

I did in fact face the problem of jittery movement when you add a lot of motors. But I did solve this in my code with these lines:

            int durationToSetCurrentParamsandWriteToFile = duration_cast<std::chrono::milliseconds>(steady_clock::now() - TimeToRunSimulation).count();
            cout << "Simulation Run In: " << durationToSetCurrentParamsandWriteToFile << endl;

            int timeToSleep = simulationResolutionInMilliSeconds - durationToSetCurrentParamsandWriteToFile;
            std::this_thread::sleep_for(std::chrono::milliseconds(timeToSleep));

Here is the explanation of this.

Everytime you run a round for example 200ms, you basically command a few motors to to a certain position, which these commands usually will take anywhere from 10ms to 50ms to execute depending on how many motors are included in the simulation.

If you ask the program to sleep for 200ms then the motors will jitter when running the simulation.

But if you wait for 200ms - (amount of time it takes to send commands to X motors). Then you will get a very accurate response and no jittery behavior.

I did run my code on 9 motors (all left arm, all right arm, and bustY) and it works perfectly.

Hope this helped!