Non-holonomic constraints-based adaptive trajectory tracking controller for a multi-joint snake robot

Mechanical model

In this part, the mechanical model of the multi-joint snake robot with non-holonomic constraints is established.^24–26 The prototype structure of the snake robot consists of several link modules, which are connected by active revolute joints. These active joints enable precise control of the robot’s movements in the XY plane, allowing it to perform complex motion patterns. The joint drives the link to swing alternately so that the body of the robot presents a S-shape, to obtain the forward power. To establish the mathematical model of the snake robot, we consider that the robot is composed of N links with a length of 2 h. There is a joint between the links, the joint steering gear controls the drive of the link, and there are auxiliary wheels in the middle of each link. In the dynamic model of the multi-joint snake robot, the width of joints and links are not considered temporarily. A simplified model of the robot dynamics is shown in Figure 1.

OPEN IN VIEWER

Figure 1.

Dynamic model of the multi-joint snake robot.

The directional angle of the j ∈ { 1 , … , N } th link in the multi-joint snake robot is θ j , which is the angle between the link and the horizontal direction. ²⁷ The set of link angles is θ = [ θ 1 ⋯ θ N ] T ∈ R N . The directional angle of the j ∈ { 1 , … , N } th joint in the robot is ϕ j = θ j − θ j + 1 , which is the angle of rotation between adjacent links. ²⁷ The set of joint angles is ϕ = [ ϕ 1 , ⋯ , ϕ N - 1 ] T ∈ R N - 1 . The joint centroid is p = [ p x , p y ] T . The link angles velocity of the robot is v θ = [ v θ 1 , ⋯ v θ N ] T ∈ R N , the joint angles velocity of the robot is v ϕ = [ v ϕ 1 , ⋯ v ϕ N − 1 ] T ∈ R N − 1 , the tangent vector velocity of the robot is v t ∈ R N , the normal vector velocity of the robot is v n ∈ R N , and the joint mass of the robot is m. The control input is u = [ u 1 , ⋯ u N − 1 ] T ∈ R N − 1 , which is the rotational torque of the joints. The centroid angle is θ = 1 N ∑ j = 1 N θ j . The viscous friction coefficients of the system are μ 1 , μ 2 > 0 , and the size of the viscous friction coefficients is affected by the geographical environment. The rotation coefficients of the joint are λ 1 , λ 2 > 0 .

A simplified model of the multi-joint snake robot with non-holonomic constraints is established as (1).^10,11

{ ϕ · = v ϕ θ · = v θ p · x = v t cos θ − v n sin θ p · y = v t sin θ + v n cos θ v · ϕ = − μ 1 m v ϕ + μ 2 m v t A e D T ϕ + 1 m D D T u v · θ = − λ 1 v θ + λ 2 N − 1 v t e T ϕ v · t = − μ 1 m v t + 2 μ 2 Nm v n e T ϕ - μ 2 Nm ϕ T A e D ¯ v ϕ v · n = − μ 1 m v n + 2 μ 2 Nm v t e T ϕ

(1)

where, A e = [ 1 1 ⋱ ⋱ 1 1 ] ∈ R ( N − 1 ) × N , D = [ 1 − 1 ⋱ ⋱ 1 − 1 ] ∈ R ( N − 1 ) × N , D ¯ = D T ( D D T ) − 1 , e = [ 1 , ⋯ , 1 ] T ∈ R N − 1 .

Coordinate transformation

In the snake robot dynamic model (1), v θ and v n will be affected by ϕ , so the change of the robot body shape will affect the direction and normal velocity of the robot. These motion characteristics lead to the complexity of the design of the adaptive trajectory tracking controller. To simplify the design of the controller, the dynamic model of the multi-joint snake robot with non-holonomic constraints is optimized by coordinate transformation (2). In the process of a multi-joint snake robot rotating without lateral force, ²⁸ the centroid position of the robot will move a certain distance along the tangent direction, and the new centroid position of the robot is p ^ = [ p ^ x , p ^ y ] T ∈ R 2 . After coordinate transformation, the simplified model of the multi-joint snake robot with non-holonomic constraints can be written as (3). This model can make the motion path of the robot converge to the target trajectory and eliminate the lateral offset when the snake robot turns.

{ p ^ x = p x + ε cos θ p ^ y = p y + ε sin θ v ^ n = v n + ε v θ

(2)

where ε = − 2 ( N − 1 ) μ 2 Nm λ 2 .

{ ϕ · = v ϕ θ · = v θ p ^ x = v t cos θ − v ^ n sin θ p ^ y = v t sin θ + v ^ n cos θ v · ϕ = − μ 1 m v ϕ + μ 2 m v t A e D T ϕ + 1 m D D T u v · θ = − λ 1 v θ + λ 2 N − 1 v t e T ϕ v · t = − μ 1 m v t + 2 μ 2 Nm v n e T ϕ - μ 2 Nm ϕ T A e D ¯ v ϕ v ^ n = X v θ − Y v ^ n

(3)

where X = ε ( μ 1 m − λ 1 ) and Y = μ 1 m are auxiliary simplified calculation constants.

Gait control

The forward motion of the robot is realized by controlling the swing amplitude and swing frequency of each joint of the multi-joint snake robot. ²⁰ The motion function of the j ∈ { 1 , ⋯ , N − 1 } th joint of the robot is (4).

ϕ ~ j = α sin ( ω t + ( j − 1 ) δ ) + ϕ 0

(4)

where ϕ ~ = [ ϕ ~ 1 ⋯ ϕ ~ N − 1 ] T ∈ R N − 1 . The swing amplitude of the sinusoidal motion of the robot joints is α , and the swing frequency of the joints is ω . The function of α and ω is to compensate for the movement velocity of the robot. The phase shift between the joints is δ , and the joint offset is ϕ 0 . The function of δ and ϕ 0 is to compensate for the movement direction of the robot. The motion direction and movement velocity of the multi-joint snake robot are directly affected by these parameters.^10,11

Joint and link angle controllers

To make the multi-joint snake robot move toward the specified path direction, we set up an ideal joint angle motion function ϕ ~ and construct a virtual feedback controller to realize the asymptotic stability of the joint angle error e ϕ = ϕ - ϕ ~ = 0 of the robot.

The virtual feedback controller is set as

v ϕ ~ = − F ^ ϕ e ϕ + ϕ ~ ·

(5)

where F ^ ϕ is the adaptive time-varying gain, and the updated law of F ^ ϕ is

F ^ · ϕ = e ϕ T e ϕ

(6)

The joint angle error of the robot is set to (7).

e ϕ = ϕ - ϕ ~

(7)

where e ϕ = [ ϕ 1 − ϕ ~ 1 , ⋯ , ϕ N − 1 − ϕ ~ N − 1 ] ∈ R N − 1 .

After differentiating the link angle error of the robot, equations (8) and (9) are obtained.

e · ϕ = ϕ · − ϕ ~ · = v ϕ - ϕ ~ · = ( v ϕ - v ϕ ~ ) - F ^ ϕ e ϕ

(8)

e ·· ϕ = ϕ ·· − ϕ ~ ·· ⇒ ϕ ~ ·· = ϕ ·· - e ·· ϕ

(9)

To make the multi-joint snake robot able to wind and swing, the desired link angle θ ~ of the robot to make the link angle error e θ = θ − θ ~ stable. To guide the multi-joint snake robot to the ideal path, we introduce the LOS method, which converts target path tracking into target heading tracking, if the robot’s link angle can keep up with the target’s θ ~ . So the robot can gradually converge to the target path.^10,11 Referring to the desired link angle θ ~ = − arctan p ^ t Δ of the robot, where the forward distance is Δ . Δ determines the velocity of the multi-joint snake robot to converge to the desired path. ²⁶ Moreover, Δ > 0 represents the direction forward and Δ < 0 represents the direction backwards. Since Δ directly affects the iteration velocity of the robot’s motion direction angle, the desired path of the robot can be achieved by adjusting the value of Δ .

Δ > | X | | Y | · ( 1 + V max V min )

(10)

The link angle error of the multi-joint snake robot is differentiated to get

e · θ = v θ − θ ~ ·

(11)

The angle velocity error of the robot is

e ω = v θ − v θ ~

(12)

e · ω = v · θ − v · θ ~

(13)

Let

v θ ~ − θ ~ · = − k θ e θ

(14)

where k θ > 0 is positive constant. At this time, the link angle velocity error can be written as e ω = v θ − θ ~ · + k θ e θ .

The joint offset compensation of the motion function of the multi-joint snake robot is designed as

ϕ 0 = 1 λ 2 v t [ λ 1 v θ − e θ − k θ e ω − λ 2 N − 1 v t ∑ j = 1 N α sin ( ω t + ( j − 1 ) δ ) + v · θ ~ ]

(15)

where

λ 2 N − 1 v t e T ϕ = λ 2 N − 1 v t e T ( ϕ − ϕ ~ ) + λ 2 v t ϕ 0 + λ 2 N − 1 v t ∑ j = 1 N α sin ( ω t + ( j − 1 ) δ )

(16)

To make the joints of the multi-joint snake robot maintain a stable rotation velocity, we set the ideal actuator torque vector v ϕ ~ and the estimated values of the environmental friction coefficient to make the actuator torque vector error of the robot asymptotically stable. ²⁶

The torque vector error of the actuator is differentiated to obtain

e · u = v · ϕ − v · ϕ ~ = − μ 1 m v ϕ + μ 2 m v t A e D T ϕ + 1 m D D T u - v · ϕ ~ = − c 1 e · ϕ − v ϕ m ( μ 1 − Γ 1 ) + v t m A e D T ϕ ( μ 2 - Γ 2 ) - c 2 e ϕ

(17)

To make the adaptive trajectory tracking controller of the robot adapt to the environment, we design the control input function (10)–(18).

u = m ( D D T ) − 1 ( u ¯ + Γ 1 m v ϕ − Γ 2 m v t A e D T ϕ )

(18)

where u ¯ = ϕ ~ ·· - c 1 e · ϕ - c 2 e ϕ , c 1 = [ c 1 , 1 ⋱ c 1 , N − 1 ] , c 1 , j > 0 , c 2 = [ c 2 , 1 ⋱ c 2 , N − 1 ] , c 2 , j > 0 , and j = 1 , 2 , ⋯ , ( N − 1 ) , u ¯ = [ u ¯ 1 , ⋯ , u ¯ N − 1 ] T . Γ 1 and Γ 2 are the estimated value of μ 1 and μ 2 , its adaptive tuning law is (19).

{ Γ · 1 = − c 3 m v ϕ e u T Γ · 2 = c 4 m v t A e D T ϕ

(19)

where c 3 > 0 and c 4 > 0 are positive constant gains. The estimator needs to adjust two parameters ( c 3 , c 4 ) to estimate the unknown friction coefficient. The estimated value can be adjusted to the system controller to compensate for the robot’s motion state and offset the negative impact of geographical environment on the robot.

Stability analysis

Proof

Lyapunov candidate function L 1 is

L 1 = c 2 2 e ϕ T e ϕ + 1 2 ( c 2 F ^ ϕ − f ϕ ) 2

(20)

where c 2 > 0 and f ϕ > 0 are positive gain. The differential of L 1 is

L · 1 = − c 2 f ϕ e ϕ T e ϕ + c 2 e ϕ T ( v ϕ − v ϕ ~ )

(21)

Lyapunov candidate function L 2 is

L 2 = 1 2 e θ 2 + 1 2 e ω 2

(22)

L . 2 = e θ e . θ + e ω e . ω = − k θ e θ 2 − k θ e ω 2 + λ 2 N − 1 v t e ω e T ( ϕ − ϕ ~ )

(23)

Because,

| e T ( ‖ ϕ - ϕ ~ ‖ ) | ≤ ‖ ϕ - ϕ ~ ‖ 1 ≤ N - 1 ‖ ϕ - ϕ ~ ‖ 2

(24)

( λ 2 v t N − 1 ‖ ϕ - ϕ ~ ‖ 2 - 1 2 | e ω | ) 2 = ( λ 2 v t N − 1 ‖ ϕ - ϕ ~ ‖ 2 ) 2 − λ 2 N − 1 v t | e ω | · ‖ ϕ - ϕ ~ ‖ 2 + 1 4 | e ω | 2 ≥ 0

(25)

‖ ϕ - ϕ ~ ‖ 2 2 = e ϕ T e ϕ

(26)

So,

L · 2 ≤ − k θ e θ 2 − ( k θ − 1 4 ) e ω 2 + λ 2 2 v t 2 N − 1 ‖ ϕ - ϕ ~ ‖ 2 2 = − k θ e θ 2 − ( k θ − 1 4 ) e ω 2 + λ 2 2 v t 2 N − 1 e ϕ T e ϕ

(27)

Lyapunov candidate function L 3 is

L 3 = 1 2 e u T e u + 1 2 c 3 ( μ 1 − Γ 1 ) 2 + 1 2 c 4 ( μ 2 − Γ 2 ) 2

(28)

L · 3 = − c 1 e u T e u − c 2 e u T e ϕ + ( μ 1 − Γ 1 ) ( − v ϕ e u T m − Γ · 1 c 3 ) + ( μ 2 − Γ 2 ) ( v t m A e D T ϕ - Γ · 2 c 4 )

(29)

Combined with (21), (27), and (29), the Lyapunov function L is designed as

L · ≤ − ( c 2 f ϕ − λ 2 2 v t 2 N − 1 ) e ϕ T e ϕ − k θ e θ 2 − ( k θ − 1 4 ) e ω 2 − c 1 e u T e u

(30)

As long as k θ > 1 4 and λ 2 2 v t 2 N − 1 < c 2 f ϕ are satisfied, then L · is semi-negative definite. According to Lemma 1, the angle system of the snake robot is asymptotically stable.

Position and speed control

To control the motion position and velocity of the multi-joint snake robot on the desired path, we set up a desired position p ^ ~ t and velocity v ~ t of the multi-joint snake robot. By building a feedback controller, the motion position error e p = p ^ t − p ^ ~ t and motion velocity error e v = v t − v ~ t of the robot are stable.

The motion position and motion position error of the robot are differentiated to obtain (31) and (32).

p ^ · t = v t

(31)

e · p = e v

(32)

e · v = − μ 1 m v t + 2 μ 2 Nm v n e T ϕ - μ 2 Nm ϕ T A e D ¯ v ϕ - v ~ · t

(33)

e v = − d 1 e p and q ^ = e v + d 1 e p are designed. We get e v = q ^ − d 1 e p , where d 1 > 0 is a positive constant gain.

The Lyapunov candidate function V 1 . is designed.

V 1 = 1 2 e p 2

(34)

V · 1 = e p e · p = e p e v = − d 1 e p 2 + e p q ^

(35)

And,

v t = e v + v ~ t = q ^ − d 1 e p + v ~ t

(36)

The Lyapunov candidate function V 2 is designed.

V 2 = 1 2 q ^ 2

(37)

where 2 μ 2 Nm v n e T ϕ - μ 2 Nm ϕ T A e D ¯ v ϕ - v ~ · t = e · v + μ 1 m v t and e · p = e v = − d 1 e p .

V . 2 = − μ 1 m q ^ 2 + d 1 e p q ^ ( μ 1 m − d 1 ) + q ^ ( − μ 1 m v ~ t + e . v + μ 1 m v t ) = − μ 1 m q ^ 2 + d 1 e p q ^ ( μ 1 m − d 1 ) + ( e v + d 1 e p ) ( e . v + μ 1 m e v ) = − μ 1 m q ^ 2 + d 1 e p q ^ ( μ 1 m − d 1 ) + e v e . v + μ 1 m e v 2 + d 1 e p e . v + μ 1 m d 1 e p e v

(38)

Moreover e · v = − d 1 e · p = − d 1 e v , e p e · v = − d 1 e p e v = e v 2 , and e p e v = − d 1 e p 2 .

The Lyapunov candidate function V 3 is designed.

V 3 = 1 2 ( μ 1 e v − 1 m e p ) 2

(39)

V · 3 = μ 1 2 e v e · v − μ 1 m e v 2 − μ 1 m e p e · v + 1 m 2 e p e v

(40)

Combining the Lyapunov candidate function set above, V can be obtained.

V = V 1 + V 2 + V 3

(41)

V · = − d 1 e p 2 − μ 1 m q ^ 2 + ( − d 1 2 + μ 1 m d 1 + 1 ) e p q ^ − ( μ 1 2 d 1 + μ 1 m ) e v 2 − d 1 ( μ 1 m d 1 + 1 m 2 ) e p 2

(42)

When − d 1 2 + μ 1 m d 1 + 1 = 0 , that is md 1 2 − μ 1 d 1 + m = 0 . We can get d 1 = μ 1 + μ 1 2 + 4 m 2 2 m > 0 .

V · = − μ 1 m q ^ 2 − ( μ 1 2 d 1 + μ 1 m ) e v 2 − d 1 ( μ 1 m d 1 + m 2 + 1 m 2 1 ) e p 2

(43)

So V · < 0 is negative definite. Therefore, the position and velocity system of the snake robot is asymptotically stable.

OPEN IN VIEWER

Figure 2.

Trajectory tracking controller of the robot.

Simulations

Following the shape of the multi-joint snake robot, the number of joints of the robot is set as N = 6 , the mass of each joint is m = 0 . 2 kg , the joint length is h = 0 . 2 m , and each joint is composed of a fixed cylinder and a universal joint that can be rotated. For the convenience of calculation, we take the ground viscous friction coefficients μ 1 = 0 . 2 and μ 2 = 2 . 0 as examples. The rotation coefficients are λ 1 = 0 . 5 and λ 2 = 20 . The initial tangential velocity of the robot is v t = 0 m / s , the initial normal velocity of the robot is v n = 0 m / s , the initial joint angle is ϕ = 0 ° , and the initial link angle is θ = 90 ° . The initial position is ( p x , p y ) = ( 0 , 0 ) m , the torque vector of the initial steering gear actuator is v ϕ = 0 ° / s , and the link angle velocity is v θ = 0 ° / s . The initial swing amplitude, initial swing frequency and initial phase of the multi-joint snake robot are α = 0 . 05 m , ω = 100 ° / s and δ = 60 ° , respectively. F ϕ is 1.6. To meet the requirements of k θ > 1 4 and λ 2 2 v t 2 N − 1 < c 2 f ϕ , constant gains are set to k θ = 0 . 5 and f ϕ = 18 , c 1 = 5 , and c 2 = 0 . 5 , respectively.

In addition, to demonstrate the performance of the proposed method, we compared it with traditional trajectory tracking controllers (PID) and adaptive central pattern generator (CPG) controllers ²⁶ in simulations. The link angle and the desired link angle curve of the multi-joint snake robot under the proposed controller with non-holonomic constraints are analyzed, as shown in Figure 3. There is a big deviation between the link angle θ of the robot and the desired link angle θ ~ curve during the movement of the multi-joint snake robot before 10 s. When the robot moves to 15 s, the link angle of the robot can approach and track the desired link angle. When the robot moves from 5 to 10 s, under the proposed controller, the link angle error of the robot converges faster than under PID and CPG control. Under the control of the proposed method, e θ stabilizes after a 20 s fluctuation. However, due to interference and adjustable parameters, e θ eventually converges to around 0. The above results indicate that the proposed method can make the link angle error e θ of the robot more stable.

OPEN IN VIEWER

Figure 3.

θ , θ ~ , and e θ curves of the robot.

The center of mass motion trajectory of the multi-joint snake like robot is shown in Figure 4. All three controllers can cause small-scale displacement of the robot’s center of mass motion trajectory within 10 s during the motion of a multi-joint snake robot, which is caused by small vibrations caused by frictional forces in the actual motion of the robot. In contrast, under the control of the proposed method, the trajectory of the robot’s center of mass gradually stabilizes after self-adjustment. The above results indicate that compared with other controllers, the proposed controller can more effectively reduce the offset of the robot centroid trajectory.

OPEN IN VIEWER

Figure 4.

Centroid motion trajectory.

The joint angle error curve of the multi-joint snake robot is shown in Figure 5. Under the control of the three controllers, the joint angle error of the robot gradually converges and tends to be stable during the movement of the robot after 10 s. The motion velocity of the multi-joint snake robot is directly affected by the friction in different terrain environments. The motion velocity error of the robot under the control of the different controllers gradually converges and tends to be stable during the movement of the robot after 10 s. The above results show that the proposed controller can reduce the velocity error of the robot at the initial moment.

OPEN IN VIEWER

Figure 5.

e ϕ and e v curves of the robot.

The torque control input of each joint of the multi-joint snake robot is shown in Figure 6. The control input changes obviously within 10 s, which can provide a faster starting velocity for the robot. After 10 s, the control input tends to be stable and presents a regular sinusoidal change, which is consistent with the winding motion characteristics of the snake robot.

OPEN IN VIEWER

Figure 6.

Control input u .

The estimated values of viscous friction coefficients and control parameters of the multi-joint snake robot are shown in Figure 7. Γ ^ 1 converges to 2 at 10 s and remains stable. Γ ^ 2 converges to 1.6 at 20 s, and there is a small fluctuation in the stable curve, and the influence of this fluctuation on the control input can be ignored. F ^ ϕ converges to 0.2 at 5 s and stable. The converged estimated values can replace the real value to compensate the control input u and ensure the adaptability of the robot to the changes of environmental friction.

OPEN IN VIEWER

Figure 7.

Estimates Γ ^ 1 , Γ ^ 2 , and F ^ ϕ .

The variance and root mean square (RMS) of the trajectory tracking performance under the action of different controllers are shown in Figure 8. Because the variance and RMS can reflect the degree of numerical dispersion and the offset between variables and expected values, the smaller the variance and mean square deviation of trajectory tracking performance, the better the control ability of the controller. The figure shows that compared with the other controllers, the proposed controller effectively estimates the friction coefficient, reduces the robot link angle, joint angle error, and motion speed error, and improves the system control accuracy.

OPEN IN VIEWER

Figure 8.

RMS error comparison of different controllers.

Experiments

In this part, we carry out a prototype experiment on the multi-joint snake robot with non-holonomic constraints to verify the stability of the adaptive trajectory tracking controller, as shown in Figures 9–11. The planar motion capture system is used to measure the relative position coordinates of robots. The system is fixed above the experimental area and obtains the position information of the robot relative to the preset coordinate system on the ground through video images. The coordinate system is set on the plane of the experimental site and within the field of view of the camera system to ensure accurate capture of the robot’s motion trajectory. For the convenience of design, the multi-joint snake robot adopts N = 6 links, the length of which is θ = ± 60 ° and the radius of which is r = 65 mm . Six groups of auxiliary wheels are evenly distributed in each link, and the maximum rotation angle between joints is θ = ± 60 ° .

OPEN IN VIEWER

Figure 9.

Joints and links of the multi-joint snake robot.

OPEN IN VIEWER

Figure 10.

Motion capture system.

OPEN IN VIEWER

Figure 11.

Prototype experiment of the multi-joint snake robot.

The robot is manufactured using 3D printing technology and moves forward by utilizing the friction between itself and the ground. A modular structure is adopted between the joints, with the main control module responsible for coordinating the robot’s movements. Each joint is equipped with angle sensors that record and provide feedback on joint rotation while calculating the robot’s motion direction angle in real time. The Bluetooth wireless transceiver module receives instructions from the upper computer and provides feedback on the robot’s status, while the joint modules execute the commands issued by the upper computer. Additionally, each link of the robot is equipped with an auxiliary wheel below it, which is designed to generate asymmetric friction in the normal and tangential directions, allowing the snake like robot to effectively move forward in a two-dimensional plane. They only provide auxiliary motion and do not directly provide power. Each joint has two actuators, with each motor controlling rotation in one direction. The control system of the multi-joint snake robot uses a bus to facilitate communication between the modules. To ensure fair comparisons, the same control parameters are used for components with identical structures, while original parameters are set according to relevant literature for different parts. Furthermore, to minimize the impact of randomness on the results, each algorithm is subjected to multiple experiments in the same hardware and software environment, including the operating system, computer processor, memory, hard disk capacity, and other factors.

To analyze the performance of the multi-joint snake robot in motion, we extract the performance indexes of the first joint and the sixth joint on the straight path of the robot. The robot will adjust the relationship between its actual position and the ideal position during the movement of the robot before 100 cm. When the robot moves to 100 cm, the actual position of the robot tracks the desired position almost perfectly, as shown in Figure 12. Meanwhile, the robot will adjust the relationship between the actual link angle and the desired link angle during the movement of the robot before 150 cm. When the robot moves to 150 cm, the actual link angle of the robot tracks the desired link angle almost perfectly. The multi-joint snake robot will have a small amplitude of oscillation in the initial period of movement, which is the self-adjustment process of the robot. Meanwhile, the motion position and link angle of the robot lag slightly compared with the ideal value, because the movement process of the robot will be affected by environmental factors. When the position and link angle of the robot are adjusted to reasonable values, the robot tracks the desired motion position and desired link angle under the control of the adaptive trajectory tracking controller.

OPEN IN VIEWER

Figure 12.

The position and link angle of the robot.