The Pantograph Mk-II: A Haptic Instrument - CIM - McGill University

Proc. IROS 2005, IEEE/RSJ Int. Conf. Intelligent Robots and Systems, pp. 723-728.
We find
‖P 2 − P h ‖ = (a2 2 − a 2 3 + ‖P 4 − P 2 ‖ 2 )
,
(2‖P 4 − P 2 ‖)
(3)
P h = P 2 + ‖P 2 − P h ‖
‖P 2 − P 4 ‖ (P 4 − P 2 ),
√
(4)
‖P 3 − P h ‖ = a 2 2 − ‖P 2 − P h ‖ 2 . (5)
The end effector position P 3 (x 3 , y 3 ) is then given by
x 3 = x h ± ‖P 3 − P h ‖
‖P 2 − P 4 ‖ (y 4 − y 2 ), (6)
y 3 = y h ∓ ‖P 3 − P h ‖
‖P 2 − P 4 ‖ (x 4 − x 2 ). (7)
The useful solution has a positive sign in Eq. (6) and negative
sign in Eq. (7). Since in the workspace x 4 < x 2 , the solution
with a negative sign yields larger y.
Fig. 3.
Model of the kinematics used to compute the direct problem.
B. Inverse Kinematics
Parallel manipulators frequently have an inverse kinematics
problem that is simpler than the direct kinematics problem.
The Pantograph is no exception. The problem is to find the
angles θ 1 and θ 5 given the position of point P 3 . A pentagon
can be divided into three triangles, see Fig. 4 which makes
the solution straightforward:
where
θ 1 = π − α 1 − β 1 , θ 5 = α 5 + β 5 , (8)
(
)
a 2 1 − a 2 2 + ‖P 1 , P 3 ‖
α 1 = arccos √ , (9)
2a 1 ‖P1 , P 3 ‖
β 1 = atan2 (y 3 , −x 3 ), (10)
(
)
a 2 4 − a 2 3 + ‖P 5 , P 3 ‖
β 5 = arccos √ , (11)
2a 4 ‖P5 , P 3 ‖
α 5 = atan2 (y 3 , x 3 + a 5 ). (12)
This solves the inverse kinematics for a generic Pantograph
with arm lengths a i , as long as the device is in a configuration
such that α 1 > 0 and β 5 > 0, which puts it in the permitted
workspace.
Fig. 4.
C. Differential Kinematics
Dividing the pentagon into three triangles.
The Jacobian matrix can be found by direct differentiation
of the direct kinematic map with respect to the actuated joints
θ 1 and θ 5 :
J =
[ ]
∂x3 /∂θ 1 ∂x 3 /∂θ 5
=
∂y 3 /∂θ 1 ∂y 3 /∂θ 5
[ ]
∂1 x 3 ∂ 5 x 3
∂ 1 y 3 ∂ 5 y 3
(13)
where ∂ i· denotes the partial derivative with respect to θ i . Let
d = ‖P 2 − P 4 ‖, b = ‖P 2 − P h ‖ and h = ‖P 3 − P h ‖.
Applying the chain rule to Eqs. (6) and (7):
∂ 1 x 2 = a 1 sin(θ 1 ), ∂ 1 y 2 = a 1 cos(θ 1 ), (14)
∂ 5 x 4 = a 4 sin(θ 5 ), ∂ 5 y 4 = a 4 cos(θ 5 ), (15)
∂ 1 y 4 = ∂ 1 x 4 = ∂ 5 y 2 = ∂ 5 x 2 = 0, ∂ i h = −b∂ i b/h (16)
∂ i d = (x 4 − x 2 )(∂ i x 4 −∂ i x 2 )+(y 4 − y 2 )(∂ i y 4 −∂ i y 2 )
(17)
d
∂ i b = ∂ i d − ∂ id(a 2 2 − a 2 3 + d 2 )
2d 2 (18)
∂ i y h = ∂ i y 2
+ ∂ ib d − ∂ i d b
d 2 (y 4 − y 2 ) + b d (∂ iy 4 − ∂ i y 2 ) (19)
∂ i x h = ∂ i x 2
+ ∂ ib d − ∂ i d b
d 2 (x 4 − x 2 ) + b d (∂ ix 4 − ∂ i x 2 ) (20)
∂ i y 3 = ∂ i y h
− h d (∂ ix 4 − ∂ i x 2 ) − ∂ ih d − ∂ i d h
d 2 (x 4 − x 2 ) (21)
∂ i x 3 = ∂ i x h
+ h d (∂ iy 4 − ∂ i y 2 ) + ∂ ih d − ∂ i d h
d 2 (y 4 − y 2 ) (22)
D. Kinematic Conditioning
All entries of the Jacobian have the dimension of lengths
mapping angular velocities ω = [ θ ˙ 1 θ˙
5 ] ⊤ to linear velocities
v = [ x˙
3 y˙
3 ] ⊤ : v = J ω. Thus, the 2-norm of the Jacobian
matrix (which also is a length) has the physical meaning of
scaling the sensor nominal resolution to the nominal resolution
of the device. The Jacobian matrix is well conditioned on
all the workspace and the device becomes isotropic at (Fig. 5):
(
)
θ 1 iso =arccos − 25
126 + 25√ 2
, θ 5 iso = π − θ 1 iso (23)
42
corresponding to the point P iso ≃ (−12.5, 101.2) in Fig. 5.
At this point the two distal links intersect orthogonally at