Smooth Head Rotation

[nilsb]

AngleLookupTable.xls (148480Bytes)

Did you ever tried to rotate the robot head and found the movement nervous?
   This tip can help to smooth movements out. 

Did you ever looked at a robot and found its movements human like?
   This tip can tell you how you can humanize your robot's moves. 
 

Motivation

Most of us one day have the challenge to rotate something from one angle to another. It might be a head, arm, finger, tail or whatever you have to move. Most of us initially go on and write a for-loop that counts from 0° to i.e. 180° with an increment of 1 and write the counter to the servo, i.e. [.., servo.write(42°), servo.write(43°), ...]. This is fine. A classic linear movement.

In this tip we want to go one step further and use a non-linear movement to make the move smooth. You also get an idea what mathematical function is used to create the data for you.

 

Introduction

The movement must start from zero velocity and go to zero velocity. There you find an intrinsically nature of the mathematical function we are looking for. It must be near zero in the beginning and near zero in the end. 

One function that has this nature is the Sinus Square velocity over time function. This function is used alot in feedback control systems. It accelerates progressive in the first third, continues with almost no acceleration in the second third and then decelerates regressive in the last third.

Using this Sinus Square function makes sense in almost every move your robot makes. It not only looks and sounds better than the linear function but is also more gentle to the servo gears. The gears grip and "slowly" start to rotate.

If you want to find out more about this topic then have a look into the further readings at the end of this tip.

 

The Smooth Head Rotation

Start your Arduino IDE and copy the sample code from the end of this page. Paste it into the C editor and upload it to the microcontroller.

When you now observe the servo movement you see, that it does exactly what I mentioned before. It accelerates smooth. No sudden full speed but a smooth beginning of a movement. Then constant speed. Then decelerates smooth back to zero velocity at 180°.

Now play around with the timerInMillies that your ears hear the smoothness too. 

 

The Electronics

Plug the servo to pin 6 and the led to pin 13.

Use pin 6 when you want to use the code below.

 

The Math

Skip this chapter if you just want to have a smooth head rotation and don't bother about the calculus behind it.

 

The sinus square function we use is in velocity over time. That is the V(t) function. We want to set angles (distances) and not velocities therefore we need to apply some calculus. To calculate the way over time values the V(t) function must be integrated to a W(t) function (W for way). The W(t) function must then be scaled, stretched and parametrized.

This function is going to give us the angle for a given t and the amount of sampling points x. t goes from 0° to 180°. x is the amount of sampling points you want. With the sampling point amound you deside, how many steps the movement must make from start to end, that is i.e. if you go from 1° to 45° in 30 steps you need to set the x to 30.

As we now have the analytical function we need to translate it into a Excel forumla to then calculate each sampling point.

 

The Discreete Profile

To use the integrated sinus square function in your C code you need to have a discreete profile. This data is going to be in a lookup table so the servo controller can use the data to tell the servo to what angle it needs to go next.

Using this function you can use a spreadsheet table to calculate the values.

I use a Google Doc Table to create the data, to get all the nessessary angles. Excel is good to. Use this forumla to calculate the angles from 0° to 45° in 100 steps:

=Round( 100 / Pi() * ( ( Pi() * A1) / 45 - cos( ( Pi() * A1 ) / 45 ) * sin( ( Pi() * A1 ) / 45 ) ) , 0 )

The 100 can be changed to that many steps that are useful to you. The 45° can be changed to what angle the move should go to.

Here more generic with the t value in the A column and the amount of samples in the $D$2 and the end angle in the cell $E$2. 

=Round( $D$2 / Pi() * ( ( Pi() * A1 ) / $E$2 - cos( ( Pi() * A1 ) / $E$2 ) * sin( ( Pi() * A1 ) / $E$2 ) ) , 0 )

Use this formula for the other way around:

=Round( 180 - ( $D$2 / Pi() * ( ( Pi() *  A1 ) / $E$2 - cos( ( Pi() *  A1 ) / $E$2 ) * sin( ( Pi() *  A1 ) / $E$2 ) ) ) , 0 )

The Software

For simplicity this sketch turns only the head from 0° to 180° and then stops. To re-run the sketch press the Arduino reset button.

This example uses Timer2. As you might know I don't like the delay() function. The Timer2 lets me run a timer that calls the move() function periodically after a duration. This keeps the main-loop free to do the behaviors and stay reactive.

Further you find a lot of functions in the code. This is due the Single Level of Abstraction Principle. And an intension revealing method-name I like a lot more than // comments . So methods can be used to make self-explaining code. 

A second example is in the Appendix A. The second example uses the delay and no timer.

/* 
 * Smooth servo rotation using a sinus square function.
 * more infos: https://www.robotshop.com/letsmakerobots/node/31697
 * created by NilsB 
 */
#include <MsTimer2.h>

#include <Servo.h> 
const int timerInMillies = 60;

const int countSinusSquareLookupTableEntries = 34;

const int sinusSquareLookupTable[] = {

0,0,0,0,1,2,

5,8,12,17,24,31,

40,49,60,71,82,93,

105,116,126,136,145,153,

160,166,171,174,177,179,

180,180,180,180

};
int currentValueIndex = 0;

const int movmentIndicatorPin = 13;
Servo headServo;
/*

• This is the function that gets called periodically.
  */
  void move()
  {
  moveServoTo(angle());

  indicateMovement();

  incrementOrStop();
  }

void moveServoTo(int angle){
Serial.println(angle);
headServo.write(angle);
}

int angle(){
return sinusSquareLookupTable[currentValueIndex];
}

void indicateMovement()
{
static boolean output = HIGH;

digitalWrite(movmentIndicatorPin, output);
output = !output;
}

void incrementOrStop(){
currentValueIndex++;
if(currentValueIndex == countSinusSquareLookupTableEntries - 1){
stopMove();
}
}

void stopMove(){
MsTimer2::stop();
headServo.detach();
}

void setupMovementIndicator(){
Serial.begin(9600);
pinMode(movmentIndicatorPin, OUTPUT);
}

void setupTimer2(){
MsTimer2::set(timerInMillies, move);
MsTimer2::start();
}

void setupServo(){
headServo.attach(6);
headServo.write(0);
}

void setup(){
setupMovementIndicator();
setupServo();
setupTimer2();
}

/*

• The loop method is empty and can be used to
• read sensor data and feed the behaviors.
  */
  void loop(){;}

Comparison to the Linear Ramp

In the comments was asked about the ramp function. The ramp function is used to make linear movements. The first part of the move the velocity increases by a fixed increment delta. In the middle part the velocity stays the same. In the third part the velocity decreases again to zero.

When we compare the velocity over time V(t) and acceleration over time a(t) diagrams we see that the Sinus Square V(t) from this tip has a acceleration transition. The ramp V(t) has a peak. This peak is hearable. You hear noise from the power transmission of the motor to the gears. On every orange dot you should hear this noise. This may slowly destroy the gear since with every move there are four times the gears crash into each other… While for our little light microservos that carry a ultrasonic range sensor only this crashes are not so dramatic. But when you build a heavier head like a steampunk metal head or even a plastic head with microcontroller and sensors the mass inertia starts to come in and Newton’s Laws of Physics.

But I must agree compound ramp functions are very easy to understand and applicable without mathematics. It's your choice to smoothen things and accept higher level complexity or less smooth with lower complexity.

Sinus Square Lookup Tables

Here you have some useful lookup tables. You find one that goes from 0° to 180° and the other way around. And the same with a higher precision.

t	y	from 0° to 180°	inverse	from 0° to 90°	inverse	from 90° to 180°	inverse	from 90° to 135°	inverse	from 90° to 45°	inverse
0	0	0	180	0	90	90	180	90	135	90	45
1	0.000203066	0	180	0	90	90	180	90	135	90	45
2	0.00162423	0	180	0	90	90	180	90	135	90	45
3	0.005480108	0	180	0	90	90	180	90	135	90	45
4	0.012984347	0	180	0	90	90	180	90	135	90	45
5	0.02534615	0	180	0	90	90	180	90	135	90	45
6	0.043768802	0	180	0	90	90	180	91	134	89	46
7	0.069448205	0	180	0	90	90	180	91	134	89	46
8	0.103571418	0	180	0	90	90	180	92	133	88	47
9	0.147315212	0	180	1	89	91	179	92	133	88	47
10	0.201844639	0	180	1	89	91	179	93	132	87	48
11	0.26831161	0	180	1	89	91	179	94	131	86	49
12	0.347853489	0	180	1	89	91	179	95	130	85	50
13	0.441591714	0	180	2	88	92	178	96	129	84	51
14	0.550630425	1	179	2	88	92	178	97	128	83	52
15	0.676055122	1	179	3	87	93	177	99	126	81	54
16	0.818931338	1	179	3	87	93	177	100	125	80	55
17	0.980303349	1	179	4	86	94	176	102	123	78	57
18	1.161192892	1	179	4	86	94	176	104	121	76	59
19	1.362597928	1	179	5	85	95	175	106	119	74	61
20	1.585491421	2	178	6	84	96	174	108	117	72	63
21	1.830820156	2	178	7	83	97	173	110	115	70	65
22	2.099503585	2	178	8	82	98	172	112	113	68	67
23	2.392432702	2	178	9	81	99	171	113	112	67	68
24	2.710468967	3	177	10	80	100	170	115	110	65	70
25	3.054443245	3	177	11	79	101	169	117	108	63	72
26	3.425154804	3	177	12	78	102	168	119	106	61	74
27	3.823370334	4	176	13	77	103	167	121	104	59	76
28	4.249823017	4	176	15	75	105	165	123	102	57	78
29	4.705211633	5	175	16	74	106	164	125	100	55	80
30	5.190199706	5	175	18	72	108	162	126	99	54	81
31	5.705414699	6	174	19	71	109	161	128	97	52	83
32	6.251447248	6	174	21	69	111	159	129	96	51	84
33	6.828850442	7	173	22	68	112	158	130	95	50	85
34	7.43813915	7	173	24	66	114	156	131	94	49	86
35	8.079789395	8	172	26	64	116	154	132	93	48	87
36	8.754237769	9	171	28	62	118	152	133	92	47	88
37	9.461880908	9	171	29	61	119	151	133	92	47	88
38	10.203075	10	170	31	59	121	149	134	91	46	89
39	10.97813537	11	169	33	57	123	147	134	91	46	89
40	11.78733606	12	168	35	55	125	145	135	90	45	90
41	12.63090954	13	167	37	53	127	143	135	90	45	90
42	13.50904638	14	166	39	51	129	141	135	90	45	90
43	14.42189506	14	166	41	49	131	139
44	15.36956176	15	165	43	47	133	137
45	16.35211024	16	164	45	45	135	135
46	17.36956176	17	163	47	43	137	133
47	18.42189506	18	162	49	41	139	131
48	19.50904638	20	160	51	39	141	129
49	20.63090954	21	159	53	37	143	127
50	21.78733606	22	158	55	35	145	125
51	22.97813537	23	157	57	33	147	123
52	24.203075	24	156	59	31	149	121
53	25.46188091	25	155	61	29	151	119
54	26.75423777	27	153	62	28	152	118
55	28.07978939	28	152	64	26	154	116
56	29.43813915	29	151	66	24	156	114
57	30.82885044	31	149	68	22	158	112
58	32.25144725	32	148	69	21	159	111
59	33.7054147	34	146	71	19	161	109
60	35.19019971	35	145	72	18	162	108
61	36.70521163	37	143	74	16	164	106
62	38.24982302	38	142	75	15	165	105
63	39.82337033	40	140	77	13	167	103
64	41.4251548	41	139	78	12	168	102
65	43.05444324	43	137	79	11	169	101
66	44.71046897	45	135	80	10	170	100
67	46.3924327	46	134	81	9	171	99
68	48.09950358	48	132	82	8	172	98
69	49.83082016	50	130	83	7	173	97
70	51.58549142	52	128	84	6	174	96
71	53.36259793	53	127	85	5	175	95
72	55.16119289	55	125	86	4	176	94
73	56.98030335	57	123	86	4	176	94
74	58.81893134	59	121	87	3	177	93
75	60.67605512	61	119	87	3	177	93
76	62.55063043	63	117	88	2	178	92
77	64.44159171	64	116	88	2	178	92
78	66.34785349	66	114	89	1	179	91
79	68.26831161	68	112	89	1	179	91
80	70.20184464	70	110	89	1	179	91
81	72.14731521	72	108	89	1	179	91
82	74.10357142	74	106	90	0	180	90
83	76.06944821	76	104	90	0	180	90
84	78.0437688	78	102	90	0	180	90
85	80.02534615	80	100	90	0	180	90
86	82.01298435	82	98	90	0	180	90
87	84.00548011	84	96	90	0	180	90
88	86.00162423	86	94	90	0	180	90
89	88.00020307	88	92
90	90	90	90
91	91.99979693	92	88
92	93.99837577	94	86
93	95.99451989	96	84
94	97.98701565	98	82
95	99.97465385	100	80
96	101.9562312	102	78
97	103.9305518	104	76
98	105.8964286	106	74
99	107.8526848	108	72
100	109.7981554	110	70
101	111.7316884	112	68
102	113.6521465	114	66
103	115.5584083	116	64
104	117.4493696	117	63
105	119.3239449	119	61
106	121.1810687	121	59
107	123.0196967	123	57
108	124.8388071	125	55
109	126.6374021	127	53
110	128.4145086	128	52
111	130.1691798	130	50
112	131.9004964	132	48
113	133.6075673	134	46
114	135.289531	135	45
115	136.9455568	137	43
116	138.5748452	139	41
117	140.1766297	140	40
118	141.750177	142	38
119	143.2947884	143	37
120	144.8098003	145	35
121	146.2945853	146	34
122	147.7485528	148	32
123	149.1711496	149	31
124	150.5618608	151	29
125	151.9202106	152	28
126	153.2457622	153	27
127	154.5381191	155	25
128	155.796925	156	24
129	157.0218646	157	23
130	158.2126639	158	22
131	159.3690905	159	21
132	160.4909536	160	20
133	161.5781049	162	18
134	162.6304382	163	17
135	163.6478898	164	16
136	164.6304382	165	15
137	165.5781049	166	14
138	166.4909536	166	14
139	167.3690905	167	13
140	168.2126639	168	12
141	169.0218646	169	11
142	169.796925	170	10
143	170.5381191	171	9
144	171.2457622	171	9
145	171.9202106	172	8
146	172.5618608	173	7
147	173.1711496	173	7
148	173.7485528	174	6
149	174.2945853	174	6
150	174.8098003	175	5
151	175.2947884	175	5
152	175.750177	176	4
153	176.1766297	176	4
154	176.5748452	177	3
155	176.9455568	177	3
156	177.289531	177	3
157	177.6075673	178	2
158	177.9004964	178	2
159	178.1691798	178	2
160	178.4145086	178	2
161	178.6374021	179	1
162	178.8388071	179	1
163	179.0196967	179	1
164	179.1810687	179	1
165	179.3239449	179	1
166	179.4493696	179	1
167	179.5584083	180	0
168	179.6521465	180	0
169	179.7316884	180	0
170	179.7981554	180	0
171	179.8526848	180	0
172	179.8964286	180	0
173	179.9305518	180	0
174	179.9562312	180	0
175	179.9746539	180	0
176	179.9870157	180	0
177	179.9945199	180	0
178	179.9983758	180	0
179	179.9997969	180	0

 

Further Readings

http://www.wolframalpha.com/input/?i=plot%28sin%28t%29%5E2%2Ct%2C0%2CPi%2F2%29
   plot( sin( t )^2, t, 0,Pi/2 )

http://www.wolframalpha.com/input/?i=plot%28integrate%28sin%28t%29%5E2%29%2Ct%2C0%2CPi%29
   plot( integrate( sin( t )^2 ), t, 0, Pi)

http://www.wolframalpha.com/input/?i=%281%2F2*%28t%2F180*Pi+-+Sin%28t%2F180*Pi%29*cos%28t%2F180*Pi%29%29%29%2F1.57*180
   (1/2*(t/180*Pi - Sin(t/180*Pi)*cos(t/180*Pi)))/1.57*180

http://www.wolframalpha.com/input/?i=x%2FPi+*+%28%28%CF%80+t%29%2F180-cos%28%28%CF%80+t%29%2F180%29+sin%28%28%CF%80+t%29%2F180%29%29
   x/Pi * ((π t)/180-cos((π t)/180) sin((π t)/180))

http://books.google.ch/books?id=_VfzuBute1cC&lpg=PA183&ots=JmxoR_MWsV&dq=sinus%20quadrat%20regelungstechnik&hl=de&pg=PA183#v=onepage&q&f=false
   "Regelungstechnik. Erweiterungen der Regelungsstruktur."

https://www.robotshop.com/letsmakerobots/node/28278
   "This tutorial shows the use of timers and interrupts for Arduino boards."

http://arduino.cc/playground/Main/MsTimer2
   "MsTimer2 is a small and very easy to use library to interface Timer2 on the ATmega168/328"

http://www.scribd.com/doc/49262022/Clean-Code-Cheat-Sheet-V1-3
   "Clean Code Development Cheat Cheat"

 

Appendix A:

This is an alternative implementation that uses the delay() function in the main loop.

 

/* 
 * Smooth servo rotation using a sinus square function.
 * ATTENTION: THIS SKETCH USES THE DELAY FUNCTION
 * More infos: https://www.robotshop.com/letsmakerobots/node/31697
 * created by NilsB 
 */
#include <Servo.h> 
const int timerInMillies = 20;

const int countSinusSquareLookupTableEntries = 181;

const int sinusSquareLookupTable[] = {

0,0,0,0,0,0,0,0,0,

0,0,0,0,0,1,1,1,1,

1,1,2,2,2,2,3,3,3,

4,4,5,5,6,6,7,7,8,

9,9,10,11,12,13,14,14,15,

16,17,18,20,21,22,23,24,25,

27,28,29,31,32,34,35,37,38,

40,41,43,45,46,48,50,52,53,

55,57,59,61,63,64,66,68,70,

72,74,76,78,80,82,84,86,88,

90,92,94,96,98,100,102,104,106,

108,110,112,114,116,117,119,121,123,

125,127,128,130,132,134,135,137,139,

140,142,143,145,146,148,149,151,152,

153,155,156,157,158,159,160,162,163,

164,165,166,166,167,168,169,170,171,

171,172,173,173,174,174,175,175,176,

176,177,177,177,178,178,178,178,179,

179,179,179,179,179,180,180,180,180,

180,180,180,180,180,180,180,180,180,

180

};
const int movmentIndicatorPin = 13;
Servo headServo;
void setup(){

setupMovementIndicator();

setupServo();

}
void loop(){

move();

wait();

}
/*

• This is the function that gets called periodically.
  */
  void move()
  {
  for(int angleIndex = 0; angleIndex < countSinusSquareLookupTableEntries; angleIndex++){
  moveServoTo(angle(angleIndex));
  indicateMovement();
  wait();
  }

  waitLong();
  for(int angleIndex = countSinusSquareLookupTableEntries-1; angleIndex >= 0; angleIndex--){
  moveServoTo(angle(angleIndex));
  indicateMovement();
  wait();
  }

  waitLong();
  }

void moveServoTo(int angle){
Serial.println(angle);
headServo.write(angle);
}

int angle(int index){
return sinusSquareLookupTable[index];
}

void indicateMovement()
{
static boolean output = HIGH;

digitalWrite(movmentIndicatorPin, output);
output = !output;
}

void stopMove(){
headServo.detach();
}

void setupMovementIndicator(){
Serial.begin(9600);
pinMode(movmentIndicatorPin, OUTPUT);
}

void setupServo(){
headServo.attach(6);
headServo.write(0);
}

void wait(){
delay(timerInMillies);
}

void waitLong(){
delay(5*timerInMillies);
}

 

 

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

[ignoblegnome]

I’ve collected this so I can

I’ve collected this so I can read it again when I’m not half asleep. Very interesting post.

[danceswithrobots]

So. . .this could also be

So. . .this could also be used for drive motors to ramp up to speed without a jerky start?

[ignoblegnome]

Instead of going from 0 to

Instead of going from 0 to 180 and back, you’d want to scale it to the PWM lower and upper limits of 0 to 255. 

Unless you are using continuous rotation servos, in which case it’d be perfect as it is.

[nilsb]

Applicable too
Yes. The sinus square can be used for this too. So your Houston would not shake due to mass inertia when it starts to move, and not shake when it makes a normal stop. In this tip I wanted to stay focused on servo rotations. But indeed the topic is not so narrow.

[danceswithrobots]

Heh, you’re a mind reader

Heh, you’re a mind reader NilsB.  Nothing would please me more than to have Houston’s first “steps” be smooth and graceful.  I figured he was going to look like the shaking tower of robot.

[ro_bot_x]

OK. Question: Say I want to

OK. Question: Say I want to use this method with legged robots and all mevement is related to the servo center position. All movement angles are expressed as center+angle or center-angle. How do I get the index to start the movement from 90 (center) and move to 115 (center+25) for example? There must be something simple that eludes me at this hour…

[nilsb]

Squeeze and Lift

As I understand your question you ask for the correct profile and the index strategy, right?

Find your profile. To get this profile I used the periodicity of the sinus square and squeezed the integral of it from 0° to 25°. The periodical sinus gives you now your data from -25° via 0° to 25°. Next I lifted this wave up to 65° by adding 65 to the formula. The result is visualized in the picture above. The vertical axis is angle, the horizontal axis is time/steps.

I have chosen 25 steps from 65° to 90° and 25 from 90° to 115°.

Here is the Google Doc Table formula:  A2 is a cell with one discreete x and the A column contains data [0...50]

=Round( 25 / Pi() * ( Pi() * A2 / 25 - Cos( Pi() * A2 / 25) * Sin( Pi() * A2 / 25)) + 65 ,0)

Find the index. So now you get a list of 50 samples. If you move from 90° to 115° and want to know each angle then use the (50/2 + index) to lookup the angle at the given index. If you go from 90° to 65° use the (50/2 - index) for lookup. I assume index in each loop [0..25].

[duaneb]

Nice idea for scale movement in general

Hi,

   I had not thought of this before reading your post, but this will also make the movement of small tracked robots more realistic, I plan to encorporate a simple ‘easing’ into my first robot so that it does not start and stop every move at full speed or full stop. It will hopefully look like something more substantial, as if it has weight and momentum.

Thanks for sharing

Duane.

rcarduino.blogspot.com

 

 

[birdmun]

I thought I had posted this earlier.

 

There are two formulas to allow accel/deaccel over time:

f(x) = (x^2)/(200t/200) //accel
f(x) = (-((x-t)^2)/(200t/200)+100 //deaccel

All that will need to be passed is time to target and target.
The function will calculate percentages of target and write them to a servo.

void smoothMove(float timeToTarget, int target, int servoToMove) {
int accel, deaccel;

for (int i = 0; i <= timeToTarget/2; i++) {
accel = (i^2)/(200target/200);
analogWrite(servoToMove, accel * target);
}

for (int i = timeToTarget/2; i <= timeToTarget; i++) {
deaccel = (-((i-target)^2)/(200target/200)+100;
analogWrite(servoToMove, deaccel * target);
}
}

I don’t know if this will be better/worse/different. Just thought I would share.

PS: I only worked out the formulas on a graphing program. The curves looked right. I don’t know if they would work.

PPS: I am going to have to rework all of this. I just is not coming out as I remember it doing.

[robopunk]

How can we inject a Random

Now how can we inject a Random function into this code to make 2 servos move smoothly but randomly in both directions ? the aim is to replicate human head movement in 2 axis?