9 15

4 28 25
29 3
16
10 24
27

1
6
21 22 9 19
26 14
13 11 12 2 5
7
18 17
20
23
30

1x Arduino + USB Cable 1x Extension Shield 4x Servo motor 1x Li-Ion 16340 Battery

1x DIY 16340 Powerbank Spacers, Screws and Selftap screws 2mm

Nuts

1x Hot-glue gun Pliers y screwdriver

 9 15
4 28 25
29 3
16
10 24
27

1
6
21 22 9 19
26 14
13 11 12 2 5
7
18 17
20
23
30

1x ESP-01S with 10x DuPont Cables F-F 1x Servo Controller PCA9685 4x Servo motor
USB Programmer

1x DIY 16340 Powerbank 1x Li-Ion 16340 Battery Spaces, Scews, Nuts Selftap screws
2mm

1x Hot-glue gun Pliers and scewdriver

Arduino IDE (https://www.arduino.cc/en/main/software) is the programming environment
that we will use to program the robot, although you can use others. The program includes
all the libraries that we will use by default, so it does not need a special configuration.

Select the Arduino Nano board in ToolsYBoard. In ToolsYProcessor, you must take into
consideration that if you are using a cloned version, you must use the ATmega328P (old
bootloader) version. If the Arduino board is original, then you must use ATmega328P.

Once the board and the processor have been selected, you must select the communication
port in ToolsYPort. Connect the Arduino to the PC and a new serial port will appear that
you must select. If no new port appears, then it probably means that you have not properly
installed the driver. In the cloned versions of Arduino, the CH340G chip is used, so you must
install the driver for this chip following the instructions indicated here, if that’s the
case:https://sparks.gogo.co.nz/ch340.html

Connect the USB cable, with an empty program (with the "setup" and "loop" functions that
come by default), try uploading the code to the Arduino board by accessing ProgramYUpload.
Usually, it is convenient to previously compile the program, to verify that you do not have any
errors in the code. In that case, to compile the program (do not upload the code), go to
ProgramYVerify/Compile.
The ESP-01S is a development board based on the ESP8266 processor. In order to program
this development board it is convenient to use a USB programmer. The ESP-01S must be
connected to the USB programmer as shown in the figure. The USB programmer has
duplicated the connector, so that on the one hand it allows you to connect the ESP-01S to
the programmer and at the same time it allows you to connect the device to other devices
through the processor signals (RX, TX, GPIO0 and GPIO2) and to supply external power the
ESP -01S processor through the power pins (VCC and GND).
ESP-01S Flash Memory

Pins

ESP-01S USB Programador WiFi Antenna ESP8266 Procesador

Arduino IDE (https://www.arduino.cc/en/main/software) is the programming environment

that we will use to program the robot, although you can use others. We must previously
configure Arduino IDE to be able to program the ESP8266 processors. To do this, you must
access FileYPreferences and within the Additional Boards Manager URLs, add the
following URL: http://arduino.esp8266.com/stable/package_esp8266com_index.json
Then, in ToolsYBoardYBoard Manager, find and install the latest version of the ESP8266
boards

In order to use the Servo controller board, you need to install Adafruit PWM Servo Driver
library. You can install this library from ProgramYInclude LibraryYManage Libraries and
search for ‘PWM Servo’.

Connect the USB cable, with an empty program (with the "setup" and "loop" functions that
come by default), try uploading the code to the Arduino board by accessing ProgramYUpload.
Usually, it is convenient to previously compile the program, to verify that you do not have any
errors in the code. In that case, to compile the program (do not upload the code), go to
ProgramYVerify/Compile.
Component Arduino PIN
Servo motor 1 7
Servo motor 2 6
Servo motor 3 5
Servo motor 4 4
Powerbank (5V and GND) +5V
Gnd

PCA9685 Connection PIN ESP-01S Connection PIN

Servo motor 1 7 I2C (bus serie con PCA9685) GPIO0/SDA
Servo motor 2 6 GPIO2/SCL
Servo motor 3 5 3.3V Power (from Vcc
Servo motor 4 4 Powerbank)
I2C (serial bus connected to ESP-01) SCL Ground (any GND signal of GND
SDA the Powerbank)
5V Power for PCA9685 IC (from VCC
Powerbank)
5V Power signal to servos V+

Ground (any GND signal of the GND

Powerbank)

Previous to assembling the robot, it is convinient to set all servos at 90 position. This is an
intermediate position between 0 to 180 servo range. Brown cable is for ground signal and
yellow for signal, while red is for Vcc (a voltage between 4V and 6V approx.).

<
<

If you need to move the servo horn manually, please, be very

careful, because the servo gears are usually made of plastic and
be easily broken if a high torque is applied to move the horn.
It is a well-known library integrated in Arduino IDE to control servos. To include the library in in
your code, use the #include directive with the header file “Servo.h”.
To control a servo, you must créate an instance of a global variable with type Servo, and define
the pin that it is attached to with the attach method. If you want to disconnect the servo (to
avoid unnecessary power consumption), you can use the detach instruction.
To move the servo from one position to another one, you can use the write method, indicating
the position in degrees where we want to move the servo. Look at the picture to undestand
how servo rotate.

180
Los servos tienen un tiempo de ciclo de
20ms, con lo que no debéis mandar
comandos de movimiento por debajo de
este periodo. Utilizad instrucciones de
retardo (delay) para garantizar que se
cumple con este periodo.
0
#include <Servo.h>

Servo servos[12]; //Max number of digital signals

#define SERVO4_PIN 4
#define SERVO4_OFFSET 0
#define SERVO4_MIN_POS 0
#define SERVO4_MAX_POS 180

typedef struct
{
uint8_t pin;
int offset;
int min_pos;
int max_pos;
} RobotServo_t;

RobotServo_t servo4={SERVO4_PIN, SERVO4_OFFSET, SERVO4_MIN_POS, SERVO4_MAX_POS};

void setup() {
writeServo(servo4,90); //Sets the position of the servo to 90º
delay(1000);
detachServo(servo4);
}
void loop() {
}

void writeServo(RobotServo_t &servo, int angle)

{
angle=constrain(angle+servo.offset,servo.min_pos,servo.max_pos);
servos[servo.pin].attach(servo.pin);
servos[servo.pin].write(angle);}

void detachServo(RobotServo_t &servo)

{
servos[servo.pin].detach();
}
The following code sets the position to 90 of a servo connected to pin 4 to the PCA9685 PWM
controller. At the beginning, it creates an instance of a Adafruit_PWMServoDriver object with
the pwm variable, which is in charge of communicating with the PCA9685 PWM Controller.
The controller is connected through I2C to the processor, in this case, SDA is connected to
GPIO0, while SCL is connected GPIO2 of the ESP-01S board.
We have defined a new type RobotServo_t with all necessary values to control a servo.
Indeed, the variable servo4 is an instance of this type containing all servo-related information
so that the writeServo function can move the servo. If we want to disconnect a servo, we can
use the detachServo function.
Servos have a cycle time of
#include <math.h>
#include <Arduino.h> 20ms, so you should not send
#include <Wire.h>
#include <Adafruit_PWMServoDriver.h> movement commands below
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
this period. Use delay
instruction to ensure this
#define I2C_SDA_PIN 0
#define I2C_SCL_PIN 2 period is met.
#define MIN_PWM 130
#define MAX_PWM 570
#define FREQUENCY 50
#define SERVO4_PIN 4
#define SERVO4_OFFSET 0
#define SERVO4_MIN_POS 0 The provided code includes two
#define SERVO4_MAX_POS 180
constants defining the mínimum and
typedef struct máximum pulse width of the servo,
{
uint8_t pin; MIN_PWM and MAX_PWM. These
int offset; values must be calibrated in case of
int min_pos;
int max_pos; an incorrect operation, because the
} RobotServo_t; pwm object only undestands pulses,
RobotServo_t servo4={SERVO4_PIN, SERVO4_OFFSET, SERVO4_MIN_POS, while the writeServo requires an
SERVO4_MAX_POS}; angle.
void setup() {
Wire.pins(I2C_SDA_PIN,I2C_SCL_PIN); Calibration procedure:
Wire.begin(I2C_SDA_PIN,I2C_SCL_PIN); • Set the servo at position 0 and
pwm.begin();
pwm.setPWMFreq(FREQUENCY); decrease the value of MIN_PWM
yield(); until the servo stops moving from
writeServo(servo4,90); //Sets the position of the servo to 90º
delay(1000); its previous position.
detachServo(servo4); • Set the servo at position 180 and
}
void loop() { increase the value of MAX_PWM
} until the servo stops moving from
void writeServo(RobotServo_t &servo, int angle) its previous position.
{
int pulse_width;
angle=constrain(angle,servo.min_pos,servo.max_pos);
pulse_width = map(angle+servo.offset, 0, 180, MIN_PWM, MAX_PWM);
pwm.setPWM(servo.pin,0,pulse_width);
}

void detachServo(RobotServo_t &servo)

{
pwm.setPWM(servo.pin,0,0);
}
 Gripper Joint

q4=90

q2=90
q4=55 Joint 2
q2=50 q3=20

q2=170 q3=90
q1=0

q3=160 Joint 3

q1=90

Joint 1
q1=180

Base0 fija
Link Link 1 Link 2 Link 3 (left)

4 bar mech. Parallelogram Parallelogram Gripper

(link 2) (link 3)
 l1 l0
l5 l3

q1

d2
X
d5

d1
Y
q3,O

l3,O

l3 l4
l2
q3 q2

Dimensiones
Dimensions mm l3,I
d1 75 Z
d2 83.7
l0 5 h1
h1 64.5
l1 15.1
l2 80
l3 80 X
l3,I 23.9
l3,O 35
l4 80 l0
l5 36
52
d5 5
 q1=0

q1=90

q1=180

q4=70
q2=65
q3=110 q4=90
q2=90
q3=90

q2=160
q3=25
Fix four M3x10mm spacers to the base (1) of the robot with M3 nuts.

Now screw a servo (SG90 or MG90S) to the base of servo 1. Use two M3x8mm screws and a
servo bracket (2) for fixing. Then, fix the assembly to the robot base (3) with four M3x16mm
(metal) screws and their corresponding M3 nuts. To fix the height, the screws are screwed
against the wood itself. Note that the direction of the servo cable should be at the front of
the robot.

2 3

Fix a servo (SG90 or MG90S) to the right side of link 1 (4). Use two M3x8mm screws and a
servo bracket (6) to fix it (the screws are screwed against the wood itself). Note that the
direction of the servo cable should be as shown in the figure. Then, screw the base of link 1
(5) to the right side with a M3x10mm screw and a M3 nut. At the bottom of link 1, screw the
double servo horn with four M2x6mm sheet metal screws.

4
6

5
 Screw a simple servo handle to the right link 2 (7) with an
M2x6mm sheet metal screw. Then, with the servo positioned at
90, screw the link onto the servo so that it is in the vertical
position, as shown in the figure. If you don't know how to
position the servo at 90, please see the Servo Control section.

8
Now, screw the support for
link 2 (8) to the right link 2
(7), as shown in the figure.
Use an M3x10mm screw and
M3 nut.

Screw the lower right beam of link 3 (9) and the triangle to the
right link 2 (10) from the outside. When screwing, use two
washers between the pieces to reduce friction (see the figure
9
with the enlarged view). The screw is screwed against the
wood itself. The M3x10mm screw must be tight enough so that 10
the robot does not have too much slack, but at the same time,
the parts must rotate easily.
 11

Screw in a simple beam (11) to form the four bar

mechanism of the rear right side that joins link 2 (7) to
link 1 (3). The screws are M3x8mm (screwed against
the wood itself ). Use washers so that is tight enough
so that the robot does not have too much slack, but at
the same time, the parts should rotate easily. Make
sure you use one of the beam with two holes, one of
them slightly larger that allows the screw to pass
easily (there are two of them identical).

Similarly, now screw in the 12

beam that has a slightly smaller 12
hole (12) and a larger one (link
3 above). Screw a M3x8mm
screw against the wood itself
and use a washer to obtain a
low friction movement and
avoid excess of slack.

Screw the left link 2 (13) and the vertical support (14)
that joins the link 2 (7) with the link 1 (3). Use a small 13
M3x6mm screw with a washer against the wood itself.
14
Subsequently, screw, using M3x10mm screws and M3
nuts, the two pieces on one side to link 1 (3) and on
the other side to the support of link 2 (7). Make sure
that when screwing the M3x6mm screw that it has no
slack, but allows movement.
 15

Screw in link 3 left (15), the longest beam of all. Use a

M3x8mm screw and washer and screw it against the
wood itself. Make sure it has no slack, but with the
least possible friction.

16 17

Now assemble the left side of

link 1 (16), along with a servo
bracket (17) and a servo (SG90
or MG90S). Observe the
direction of the servo cables for
the correct assembly. Screw two
M3x8mm screws against the
wood itself.

18
19

Screw a servo horn to the rocker of the four-bar

mechanism of link 3 (19) with an M2x6mm sheet
metal screw. Then, with the servo positioned at 90,
screw the rocker to the servo so that it is in a
horizontal position, as shown in the figure with
another M2x6mm sheet metal screw. If you don't
know how to position the servo at 90 , please see the
Servo Control section. Finally, screw the link 3 four-bar
mechanism coupler (18) with an M3x8mm screw and
washer. It leaves little slack, but enough that it turns
freely.
Using an M2x6mm sheet metal
screw, screw link 1 (5) to the servo
of link 1 positioned at 90. If you
don't know how to position the
servo at 90, please see the Servo
Control section. Then place the
electronics on link 1

The Arduino Nano Powerbank

and expansion board can be
screwed to the base of link 1
with M3x8mm screws and
nuts.

PCA9685 servo controller

and 16340 powerbank can
be screwed with M3x8mm
screws. The USB
programmer for ESP-01S
cannot be screwed, so you
will need to use glue (with
the hot glue gun).

Place the front support of

link 1 (21) and screw it with
M3x8mm screws and M3 20
nuts, joining the assembled 21
parts of the right side with
the left side. The rear link 1
part (20) does not need
screws. Finally, screw with a
M3x8mm screw, to the
wood itself, and a washer
the left link 3 (15) with link
2 (7).
Insert a servo (SG90 or MG90S) into the 22 24
gripper base (24) and, using four
M3x8mm screws, screw the base 23 25
together with the gripper servo bracket
(22), the gripper small beam (23) and
the small adapter with a hole (25). Pay
attention to the servo cable, must be in
the front of the gripper.

26

Now screw a simple servo horn to

the gripper cranck (26) and then
attach it to the servo with M2x6mm
screws. The servo must be
positioned at 90. If you don't know
how to position the servo at 90,
please see the section Servo Control.

27 28

Now screw the fingers of the

gripper (27 and 28) with
M3x8mm screws and washers.
Position the fingers as shown in
the figure, with the fingertips
practically touching at the 90
30
position. 29

Screw the gripper coupler (29)

between the crank (26) and the left
finger (27). Use a M3x8mm screw to
screw in the finger and a M3x10mm
screw to screw in the crank. Use
washers between the wooden pieces
to have less friction and the round
spacer (30) as a spacer. Then, screw
the gripper to the rest of the robot
with M3x8mm screws.
Congrats! You’re done!
In axis control mode, the robot must move to a joint configuration determined by the
target values in a given time. In this mode, the joints are coordinated, in the sense that
each of them tries to reach a target position in the configuration space, but all at the same
time.

𝑞
𝑞

𝑞
𝑞

𝑞
𝑞

0 T
We define a smooth trajectory, 𝑞 𝑡 , in the configuration space with a polynomial
expression (the derivative of the trajectory is 𝑞̇ 𝑡 ) and we set the contour conditions,
𝑞 (0) and 𝑞 𝑇 and their derivates, 𝑞̇ (0) y 𝑞̇ 𝑇 . In particular, for a cubic trajectory, we
cant set as contour conditions for the trajectory the initial and final configurations 𝑞 and
𝑞 , respectively; and the initial and final speeds will be set to zero. Thus, the cubic
expression is defined as follows:

Trajectory (position) 𝑞 𝑡 =𝑎 𝑡 +𝑏𝑡 +𝑐 𝑡+𝑑

Trajectory (speed): 𝑞̇ 𝑡 = 3𝑎 𝑡 + 2𝑏 𝑡 + 𝑐

Contour conditions: 𝑞 0 =𝑞 𝑞 𝑇 =𝑞
𝑞̇ 0 = 0 𝑞̇ 𝑇 = 0

From contour conditions, we can get the values of the cubic trajectory parameters:

2 𝑞 −𝑞 3 𝑞 −𝑞 𝑐 =0 𝑑 =𝑞
𝑎 =− 𝑏 =
𝑇 𝑇

The parameters of the path are different for each servo and must be recalculated each
time the start, end position, speeds or the time of the path is modified.
In order to evaluate a trajectory over time, we must know the time of the microprocessor
clock. In this case, the most appropriate instruction in Arduino code is millis(), which
provides the clock time in milliseconds. On the other hand, to ensure that we evaluate the
trajectory at least every 20ms, which is the minimum time that the servo needs to
generate the control signal, we must use the delay(…) instruction, which will generate a
wait for the time indicated in milliseconds. Keep in mind that when calling the millis()
function for the first time, the return time does not have to be zero.

#define JOINTS 3 The following code is to be completed

and can be used to implement the axis-
RobotServo_t robotServos[JOINTS] ={{…},{…},{…}};
by-axis control of a polynomial path. At
void moveAbsJ(const RobotServo_t servos[JOINTS], const the beginning, define the number of axes
double q0[JOINTS], const double qT[JOINTS], const double
T); to be controlled with the JOINTS
constant, which in our case is equal to 3.
double q0[JOINTS]={…};
double qT[JOINTS]={…}; Furthermore, the parameters for each of
the robot's joints must be properly
void setup()
{ initialized in the robotServos variable, it
… is an array with the parameters of the
for (int i=0;i<JOINTS;i++)
writeServo(robotServos[i],(int)q0[i]); three axes to move of the type
delay(3000); RobotServo_t. Adequate values must
moveAbsJ(…);
delay(1000); also be provided to the variables q0 and
for (int i=0;i<JOINTS;i++) qT that represent the initial and final
detachServo(robotServos[i]);
… configuration in degrees, it is an array
} with the values of type double. The code
void moveAbsJ(const RobotServo_t robotServos[JOINTS], positions the servos in the initial
const double q0[JOINTS], const double qT[JOINTS], const position, waits a while and then calls the
double T)
{ moveAbsJ function, which is the one that
//TODO must implement the polynomial
//Compute trajectory parameters
//Get the initial timestamp trajectory with the information provided.
//Compute current joint values and move the servos those The parameter T of the moveAbsJ
positions. Wait at least 20ms and repeat until the time of the
trajectory is due. function represents the time in seconds
of the trajectory.
}

Move the robot to three consecutive configurations with a Point To Point trajectory (cubic)
control starting from home configuration. Once the configurations have been reached,
move the robot back to the home configuration.
If the input information is given in the Cartesian space (a position in the Cartesian space), then
we must use the inverse kinematics to compute the joint configurations. The computation Will
be done in four steps: :

1) Joint 𝑞 is computed from coordinates x and y of gripper point 𝑝 . Due to the non-zero term
𝑑 , the computation of 𝑞 must be obtained from the sum of two angles: the first as a
consequence of the angle of 𝑝 w.r.t. the reference frame 0 (shown in the figure). Then, we
need to compensate such angle due to distance 𝑑 from a simple trigonometric relation. On
the other hand, the point of the gripper wrist with respect to the robot, 𝑝 , can be computed
as:

𝑝 −𝑙 𝑑
l5 𝑞 = tan
,
+ sin
−𝑝 ,
𝑝 −𝑙 +𝑝
d5 l3 , ,

q1
𝑙 sin 𝑞 − 𝑙
𝑝 = 𝑝 + −𝑙 cos 𝑞
l1
X 0
x0
y0
l0
Y
2) Joint coordinates 𝑞 and 𝑞 , are computed from the triangle formed by links 2 and 3 of a
conventional robot arm:

𝑟= 𝑝 , +𝑝 , −𝑙 q3,O

l3 g
𝑧 =𝑝 , −ℎ
l2 q2
𝛼 = tan
𝑧 s
𝑟 ze b
a
Z
𝑠= 𝑟 +𝑧 r
h1
𝛾 = cos
𝑙 +𝑙 −𝑠
𝑞 =𝜋−𝛾 z0
,
2𝑙 𝑙
X x0
𝑠 +𝑙 −𝑙
𝛽 = cos 𝑞 =𝜋−𝛼−𝛽
2𝑠𝑙 l1
3) Finally, the angle of the third joint 𝑞 is computed from the inverse kinematics of the four-
bar mechanism:
l3,O

q3,O
e l4
l2 q2
y
q3
f

l3,I Z

𝑙 , sin 𝑞 ,
𝑒= 𝑙 , + 𝑙 − 2𝑙 , 𝑙 cos 𝑞 , 𝜓 = sin
𝑒

𝑒 +𝑙 , −𝑙 𝜋
𝜙 = cos 𝑞 = 𝜓+𝜙+ −𝑞
2𝑒𝑙 , 2
The following code proposes the definition of a new data type in a struct to include all required
robot geometric parameters, RobotParams_t and it also proposes another data type to indicate
a target position to reach in RobotPosition_t. Also, defines the function inverseKin so that from
a target configuration and the robot parameters, returns a goal configuration to reach (in an
array). Function moveJ combines the inverse kinematic with the point to point trajectory
control.
…
typedef struct
{
double l0;
double h1;
double l1;
double l2;
double l3;
double l3I;
double l3O;
double l4;
double l5;
} RobotParams_t;

typedef struct
{
double x;
double y;
double z;
} RobotPosition_t;
…
RobotParams_t params={…,…,…,…,{…}};
RobotPosition_t pT={…};

void inverseKin(const RobotPosition_t &target,const RobotParams_t &params, double *q);

void moveJ(const RobotServo_t robotServos[JOINTS], const double q0[JOINTS], const RobotPosition_t target, const float T, const
RobotParams_t &params);
…
void moveJ(const RobotServo_t robotServos[JOINTS], const float q0[JOINTS], const RobotPosition_t target, const float T, const
RobotParams_t &params)
{
//TODO: Call inverseKin to compute a target configuration qT, then call moveAbsJ
}
void inverseKin(const RobotPosition_t &target,const RobotParams_t &params, double *q)
{
//TODO: Return in q the values of the configuration for the given target position.
}

Compute three robot configurations from three target positions provided in mm and move
the robot to those configurations using the Point to Point trajectory control. Start from the
home position, where all servos are in a 90 position.
 Linear motion control with a trajectory can be done with a cubic trajectory, where the
trajectory displacement 𝑠 𝑡 is computed as before. The actual trajectory is computed
from a unitary vector 𝑝⃗ from the initial position, 𝑝 , that reaches the final position 𝑝 . The
initial speed will be zero at the beginning and the end of the trajectory, while in the
intermediate points the speed will be greater, as shown in the following figure:

𝑠(𝑡) 𝑝 𝑝⃗
𝑠

𝑠
0 T 𝑝

Therefore, the lineal trajectory is defined as: 𝑝 𝑡 = 𝑠 𝑡 𝑝⃗ + 𝑝

𝑠 𝑡 = 𝑎𝑡 + 𝑏𝑡 + 𝑐𝑡 + 𝑑 𝑠 0 =0 𝑠̇ 0 = 0
𝑠̇ 𝑡 = 3𝑎𝑡 + 2𝑏𝑡 + 𝑐 𝑠 𝑇 = 𝑝 −𝑝 𝑠̇ 𝑇 = 0
with
2 𝑝 −𝑝 3 𝑝 −𝑝 𝑝 −𝑝
𝑎=− 𝑏= 𝑐=0 𝑑=0 𝑝⃗ =
𝑇 𝑇 𝑝 −𝑝
Being the Euclidean distance operator:
𝑝 −𝑝 = 𝑝 , −𝑝 , + 𝑝 , −𝑝 , + 𝑝 , −𝑝 ,

To implement the linear control you need to compute the position of the gripper from a
given configuration, that is, the servo angles. As before, we explain the required
computations to implement the forward kinematics of the robot.
The separation of the wrist with respect to the joint 1 axis is computed as follows (in the
figure, when 𝑞 = and 𝑞 , = , the distance value is 𝑙 + 𝑙 ):

𝑟 = −𝑙 cos 𝑞 − 𝑙 cos 𝑞 + 𝑞 ,
On the other hand, the height of the gripper is computed from:

𝑝 , = ℎ + 𝑙 sin 𝑞 + 𝑙 sin 𝑞 + 𝑞 ,

The x and y coordinates of the gripper are:

𝑝 , =𝑙 + 𝑟+𝑙 +𝑙 sin 𝑞 − 𝑑 cos 𝑞
𝑝 , = − 𝑟+𝑙 +𝑙 cos 𝑞 − 𝑑 sin 𝑞
Link 3 angle, 𝑞 , is computed from the 4 linkage mechanism:

l3,O

q3,O m

t f l4
l2
q2
q3
j
l3,I Z

𝜋
𝜑 =𝑞 +𝑞 −
2
𝑙 , sin 𝜑
𝑓= 𝑙 , + 𝑙 − 2𝑙 , 𝑙 cos 𝜑 𝜏 = sin
𝑓

𝑓 +𝑙 , −𝑙 𝑞 =𝜏+𝜇
𝜇 = cos ,
2𝑓𝑙 ,
The following code proposes an implementation of two functions forwardKin y moveL.
Function forwardKin must implement the forward kinematics of the robot, while function
moveL, must implement the linear control with a cubic trajectory.

…
void forwardKin(const double q[JOINTS], const RobotParams_t &params, RobotPosition_t *target);
void moveL(const RobotServo_t robotServos[JOINTS], const double q0[JOINTS], const RobotPosition_t target, const float T, const
RobotParams_t &params);
…
void moveL(const RobotServo_t robotServos[JOINTS], const float q0[JOINTS], const RobotPosition_t target, const float T, const
RobotParams_t &params)
{
//TODO: Call forwardKin to compute the current gripper position from the given configuration. Then compute a target position and
move the gripper along the trajectory by sending proper servos commands using the inverse kinematics.
}
void forwardKin(const double q[JOINTS], const RobotParams_t &params, RobotPosition_t *target)
{
//TODO: Return in target the values of the position of the gripper.
}

Implement now the three consecutive movement using a linear control. At the end, the
robot must return to the initial configuration, also with a linear movement.
In order to move the robot with a circular movement in the 3D Cartesian space, we need
the position of three points. The first point, 𝑃 , corresponds to the actual position of robot,
while the other two points, 𝑃 and 𝑃 must be given. Obviously, the points must met some
conditions so that they belong to a 3D circle, such as they must not be aligned or repeated,
so we will assume that these conditions are met (checking those conditions is out of the
scope). The procedure is actually quite simple, we need to define a reference frame with
origin 𝑃 and with axis X pointing towards the direction 𝑃 = 𝑃 − 𝑃 and axis Z
perpendicular to vectors 𝑃 and 𝑃 = 𝑃 − 𝑃 . By doing this, we can express the original
points in the new reference frame, where the z coordinate is zero and therefore all points
will be contained in a 2D plane. If we extract the circle center 𝐶 and undo the
transformation to obtain the original 3D center, 𝐶. Once the center is obtained, we can
create a trajectory starting from 𝑃 and finishing in 𝑃 passing through point 𝑃 . The
temporal implementation for this trajectory can use a polynomic expression for the
travelled distance.
The maths with all these computations are explained below. The transformation of the
points is expressed with the following homogenous transformation matrix:
𝑋 𝑌 𝑍 𝑃
𝑇=
0 0 0 1
×
with 𝑋 = ,𝑍 = , Y = 𝑍 × 𝑋, being × the cross product.
×
The points transformed are:
𝑃 =𝑇 𝑃 𝑃
, 𝑃 =𝑇
1 1 1 1

In the new reference frame, the center must be located

in 𝐶 = ,
ℎ 0 , being 𝑃 = 𝑃 , 𝑃 , the
coordinates of the first point 𝑃 and ℎ the height of the
center point. From the distance relation to the center,
the second point 𝑃 = 𝑃 , 𝑃 , and the origin, we
can obtain the height:
𝑃, 𝑃,
𝑃 , − + 𝑃 , −ℎ = +ℎ
2 2

Therefore, the position of the cernter in the XY plane of the new reference frame and its
corresponding 3D position are computed as:

𝑃 𝑃,
𝐶= 𝑃, 𝑃 − 2, +𝑃 − 𝐶
=𝑇 𝐶
, , 2 0 𝑅= 𝐶
2 2𝑃 1 1
,

being R, the radius of the circle, that is, the distance of the center to the origin of the new
reference frame.
 𝑃
Finally, the circular trajectory 𝑃(𝑡) is obtained from:
cos 𝜃(𝑡) 𝜃(0)
𝑃(𝑡)
𝑃 𝑡 = 𝑅 sin 𝜃(𝑡) + 𝐶 = 𝑇 𝑃(𝑡)
1 1
0 𝑃
being 𝜃(𝑡) the angle along the circular trajectory evaluated 𝜃(𝑇) 𝑃
from 0 to T. This trajectory is not necessary linear and we
can demand some initial and final conditions for the speed
of the trajectory using, for instance a cubic expresión as
previously seen. In addition to this, we set the conditions
that the trajectory must start from 𝑃 and end at 𝑃 :

𝜃 𝑡 = 𝑎𝑡 + 𝑏𝑡 + 𝑐𝑡 + 𝑑 𝜃̇ 𝑡 = 3𝑎𝑡 + 2𝑏𝑡 + 𝑐

−𝐶
𝜃 0 = tan 𝜃̇ 0 = 0 𝜃̇ 𝑇 = 0
−𝐶

𝑃 −𝐶 ⋅𝐶 𝑃 −𝐶 ⋅𝐶
𝜃 𝑇 = cos − + 𝜃(0) or 𝜃 𝑇 = 2𝜋 − cos − +𝜃 0
𝑅 𝑅
2 𝜃 𝑇 −𝜃 0 3 𝜃 𝑇 −𝜃 0
with 𝑎 = − 𝑏= 𝑐=0 𝑑 = 𝜃(0)
𝑇 𝑇
Remark: One of the two solutions Will pass through point 𝑃 while the other condition will
not. Choose the right choice for your case.

Now, you are in position for implementing a function moveC, that generates a circular
trajectory from the actual robot configuration and two target points: target1 and target2,
given by their Cartesian coordinates. The reminder of input arguments of the function are
similar to the previous functions.

…
void moveC(const RobotServo_t robotServos[JOINTS], const float q0[JOINTS], const RobotPosition_t target1, const RobotPosition_t
target2, const float T, const RobotParams_t &params);
…
void moveC(const RobotServo_t robotServos[JOINTS], const float q0[JOINTS], const RobotPosition_t target1, const RobotPosition_t
target2, const float T, const RobotParams_t &params)
{
//TODO: Compute a ciruclar trajectory based on the current configuration, and target points 1 and 2
}
…

Now implement a circular movement in the XY, YZ or XZ plane and with a free-choice
radius, as well as the amount of rotation within the circle. If you want to make a complete
circle, you must do it in two steps.
 From the inverse kinematics, we know that:

𝑝 −𝑙, 𝑑 𝑝 𝑑
𝑞 = tan + sin = tan + sin
−𝑝 , −𝑝
𝑝 , −𝑙 +𝑝 ,
𝑝 +𝑝

Its derivative can be computed as: −𝑝 𝑑 𝑝

𝐽 , = −
𝑝 +𝑝
𝑝 +𝑝 𝑝 +𝑝 −𝑑
𝑞̇ = 𝐽 , 𝐽 , 0 ⋅ 𝑝̇ = 𝐽 ⋅ 𝑝̇
𝑝 𝑑 𝑝
𝐽 , = −
𝑝 +𝑝
𝑝 +𝑝 𝑝 +𝑝 −𝑑
The jacobian of the wrist point can be computed as:
−𝑙 sin 𝑞 − 𝑙 𝐽 , =1 − 𝐽 , 𝑙 cos 𝑞
𝑝 =𝑝 + 𝑙 cos 𝑞 𝐽 , 𝐽 , 0
𝐽 = 𝐽 𝐽 , =−𝐽 , 𝑙 cos 𝑞
0 , 𝐽 , 0
𝑙 cos 𝑞 0 0 1 𝐽 , =−𝐽 , 𝑙 sin 𝑞
𝑝̇ = 𝑝̇ − 𝑙 sin 𝑞 ⋅ 𝑞̇ = 𝐽 ⋅ 𝑝̇ 𝐽 =1 − 𝐽 𝑙 sin 𝑞
, ,
0
Now, we can compute the jacobians of intermediate variables used in the inverse kinematics
computation:

𝑝 , 𝐽 , +𝑝 , 𝐽 , 𝑝 , 𝐽 , +𝑝 , 𝐽 ,
𝑟= 𝑝 , +𝑝 , −𝑙 𝑟̇ = 0 ⋅ 𝑝̇ = 𝐽 ⋅ 𝑝̇
𝑟+𝑙 𝑟+𝑙
𝑧 𝐽 , 𝑧 𝐽 , 𝑟
𝑧 𝛼̇ = − − ⋅ 𝑝̇ = 𝐽 ⋅ 𝑝̇
𝑧 =𝑝 , −ℎ 𝛼 = tan 𝑠 𝑠 𝑠
𝑟
𝑟𝐽 , 𝑟𝐽 , 𝑧
𝑙 +𝑙 −𝑠 𝑠̇ = ⋅ 𝑝̇ = 𝐽 ⋅ 𝑝̇
𝑠= 𝑟 +𝑧 𝛾 = cos 𝑠 𝑠 𝑠
2𝑙 𝑙
𝑠
𝛾̇ = ⋅ 𝐽 ⋅ 𝑝̇ = 𝐽 ⋅ 𝑝̇
𝑠 +𝑙 −𝑙 𝑙 𝑙 sin 𝛾
𝛽 = cos
2𝑠𝑙 1 1
𝛽̇ = − ⋅ 𝐽 ⋅ 𝑝̇ = 𝐽 ⋅ 𝑝̇
𝑠 tan 𝛽 𝑙 sin 𝛽
𝑞 , =𝜋−𝛾

𝑞 =𝜋−𝛼−𝛽 𝑞̇ = − 𝐽 + 𝐽 ⋅ 𝑝̇ = 𝐽 ⋅ 𝑝̇

𝑙 , 𝑙 sin 𝑞 ,
𝑒= 𝑙 , + 𝑙 − 2𝑙 , 𝑙 cos 𝑞 , 𝑒̇ = − ⋅ 𝐽 ⋅ 𝑝̇ = 𝐽 ⋅ 𝑝̇
𝑒

𝑙 sin 𝑞 1
𝜓 = sin
, , 𝜓̇ = − ⋅ 𝑙 , cos 𝑞 , 𝐽 + sin 𝜓 𝐽 ⋅ 𝑝̇ = 𝐽 ⋅ 𝑝̇
𝑒 𝑒 cos 𝜓
𝑙 , cos 𝜙 − 𝑒
𝑒 +𝑙 , −𝑙 𝜙̇ = ⋅ 𝐽 ⋅ 𝑝̇ = 𝐽 ⋅ 𝑝̇
𝜙 = cos 𝑒𝑙 , sin 𝜙
2𝑒𝑙 ,
𝜋 𝑞̇ = 𝐽 + 𝐽 − 𝐽 ⋅ 𝑝̇ = 𝐽 ⋅ 𝑝̇
𝑞 =𝜓+𝜙+ −𝑞
2
 In summary, we can compute the velocities of the servos bsaed on the velocities of the end
effector by computing its jacobian. In this way, we can implement a kinematic control law so
that the end effector follows a trajectory given by 𝑝 ∗ 𝑡 and its velocity 𝑝̇ ∗ 𝑡 , being 𝑘 the
proportional gain of the controller:

𝑞̇ 𝑡 = 𝐽 𝑝 𝑡 ⋅ 𝑘 𝑝∗ 𝑡 − 𝑝 𝑡 + 𝑝̇ ∗ 𝑡

Please, note that since we have computed the jacobian from inverse kinematic expressions,
we do not need to compute the inverse of the jacobian. It is also important to remark that
since the robot uses position servos, we need to integrate the control law on evary stpe to
actually compute the reference to set the servo position.

A trajectory XY describing an infinite shape of size wxxwy and centre cx,cy,cz and duration T
seconds would have the following timed expression:

𝑤 4𝜋𝑡 𝜋 2𝑤 𝜋 4𝜋𝑡 𝜋
cos − +𝑐 − sin −
2 𝑇 2 𝑇 𝑇 2
𝑝∗ 𝑡 = 𝑤 2𝜋𝑡 𝑝̇ ∗ 𝑡 = 𝑤 𝜋 2𝜋𝑡
sin −𝜋 +𝑐 cos −𝜋
2 𝑇 𝑇 𝑇
0 0
The following code proposes the implementation of three function infiniteTraj,
inverseDiffKin and kinControl. Function infiniteTraj is a function that must provide with
the position and velocity of the trajectory for a given time instant, function inverseDiffKin
must return the velocity of servos for a given velocity of the end effector, while function
kinControl computes the proportional control law with velocity feed-forward.

…
typedef struct
{
double vx;
double vy;
double vz;
} RobotVelocity_t;

typedef struct
{
double cx;
double cy;
double cz;
double wx;
double wy;
double wz;
double T;
} TrajectoryParams_t;

typedef struct
{
double gain;
} ControlParams_t;

typedef void (*trajectoryPtr)(const double t , const TrajectoryParams_t &trajParams, RobotPosition_t* target, RobotVelocity_t
*vel_target);

void infiniteTraj(const double t , const TrajectoryParams_t &trajParams, RobotPosition_t* target, RobotVelocity_t *vel_target)
{
//TODO: Compute position and velocity of the infinite shape trajectory for the given time.
}

void inverseDiffKin(const RobotPosition_t &end_effector, const RobotVelocity_t &vel_end_effector, const RobotParams_t &params,
double *dq);
void kinControl(const RobotServo_t robotServos[JOINTS], const double q0[JOINTS], const trajectoryPtr trajectory, const double T,
const RobotParams_t &params, const TrajectoryParams_t &trajParams, const ControlParams_t &controlParams);
…
void setup()
{
…
kinControl(robotServos, q0, &infiniteTraj, T, params,trajParams,controlParams); //How to call “kinControl”
…
}
…
void kinControl(const RobotServo_t robotServos[JOINTS], const double q0[JOINTS], const trajectoryPtr trajectory, const double T,
const RobotParams_t &params, const TrajectoryParams_t &trajParams, const ControlParams_t &controlParams)
{
//TODO: Call forward kinematics to compute end effector position based on the current joint position. Call “trajectory” to evaluate
the target position and velocity at each time instant and compute the desired end effector velocity. Compute joint velocities using
“inverseDiffKin” and apply next joint angles considering the computed velocities. Repeat until the time has ellapsed.
}

Implement the movements so that the robot follows a trajectory with the ∞ shape. To
validate the control, the robot and the trajectories must start at different positions.