View Full Version : [Question(s)] Generating a sine wave angular velocity motion with AX-12+

05-26-2012, 11:51 AM
Hi all,

I want to make the servo move back and forth in cyclic motion. This should generate a sine-wave like profile of the angular velocity around the axis of rotation of dynamixel ax-12+. I have an imu mounted on top of it which gives me the readings of angular velocity of this motion. For information, my code looks like this:





With this velocity and this delay of "usleep" I get an angular velocity profile that looks like this:
www.doc.ic.ac.uk/~ahanda/SineWave200.png. This is a near sine-wave but still not a very perfect sine-wave. However, if I increase the velocity from 200 to 500 or even more I see the kinks in the this wave and it starts looking like this www.doc.ic.ac.uk/~ahanda/SineWave500.png. This is because the servo can't keep that velocity till that goal position and due to inertia of motion there is a small backlash that is shown in the from of kinks in the graph. Does that mean I need to write a software PID control to make sure that I set the velocity according to the goal position? I want to have a continuous back and forth rotation as fast as I can without intervals of it slowing down at the end points. Does endless-run allow me to do that over a given angular-position slab? The problem with this code is that the servo doesn't necessarily reach the goal position and it gets the command to move backwards immediately (increasing the usleep decreases the frequency of motion)I did this only to make sure that I don't see the regions of servo moving with small velocity (near to no motion!) as I want it to move back and forth continuously. Simply polling for whether the goal position has been reached is also leading to slowing of servo near to goal position but I want to still make sure that I observe as small a region of small velocity as possible. I may be missing something in how servo works and how to make servo go cyclically back and forth without a backlash and obtain a sine-wave motion. Thanks for your patience your suggestions will be of great help.

05-26-2012, 05:15 PM
Dynamixels have only two operating modes at the present time: Servo mode and Wheel/Endless mode. Servo mode moves to goal positions, and wheel mode simply spins at an approximate rotational rate without any position control (...or really accurate speed control as there are no quadrature encoders, only a ~300 [degree] sensing potentiometer in AX/RX-series servos or a 360 [degree] magnetic encoder in the MX-series).

Not really sure of any way to make it create a sine wave other than the method you are already using. The AX-12 does not have a very complex motion control system (http://support.robotis.com/en/product/dynamixel/ax_series/dxl_ax_actuator.htm#Actuator_Address_1A). When you give it a goal position, it starts moving to that location at the MOVING_SPEED using however much torque is required (less than the TORQUE_LIMIT). Then when the horn rotates to within the CW/CCW_COMPLIANCE_SLOPE (goal position +/- some position value), the torque is decreased linearly until the horn is within the CW/CCW_COMPLIANCE_MARGIN (goal position +/- some smaller position value) at which point torque is cut off (PUNCH is the lowest torque it will attempt to use before cutting off). The linear relations between torque and position are:
{ (TORQUE_LIMIT - PUNCH) / (CW_COMPLIANCE_SLOPE - CW_COMPLIANCE_MARGIN) } * (current_position - goal_position) and
{ (TORQUE_LIMIT - PUNCH) / (CCW_COMPLIANCE_SLOPE - CCW_COMPLIANCE_MARGIN) } * (goal_position - current_position), with different relationships available for CW and CCW approaches to the goal_position.

The reason the movements of the various Robotis and user-created dynamixel based bots are so smooth is because the firmware on the CM-5/510/530/700 and arbotix controller boards implement interpolating motion engines which give the servos intermediate goal positions between static poses. So instead of trying to rely on the inaccurate speed control to get the servos to their pose goal positions on time, they calculate the intermediate positions to send the at full speed so that they arrive at their pose goal positions at the correct time.

If you really need PID, the new MX-series servos have ARM Cortex-M3 microcontrollers from STMicroelectonics so they possess sufficient processing power for the servo firmware to implement a (user-tunable) PID control system over a full 360 [degrees] of goal positions. It is important to note that there is still not relative move function when using servo mode, so moving from 5 [degrees] to 355 [degrees] will never pass through 0 [degrees].

05-27-2012, 05:17 AM
Hmm.. but can you help me understand the reason why I am getting this kind of curve http://www.doc.ic.ac.uk/~ahanda/SineWave500.png when I increase the speed to 500. I am a bit clueless about the kinks I observe in the graph. My hunch is that it is due to the servo inertia of motion to move in one direction and then immediately it receives a command to go in other direction since I'm not polling to check if it has reached the goal position and therefore it takes time for servo to settle before it can go immediately resume to moving with that velocity in the other direction.


05-28-2012, 01:27 AM
First: Even with a perfect servo, you will not get a sine wave. You will get a parabola -- a second degree polynomial. This assumes that the servo applies maximum acceleration until it's at the mid-point, at which point it applies maximum decelleration, and there are no other losses.

In reality, there are large losses, and the servo is probably better at coming up to speed, than it is at keeping a very high speed. Thus, the mid-section of the rising and falling cycles will be less curved (derivative near a constant) and the end will probably be shorter than the beginning, because friction and other losses will help the servo reduce speed. This is even assuming you have a "perfect" PID controller and no non-linear losses. It turns out, neither of those are true :-)

First, there are non-linear losses in the form of backlash, as well as potentially other losses (depending on what your mechanics are.)
Second, I believe the AX-12+ simply has a "slope" and a "slop." Think of position along X, and force along Y. Draw a line diaginally up from the target position. This line limits amount of force applied, based on the current position. Where the line is above the maximum amount of force (torque) the servo can put out, that's the limit. Once the servo gets to within "slop" units of the target position, then it outputs zero torque.

My knowledge of mechanical dynamics comes more from software simulation than hands-on experience, and I know that both real and software systems have significant ringing, flex, and second/third/non-linear order effects, and I really can't (or at least "shouldn't") tune a system to be "perfect" at the limit of the specification of the components involved. Instead, when I want "smooth," behavior, I specify at some ridiculous factor times the "needed" specifications (two X? five X?) and then use a continuous controller to actually drive the smooth behavior. To translate to your case, you should specify a movement curve that is not just two points with fast oscillation, but instead you should run it slower, and you should specify goal positions constantly along the path you want to move the servo. A system like that will be able to perform well under a variety of real-world situations, up to some fraction of the limit of theoretical specifications.

A system that's "tuned" to the limit of the specification will only perform when all the variables are exactly right, and the moon phase hasn't changed too much since you last tuned it.

05-29-2012, 07:59 AM
Thank you tician and jwatte for your replies. Another thing that I haven't really understood yet is how the PID control in the dynamixel ax-12 actually changes the speed as a function of torque profile specified by the compliance and margin, the goal position and the set speed. I would like to know how the servo decides its velocity and how quickly it switches from one velocity level to the other level.


05-29-2012, 01:09 PM
There is no PID controller. There are no current sense ICs or resistors. It uses an internal table or equation to convert between the 8/16-bit PWM values of the ATmega88 used to switch the MOSFET H-Bridge and the "torque" values in the device registers without regard to the actual load on the servos. I would hope that it at least uses the potentiometer to estimate the moving speed of the horn to cut back on the torque when it is moving too fast, but I don't know for certain.

Do not take the following example as gospel. It is based on the best of my understanding with a few assumptions made as to the actual algorithm employed. I have not checked with Robotis (I doubt they would share) and I am sure there are a few errors, but this control method does seem mostly consistent with my experiences with AX-12's. In this example, torque is mostly synonymous with a PWM value.

Torque Limit: 1023, Moving Speed: 500, CW/CCW Compliance Slope: 32, CW/CCW Compliance Margin: 1, Punch: 5.
dT/dP: (torque_limit - punch)/(Slope - Margin) = (1023 - 5) / (32 - 1) = ~32 (only uses integer math. Compliance Slope values are only available as 1/2/4/8/16/32/64 for simple bit-shift operations on the deltaP).

Current position is 200 and goal position is 512.

The servo will find the current position and calculate the difference from the goal position (deltaP = (goal - curr) = 312). deltaP is larger than Slope, so spin at max torque available until we see the speed rise over the moving speed limit. When over the speed limit, reduce the applied torque (but not the limit) until speed is at speed limit (repeatedly finding the new deltaP while this is happening). When deltaP is less than Slope (say 30), it temporarily decreases the torque limit to a new allowable torque: (dT/dP)*(deltaP) = (32 * 30) = 960. When it decreases torque, it also decreases the maximum speed the motor can rotate while under load. As deltaP gets smaller, so does torque and subsequently the ability of the motor to spin at all (if load exceeds torque, there is no rotation or rotation in the opposite direction of the goal position). At a deltaP of 10, the allowable torque would be (32 * 10) = 320 and servo will try to move the horn as close to the MOVING_SPEED as allowed by that torque limit (may be significantly slower). Once deltaP is less than or equal to the Margin, the MOSFET driver is turned off.

Again, this may not be correct.

05-29-2012, 03:53 PM
Hi tician,

Thanks for your explanation. I think it makes sense to me but I am wondering if the servo maintains this same profile of velocities given the goal, torque compliance etc. are fixed for repeated trails of reaching same goal position from same current position? I was doing an experiment on that and found out that I don't get same velocity profiles all the time or am I running into issues of serial port timings? My hunch is that due to friction and other effects which don't remain constant all the time the velocity profiles should be different or I may be wrong completely.

08-27-2014, 03:19 AM
Hi tician and robot,

I read the complete thread. I have a simple doubt regarding the dynamixel motors. If I give two commands to the motor to move from goalposition1 to goalpostition2 and then goalposition2 to goalposition3 with different specified moving speeds what will the velocity profile without sufficient delay between the commands?

To put it differently, how are commands executed by the motor? Does the motor necessarily come to a stop before executing the next moving command? Perhaps username-robot might help with his experience.


08-27-2014, 04:37 PM
It updates on-the-fly. Once you send it the new goal position, speed limit, and/or 'torque' limit, those new values are used during the next iteration/update of the control algorithm (MX servos should run at ~125Hz or faster) along with whatever 'present position' and 'present speed' values currently sensed/calculated. No need to come to a stop before updating or starting again. Actually, requiring a full stop would make interpolation motion engines without lots of position/load feedback fail (like the DARwIn-OP walking engine which uses no servo feedback).

08-28-2014, 04:45 AM
Hi tician,

Thank you for the quick reply.

So If I understand your explanation correctly, it keeps a stack of commands to be executed and with a frequency of 125 Hz checks the current position and current velocity is correct or not.

My two commands were :

So how and when the motor changes the speed from speed 1 to speed2?

Basically, My task is to make the motor perform according to a velocity profile given the initial and final position of the robot.
This profile may be a smooth curve, maybe a higher order polynomial. Can you comment on the solution that i have thought:

Solution 1: Run the motor in free running mode giving velocity commands with 125 Hz frequency. This will closely follow the curve albeit with a little discretization/quantization. If the velocity commands are correct the motor will come to stop at the specified final position.

Solution 2: In servo mode, calculate required position every 1/125 second and also the resulting speed and give the position,velocity command with 125 Hz.

Thank You

08-28-2014, 09:51 AM
Only the Dynamixel Pro servos (and maybe the EX-106 and MX-106) have quadrature encoders for speed calculation. Everything else uses a regularly sampled potentiometer (AX/DX/RX/EX) or magnetic encoder (MX servos) to grab the current position and uses the difference from the last position(s) to estimate moving speed. Dynamixel servos are primarily position control devices, not velocity control devices. The MX-64 and MX-106 have 'current' control modes (because of internal motor/driver current sensing of some sort), but not sure how well they actually work to approximate torque control devices.

Solution 1 will work only at very low speeds and servo loading. The wheel mode is not accurate for anything except the MX-12W and offers absolutely no position control.

Solution 2 using servo mode will get you a decent approximation of the curve, but you can only set the goal position of the servo and a maximum speed. To get better control over the speed profile, you will have to calculate the proper intermediate goal positions and expect the servo to get there at anything up to the maximum speed. You could possibly get better control over velocity if you used the control algorithm to independently calculate the PWM value the servo will be using internally which could possibly give an approximation of speed.