1

Modeling of a Pantograph Haptic Device

Islam S. M. Khalil and Mohamed Abu Seif

Kinematics of the Pantograph Haptic Device

The pantograph haptic device consists of 5 links with lengths li for i = 1, . . . , 5. First, we assign frames
of reference to each link, as shown in Fig. 1. The holonomic constrain of the pantograph mechanisms is
given by
l1 a1 + l2 b1 − l3 c1 − l4 d1 − l0 n1 = 0, (1)
where li for i = 1, . . . , 5 is the length of links N, A, B, C, and D, respectively. We assume that link N is
the frame of reference and calculate the rotation matrices of each link with respect to frame N as follows:
 
cos q1 − sin q1 0
R =  sin q1
N A
cos q1 0 , (2)
0 0 1

where N RA is the rotation matrix of link A with respect to the frame of reference N, and q1 is the
generalized coordinate of the link A. Similarly, the rotation matrix (N RB ) of link B with respect to the
frame of reference N is given by  
cos q2 − sin q2 0
R =  sin q2
N B
cos q2 0 , (3)
0 0 1
where q2 is the generalized coordinate of link B. Rotation matrices of bodies C (N RC ) and D (N RD ) with
respect to N are given respectively using
   
cos q3 − sin q3 0 cos q4 − sin q4 0
R =  sin q3
N C
cos q3 0 and N RD =  sin q4 cos q4 0 , (4)
0 0 1 0 0 1

where q3 and q4 are the generalized coordinates of links C and D, respectively. All rotations of our
pantograph haptic device are in-plane. Therefore, the angular velocities of the links are calculated by
inspection as follows:
N A
ω = q̇1 n3 , (5)
where N ω A is the angular velocity of link A with respect to the frame of reference N, and q̇1 is the
time-derivative of q1 . Similarly, the angular velocities of links B, C, and D are given by
N
ω B = q̇2 n3 , N
ω C = q̇3 n3 , and N ω D = q̇4 n3 , (6)

where N ω B , N ω C , and N ω D are the angular velocities of links B, C, and D with respect to the frame of
reference N, respectively. n3 is a unit vector perpendicular to the page. Now we calculate the relation
( )T ( )T
between the task space (x = x y ) and joint space (q = q1 q2 q3 q4 ) of the pantograph haptic
device, where x and y are the Cartesian coordinates of point e with respect to the frame of reference N,
as shown in Fig. 1.
 2

Figure 1. Kinematics of the pantograph haptic device. The position of the end-effector
(point of interaction between the operator and the pantograph) is denoted using e, and is
represented using position vector roe .

0.1 Configuration Level Kinematics

In order to solve for x and y using q1 and q4 (the active angles of the pantograph haptic device), we
define the following vector:
roe = xn1 + yn2 , (7)
= l1 a1 + l2 b1 , (8)
= l1 (cos q1 n1 + sin q1 n2 ) + l2 (cos q2 n1 + sin q2 n2 ) , (9)
= (l1 cos q1 + l2 cos q2 ) n1 + (l1 sin q1 + l2 sin q2 ) n2 , (10)
where roe is the position vector of point e in the frame of reference N. Using (10), we obtain the following
2 scalar equations:
x = l1 cos q1 + l2 cos q2 , (11)
y = l1 sin q1 + l2 sin q2 . (12)
Equations (11) and (12) represent the forward kinematics of the pantograph haptic device. Now we need
to solve for q2 in terms of q1 using the holonomic constrain (1). This step is necessary since q2 is a passive
angle and has to be represented using the active angles of the device (q1 and q4 ). Using (1), we obtain
l1 (cos q1 n1 + sin q1 n2 )+l2 (cos q2 n1 + sin q2 n2 )−l3 (cos q3 n1 + sin q3 n2 )−l4 (cos q4 n1 + sin q4 n2 )−lo n1 = 0,
(13)
Equation (13) provides the following two scalar equations:
l1 cos q1 + l2 cos q2 − l3 cos q3 − l4 cos q4 − lo = 0, (14)
l1 sin q1 + l2 sin q2 − l3 sin q3 − l4 sin q4 = 0. (15)
 3

Solving (14) and (15) using Newton-Raphson method. We make the following definitions:
( ) ( )
f1 l cos q1 + l2 cos q2 − l3 cos q3 − l4 cos q4 − lo
f= = 1 . (16)
f2 l1 sin q1 + l2 sin q2 − l3 sin q3 − l4 sin q4

All link length and q1 and q4 are know, therefore we solve for q2 and q3 . The passive angles are defined
[ ]T
as, qp = q2 q3 . Using Newton-Raphson method, qp is calculated using
δf
qi+1
p = qip − f. (17)
δqp
Once q2 is solved interns of q1 and q4 , the configuration level kinematics is completed.

0.2 Configuration Level Inverse Kinematics

Now we solve for q1 and q4 based on x and y of point e. We rewrite (11) and (12) as follows:

x − l1 cos q1 − l2 cos q2 = 0, (18)

y − l1 sin q1 − l2 sin q2 = 0. (19)

Recall the holonomic constrain and define the following scalar equations:

l1 cos q1 + l2 cos q2 − l3 cos q3 − l4 cos q4 − lo = 0, (20)

l1 sin q1 + l2 sin q2 − l3 sin q3 − l4 sin q4 = 0. (21)

Solve (18), (19), (20), and (21) using Newton-Raphson.

0.3 Motion Level Forward Kinematics

Taking the time derivative of the position vector of point e roe in the frame of reference N yields

N d oe N
r = ve = ẋn1 + ẏn2 (22)
dt
Nd d d
= (l1 a1 + l2 b1 ) = l1N a1 + l2N b1 (23)
dt
( dt
) ( dt )
A d B d
= l1 a1 + ω × a1 + l2
N A
b1 + ω × b1
N B
(24)
dt dt
= l1 q̇1 n3 × a1 + l2 q̇2 n3 × b1 (25)
= l1 q̇1 a2 + l2 q̇2 b2 (26)
= l1 q̇1 (− sin q1 n1 + cos q1 n2 ) + l2 q̇2 (− sin q2 n1 + cos q2 n2 ) (27)

Finally, the forward level kinematics is given by

ẋ = −l1 sin q1 q̇1 − l2 sin q2 q̇2 , (28)

ẏ = l1 cos q1 q̇1 + l2 cos q2 q̇2 . (29)

Arranging the terms in a linear form we obtain

[ ] [ ][ ]
ẋ −l1 sin q1 −l2 sin q2 q̇1
ẋ = = = J(q)q̇. (30)
ẏ l1 cos q1 l2 cos q2 q̇2

where J(q) is the Jacobian matrix of the pantograph haptic device.

 4

Figure 2. Dynamics of the pantograph haptic device. The red dashed vectors represent
position vectors to the center of masses.

0.4 Closed-Form Solution of the Inverse Kinematics Problem

Newton-Raphson method is not recommended in implementation since it is an iterative method. There-
fore, we devise the following closed-form solution of the inverse-kinematics:
( √ ) ( √ )
−1 −A + A2 + B12 − C12 −1 −A − A2 + B22 − C22
q1 = 2 tan and q2 = 2 tan , (31)
C1 − B1 C2 − B2

where the variables A, B1 , B2 , C1 , and C2 are given by

A = −2l2 y, (32)
( )
l0
B1 = −2l2 x − , (33)
2
( )
l0
B2 = −2l2 x + , (34)
2
l0
C1 = x2 + y 2 + ( )2 + l22 − l12 − l0 x, (35)
2
l0 2
C2 = x + y + ( ) + l22 − l12 + l0 x.
2 2
(36)
2

1 Dynamics of the Pantograph Haptic Device

The position vector of the center of mass of link A is given by
l1 l1 l1
r0A0 = a1 = cos q1 n1 + sin q1 n2 , (37)
2 2 2
 5

where r0A0 is the position vector of the center of mass of link A. The position vector of the center of mass
of link B is given by
l2
r0B0 = l1 a1 + b1 , (38)
2
l2 l2
= l1 cos q1 n1 + l1 sin q1 n2 + cos q2 n1 + sin q2 n2 , (39)
( ) 2( 2 )
l2 l2
= l1 cos q1 + cos q2 n1 + l1 sin q1 + sin q2 n2 . (40)
2 2

Similarly, the position vector of the center of mass of link D is give by

l4
r0D0 = l0 n1 + d1 , (41)
2
l4
= l0 n1 + (cos q4 n1 + sin q4 n2 ) , (42)
( 2 )
l4 l4
= l0 + cos q4 n1 + sin q4 n2 . (43)
2 2

Finally, the position vector of the center of mass of link C is given by

l3
r0C0 = l0 n1 + l4 d1 + c1 , (44)
2
l3 l3
= l0 n1 + l4 cos q4 n1 + l4 sin q4 n2 + cos q3 n1 + sin q3 n2 , (45)
( ) (2 2 )
l3 l3
= l0 + l4 cos q4 + cos q3 n1 + l4 sin q4 + sin q3 n2 . (46)
2 2

Now we calculate the velocity vector of the calculated vectors in the frame of reference N as follows:
d 0A0 l1 l1
N A0
v =N r = − q̇1 sin q1 n1 + q̇1 cos q1 n2 , (47)
dt 2 2
where N vA0 is the velocity vector of the center of mass of link A in the frame of reference N. The velocity
vector of the center of mass of link B is calculated using
( ) ( )
N d 0B0 l2 l2
N B0
v = r = − l1 q̇1 sin q1 + q̇2 sin q2 n1 + l1 q̇1 cos q1 + q̇2 cos q2 n2 , (48)
dt 2 2

where N vB0 is the velocity vector of the center of mass of link B in the frame of reference N. Similarly,
the velocity vector of the center of mass of link D is given by
( ) ( )
N d 0D0 l4 l4
N D0
v = r =− q̇4 sin q4 n1 + q̇4 cos q4 n2 , (49)
dt 2 2

where N vD0 is the velocity vector of the center of mass of link D in the frame of reference N. Finally, the
velocity vector of the center of mass of link C is calculated as follows:
( ) ( )
N d 0C0 l3 l3
N C0
v = r = − l4 q̇4 sin q4 + q̇3 sin q3 n1 + l4 q̇4 cos q4 + q̇3 cos q3 n2 , (50)
dt 2 2

where N vC0 is the velocity vector of the center of mass of link C in the frame of reference N.
 6

Link A is in rational motion only. Therefore, its kinetic energy (TA ) is calculated using

1 mA l12 2
TA = q̇ , (51)
2 3 1
Link D exhibits only rotational motion. Therefore, its kinetic energy (TD ) is calculated using

1 mD l42 2
TD = q̇ . (52)
2 3 4
The kinetic energy of Link B is due to translation and rotation and can be calculated as
1 1
TB = ICB q̇22 + mN vB0 ·N vB0 , (53)
2 2 B ( )
1 mB l22 2 1 l22 2 l1 l2
= 2 2
q̇ + mB l1 q̇1 + q̇2 + q̇1 q̇2 cos (q1 − q2 ) . (54)
2 12 2 2 4 2

Similarly, the kinetic energy of Link C is due to translation and rotation and can be calculated as
1 1
TC = ICC q̇32 + mN vC0 ·N vC0 , (55)
2 2 C ( )
1 mC l32 2 1 l32 2 l3 l4
= 2 2
q̇ + mC l4 q̇4 + q̇3 + q̇3 q̇4 cos (q3 − q4 ) . (56)
2 12 3 2 4 2

The total kinetic energy of the pantograph haptic device is

T = TA + TB + TC + TD . (57)

Finally, the equations of motion of the pantograph haptic device are

( )
d ∂T ∂T
− = Qi for i = 1, . . . , 4, (58)
dt ∂ q̇i ∂qi

where Qi is the generalized force associated with the generalized coordinate i.

The generalized force associated with q1 is
∂roe
Q 1 = Fo · = −fox l1 sin q1 + foy l1 cos q1 + T1 , (59)
∂q1
where Fo is the interaction force of the operator with the pantograph haptic device at a point e. Further,
fox and foy are the components of Fo in the frame of reference, along n1 and n2 , respectively. T1 is the
control torque input exerted on the generalized coordinate q1 . The generalized force associated with q2
is
∂roe
Q2 = Fo · = −fox l2 sin q2 + foy l2 cos q2 , (60)
∂q2
The generalized force associated with q3 is
∂roe
Q3 = Fo · = −fox l3 sin q3 + foy l3 cos q3 , (61)
∂q3
The generalized force associated with q4 is
∂roe
Q 4 = Fo · = −fox l4 sin q4 + foy l4 cos q4 + T2 . (62)
∂q4
 7

Figure 3. Generalized forces of the pantograph haptic device.

Now we recall (58) and determine the equations of motion of the pantograph haptic device as follows:
(m ) mB l1 l2 mB l1 l2 mB l1 l2
A
+ mB l12 q̈1 + q̈2 cos (q1 − q2 ) − (q̇1 − q̇2 )q̇2 sin (q1 − q2 ) + q̇1 q̇2 sin (q1 − q2 ) =
3 4 4 4
(63)
−fox l1 sin q1 + foy l1 cos q1 + T1 ,

mB l22 mB l1 l2 mB l1 l2 mB l1 l2
q̈2 + q̈1 cos (q1 − q2 ) − (q̇1 − q̇2 ) q̇1 sin (q1 − q2 ) − q̇1 q̇2 sin (q1 − q2 ) = (64)
3 4 4 4
−fox l2 sin q2 + foy l2 cos q2 ,

mc l32 mc l3 l4 mC l3 l4 mC l3 l4
q̈3 + q̈4 cos (q3 − q4 ) − (q̇3 − q̇4 ) q̇4 sin (q3 − q4 ) − q̇3 q̇4 sin (q3 − q4 ) = (65)
3 4 4 4
−fox l3 sin q3 + foy l3 cos q3 ,

(m ) mC l3 l4 mC l3 l4 mC l3 l4
D
+ mC l42 q̈4 + q̈3 cos (q3 − q4 ) − (q̇3 − q̇4 )q̇3 sin (q3 − q4 ) + q̇3 q̇4 sin (q3 − q4 ) =
3 4 4 4
(66)
−fox l4 sin q4 + foy l4 cos q4 + T2 ,
 8

Based on (63), (64), (65), and (66), we define the following inertia and nonlinear damping matrices:
 ( mA ) 2 
3 + mB l1
mB l1 l2
4 cos (q1 − q2 ) 0 0
 mB l1 l2 mB l22 
 cos (q1 − q2 ) 0 0 
M(q) =  4 3
mc l32  , (67)
 0 0 m c l3 l4
cos (q − q ) 
3 ( mD
4 3 ) 2 4
0 0 m C l3 l4
4 cos (q3 − q4 ) 3 + mC l4

where M(q) is the inertia matrix of the pantograph haptic device. Further, the nonlinear damping matrix
is given by  m B l1 l2 
− 4 (q̇1 − q̇2 )q̇2 sin (q1 − q2 ) + mB4l1 l2 q̇1 q̇2 sin (q1 − q2 )
− mB l1 l2 (q̇1 − q̇2 ) q̇1 sin (q1 − q2 ) − mB l1 l2 q̇1 q̇2 sin (q1 − q2 )
D(q, q̇) =  4 4
− mC l3 l4 (q̇3 − q̇4 ) q̇4 sin (q3 − q4 ) − mC l3 l4 q̇3 q̇4 sin (q3 − q4 )
 (68)
4 4
− mC4l3 l4 (q̇3 − q̇4 )q̇3 sin (q3 − q4 ) + mC4l3 l4 q̇3 q̇4 sin (q3 − q4 )
  
mB l1 l2
4 q̇2 s12 − mB4l1 l2 (q̇1 − q̇2 )s12 0 0 q̇1
− l1 l2 (q̇1 − q̇2 )s12
m B
− mB4l1 l2 q̇1 s12 0 0  q̇2 
=  4    (69)
0 0 m C l3 l4
4 q̇4 s34 − mC4l3 l4 (q̇3 − q̇4 ) s34  q̇3 
0 0 − 4 (q̇3 − q̇4 ) s34
m C l3 l4 mC l3 l4
q̇3 s34 q̇4
| {z 4
}
C(q,q̇)

Finally, the control torque input (T) and interaction Th with the operator are defined as follows:
   
T1 −fox l1 sin q1 + foy l1 cos q1
0 −fox l2 sin q2 + foy l2 cos q2 
T=  
 0  and Th = −fox l3 sin q3 + foy l3 cos q3  .
 (70)
T4 −fox l4 sin q4 + foy l4 cos q4
Now the equation of motion of the pantograph haptic device can be written in the following compact
form:
M(q)q̈ + D(q, q̇) = T + Th . (71)
It can also be represented in the following form:
M(q)q̈ + C(q, q̇)q̇ = T + Th . (72)
T
where M(q) is the inertia matrix (M(q) = M(q) > 0). Further, C(q, q̇)q̇ accounts for the centrifugal
( )T ( )
and Coriolis forces, and is related to the inertia matric via, Ṁ(q) − 2C(q, q̇) = − Ṁ(q) − 2C(q, q̇) .

References
1. I. S. M. Khalil, E. Globovic, and A. Sabanovic, “High precision motion control of parallel robots
with imperfections and manufacturing tolerances”, in Proceedings of the IEEE International Con-
ference on Mechatronics (ICM), Istanbul, Turkey, pages 39-44, April 2011.
2. I. S. M. Khalil, V. Magdanz, S. O. Sanchez, O. G. Schmidt, and S. Misra, “Biocompatible, accurate,
and fully autonomous: A sperm-driven micro-bio-robot”, Journal of Micro-Bio Robotics, vol. 9,
no. 3-4, pp. 79-86, August 2014.
3. K. Youakim, M. Ehab, O. Hatem, S. Misra, and I. S. M. Khalil, “Paramagnetic microparticles
sliding on a surface: characterization and closed-loop motion control”, in Proceedings of the IEEE
International Conference on Robotics and Automation (ICRA), Seattle, USA, pages 4068-4073,
May-June 2015.
 9

4. I. S. M. Khalil, B. E. Wissa, B. G. Salama, and S. Stramigioli, “Wireless Motion Control of Para-

magnetic Microparticles using a Magnetic-Based Robotic System with an Open-Configuration,” in
Proceedings of the International Conference on Manipulation, Manufacturing and Measurement on
the Nanoscale (3M Nano), pp. 190-196, Changchun, China, October 2015.