4. Motion Control Course

4.1 Coordinates System Establishing

4.1.1 Coordinate System Introduction

Regarding the design of the degrees of freedom for each leg of a quadruped robot, three actuators are distributed at the upper joints, while for the lower joint, driving force is transmitted through linkage or ball screw, which is beneficial for minimizing the inertia of lower legs and enhancing the dynamics motion performance.

When controlling the motion of ROSPug robot, the inverse kinematics can be employed by inputting the coordinates of the footholds for four legs. This allows the calculation of the rotational angles for all servos, thereby achieving the goal of controlling the walking motion of the robot dog.

Therefore, the coordinates system of ROSPug should be established in the beginning. When establishing the coordinates, take the center of four upper joints as the origin (0,0,0) and set the coordinates for the footholds of the four legs. During setting the coordinates, only the values for x-axis, y-axis and z-axis of the foothold coordinates need to be specified, as shown in the figure below.

4.1.2 Coordinate Instructions

For clarity, we’ll use the robot dog’s standing posture as an example. The coordinates for this standing position are shown in the box below:

In the image, Leg1, Leg2, Leg3, and Leg4 represent the positions of the robot dog’s four legs. From the robot dog’s perspective, the corresponding positions are shown in the table below:

Leg1	Leg2	Leg3	Leg4
front right	front left	behind right	behind left

The coordinates are in meters. For the ROSPug robot dog in its standing posture, the leg coordinates are (0.010, 0, -0.130). The Z-axis value of -0.130 means the foot contact point is 0.13 meters below the origin.

The X-axis value of 0.010 indicates the foot contact point is 0.010 meters from the origin along the X-axis.

The Y-axis coordinate is 0 by default. For lateral movement, such as moving right by 0.02 meters, the Y-axis coordinate should be -0.02 for the left leg and 0.02 for the right leg, and vice versa.

4.2 Inverse Kinematics Analysis

4.2.1 Inverse Kinematics Introduction

Inverse kinematics matters in ROSPug’s motion planning and control, because whether the inverse kinematics solution is fast and accurate directly affects the precision of ROSPug’s path planning and control. Therefore, fast and accurate inverse kinematics solution is essential.

For ROSPug, inverse kinematics involves solving the rotational angle of the upper joint and lower joint according to the foot coordinate. The joint distribution is as shown in the picture.

The rotational angle of the upper joint is controlled by 8 servos on ROSPug, as shown in the below picture.

The rotational angle of the lower joint is controlled by ID3, ID6, ID9 and ID12 servos on upper joints. Linkage mechanism is adopted in the control.

Per the inverse kinematics analysis, the solution procedure is divided into two steps:

(1) Calculate the position to which the upper and lower joints move based on the foot coordinates so as to solve the rotation angle of the corresponding servo.

(2) According to the rotation angle of servo, calculate the corresponding value, and then directly control the servo to rotate.

4.2.2 Project Analysis

Storage Path of Source Code

For better understanding, we will combine program control and inverse kinematics to analyze.

The source code of this program is stored in pug/src/pug_tutorial/scripts/intermediate_control_course/demo01_kinematics.py.

Operation Steps

Note

The entered command should be case sensitive, and Tab key can be used to complement key words.

(1) Turn on ROSPug, and access the robot system desktop using NoMachine.

(2) Click to open the command line terminal.

(3) Enter command and press Enter to initiate the game.

python3 pug/src/pug_tutorial/scripts/intermediate_control_course/demo01_kinematics.py

(4) If want to close this game, press Ctrl+C. If it fails to close, please try again.

Program Outcome

Once the feature is activated, all servos on the ROSPug robot dog will adjust to their specified angles according to the set coordinates. The terminal will then show the positions of each servo and the coordinates of the robot dog’s foot contact points.

Program Analysis

The source code of this program is stored in: /home/pug/src/pug_tutorial/scripts/intermediate_control_course/demo01_kinematics.py

                            # FR     FL     BR     BL
foot_locations = np.array([[-0.01, -0.01, -0.01, -0.01],  # X
                           [ 0.0,   0.0,   0.0,   0.0],   # Y
                           [-0.1,  -0.1,  -0.1,  -0.1]    # Z
                          ])

Set coordinate

With ROSPug as the first-person perspective, FR, FL, BR and BL respectively represent its 4 legs.

FR	FL	BR	BL
Front right	Front left	Behind right	Behind left

Note

When setting coordinates, the range for the X-axis is ‘-0.15 to +0.15,’ and the range for the Z-axis is ‘-0.15 to -0.05.’

When the coordinate of X axis is set as 0, the line connecting foothold and the origin is perpendicular to the ground. If the coordinate of X axis is positive, the foothold is at the front.

The larger the coordinate of Z axis, the higher its feet lift, which means that the perpendicular distance between foothold and the origin is shorter.

Acquire servo angle

Calculate the servo angle through inverse kinematics.

res = set_leg_ik(foot_locations.reshape(12).tolist())

Control servo to rotate

Calculate the pulse width of the servo according to the angle value so as to directly control servo rotation.

res = set_leg_ik(foot_locations.reshape(12).tolist())
if res.success:
    joint_angle = res.joint_angle
    print('ik input:\n', foot_locations)
    print('ik output:\n', joint_angle)
    set_angle_pub.publish(joint_angle, 0.5)

4.2.3 Inverse Kinematics Principles of a Quadruped Robot Dog (Using a Single Leg as an Example)

Before controlling the movement of a quadruped robot dog, it is essential to first understand the movement relationship of a single leg. In simple terms, the servos act as the driving mechanism, consisting of a hip joint servo, a thigh servo, and a calf servo.

During the calculation process, vector operations are performed using the rotation matrix R to find the 3D coordinates (x, y, z) of the end-effector relative to the hip joint center. The relationship between these coordinates and the joint angles is then used to calculate the rotation angles for the three servos. The process flowchart is shown below:

Conversion between Euler Angles and Rotation Matrix R

Euler Angles Definition: In the classical Cartesian coordinate system, which consists of the x, y, and z axes, the angles of rotation around each of these axes are collectively known as Euler angles.

Parameter	Description	Meaning
Roll	Angle of Rotation About the X-axis	Roll angle
Pitch	Angle of Rotation About the Y-axis	Pitch angle
Yaw	Angle of Rotation About the Z-axis	Yaw angle

The base coordinate system for the two left legs of the ROSpug robot dog uses a right-handed coordinate system, while the base coordinate system for the two right legs uses a left-handed coordinate system. The rotation transformation matrices for left-handed and right-handed coordinate systems are different.

For illustration, using the left-handed coordinate system as an example, the basic rotation matrix in this system can be calculated from the given Euler angles (p, h, b). For the right-handed coordinate system, the Euler angles (-p, -h, -b) should be used.

In the left-handed system, the rotation matrix Rx for a rotation of p°around the x-axis is:

\[\begin{split}R_{x}\ = \ \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos p & \sin p \\ 0 & - \sin p & \cos p \end{bmatrix}\end{split}\]

In the left-handed coordinate system, the rotation matrix Ry for a rotation of h°around the y-axis is:

\[\begin{split}R_{y}\ = \ \begin{bmatrix} \cos h & 0 & - \sin h \\ 0 & 1 & 0 \\ \sin h & 0 & \cos h \end{bmatrix}\end{split}\]

In the left-handed coordinate system, the rotation matrix Rz for a rotation of b°around the z-axis is:

\[\begin{split}R_{z}\ = \ \begin{bmatrix} \cos b & \sin b & 0 \\ - \sin b & \cos b & 0 \\ 0 & 0 & 1[conf.py](..%2Fconf.py) \end{bmatrix}\end{split}\]

The combined rotation matrix is defined as the matrix obtained by combining multiple rotations. For Euler angles in the Z-X-Y sequence, the corresponding combined matrix is R = Rz * Rx * Ry. The order of XYZ determines the rotation matrix definition; therefore, when the Euler angle sequence changes, the rotation matrix formula must be adjusted accordingly.

The rotation matrix corresponding to Euler angles (p, h, b) in the left-handed coordinate system (Z-X-Y sequence) is:

\[\begin{split}R = Rz \ast Rx \ast Ry = \begin{bmatrix} \cos b\cos h + \sin b\sin p\sin h & \sin b\cos p & - \cos b\sin h + \sin b\sin p\cos h \\ - \sin b\cos h + \cos b\sin p\sin h & \cos b\cos p & \sin b\sin h + \cos b\sin p\cos h \\ \cos p\sin h & - \sin p & \cos p\cos h \end{bmatrix}\end{split}\]

Calculation of the End-Effector Position Vector for a Single Leg

Identify the vectors necessary for calculating the end-effector position of the left front leg\(`\overset{\rightarrow}{A_{1}B_{1}}`\):

\[\overset{\rightarrow}{OB_{1}} + \overset{\rightarrow}{B_{1}A_{1}} = \overset{\rightarrow}{OO^{'}} + \overset{\rightarrow}{O'A_{1}}\]

\[\overset{\rightarrow}{O'A_{1}} = R \ast {\overset{\rightarrow}{O'A_{1}}}_{身体}\]

\[\overset{\rightarrow}{A_{1}B_{1}} = - \overset{\rightarrow}{OO^{'}} - R \ast {\overset{\rightarrow}{O'A_{1}}}_{身体} + \overset{\rightarrow}{O'B_{1}}\]

\[\overset{\rightarrow}{A_{n}B_{n}} = - \overset{\rightarrow}{OO^{'}} - R \ast {\overset{\rightarrow}{O'A_{n}}}_{身体} + \overset{\rightarrow}{O'B_{n}}(n\ = \ 1,2,3,4)\]

In the above diagram, since we are studying the standing posture control of the quadruped robot dog and O’ is given, the vectors OB1 and OO’ are known. In the equations from the diagram, vector O’A1 will change according to the rotation matrix R. Therefore, it needs to be multiplied by the rotation matrix before substitution into the calculations. Finally, we obtain the foot contact vector A1B1 relative to the center of the hip joint.

Calculation of the Rotation Angles for Single Leg Joint Servos

Once the coordinates of the foot contact point relative to the hip joint servo center are known, the rotation angles for each servo can be calculated using the inverse kinematics for the single leg.

Calculate the Rotation Angle γ for the Shoulder

In the yoz plane, given y, z, h (shoulder length), determine the intermediate variables: d, l, γ1, γ2.

Finally, calculate the rotation angle γ.

Solution process:

d=\(`\sqrt{y^{2} + z^{2}}`\)

l=\(`\sqrt{d^{2} - h^{2}}`\)

γ1=\(`{- arctan}\frac{h}{l}`\)

γ2=\(`{- arctan}\frac{y}{z}`\)

γ=γ2−γ1

Calculate the Rotation Angle β for the Calf

In the xoz plane, given l, x, hl (thigh length), and hu (calf length), determine the intermediate variable n. Then, calculate the final rotation angle β. The calculation process is as follows:

s=\(`\sqrt{l^{2} + x^{2}}`\)

(hu+n)2+m2=s2 (1)

n2+m2=hl2 （2）

Subtract Equation (2) from Equation (1) to obtain:

[(hu+n)2+m2]-(n2+m2)=s2-hl2

n=\(`\frac{s^{2} - {hl}^{2} - {hu}^{2}}{2hu}`\)

β=\(`{- arccos}\frac{n}{hl}`\)

Calculate the rotation angle α for the thigh

In the xoz plane, given l, x, hu (calf length), n, and s, determine the intermediate variables α1 and α2. Then, calculate the final rotation angle α. The calculation process is as follows:

α1=\(`{- arctan}\frac{x}{l}`\)

α2=\(`{- arccos}\frac{hu + n}{s}`\)

α=α1+α2

Using the calculations provided, once you have the coordinates of the foot contact point relative to the hip joint servo center, you can determine the rotation angles for each servo using the inverse kinematics for a single leg. However, for the quadruped robot dog, the coordinate systems differ between the legs: the right legs use a left-handed coordinate system, while the left legs use a right-handed coordinate system.

4.3 Gait Parameter Introduction

4.3.1 Gait Definition

Robot gait refers to the patterns and sequences of movement of a robot’s legs or other locomotion components. It describes how the robot moves, with common gait types including Trot, Walk, Amble, and Pace.

The table below lists some commonly used terms in gait descriptions:

Term	Description
Phase Difference	The difference in movement timing between different legs, either leading or lagging.
Swing Phase	The phase where the leg is lifted and is in the air.
Support Phase	The phase where the leg is in contact with the ground.
Cycle	The complete process from one heel strike to the next heel strike on the same side during locomotion.
Gait Frequency	The number of gait cycles completed per unit of time.
Stride Length	The distance traveled by the foot from lift-off to landing within one cycle.
Step Length	The distance traveled by the body within one cycle.
Duty Cycle	The ratio of time a single leg spends in the support phase to the total gait cycle.

4.3.2 Types of Gait

Classified by balance mode, ROSPug robot dog’s gaits can be divided into three types: static gait, dynamic gait, and quasi-static gait. Specifically, the static gait employed by ROSPug is the Walk gait, the dynamic gait is the Trot gait, and the quasi-static gait is the Amble gait.

The ROASug robot dog uses the following gaits:

Static Gait: Walk

Dynamic Gait: Trot

Quasi-static Gait: Amble

4.3.3 Leg Position

ROSPug’s four legs are numbered as below:

4.3.4 Introduction to Trot Gait

The Trot gait is a dynamic gait where diagonally opposite legs move in pairs. Specifically, Legs 1 and 2 move together, and Legs 3 and 4 move together. Ideally, each pair of diagonal legs lifts and touches the ground simultaneously. In the Trot gait, the sequence for the robot dog’s legs is 1 2 → 3 4 → 1 2. The control mechanism is illustrated in the diagram below:

(1) indicates the time when all four legs are on the ground.

(2) represents the time when a single leg is off the ground.

4.3.5 Introduction to Walk Gait

Walk gait is a static walking pattern where, throughout the movement, three legs are always in the support phase, and at most, one leg is in the swing phase. In the Walk gait for the robot dog, the sequential rotation of the four legs follows the order 1 → 2 → 3 → 4 → 1, as depicted in the diagram below:

In the diagram, the raised section represents leg lifting, ① indicates the time interval between front and rear legs, ② represents the duration when all four legs are in contact with the ground, and ③ represents the time when a single leg is lifted off the ground.

4.3.6 Introduction to Amble Gait

Amble gait is a quasi-static walking pattern, considered as an accelerated version of the Walk gait. Its characteristic feature is that during movement, at least two legs are in the support phase, with a maximum of two legs in the swing phase. In the Amble gait for the robot dog, the sequential rotation of the four legs follows the order 1 → 2 → 3 → 4 → 1, as illustrated in the diagram below:

In the diagram, the raised section represents leg lifting, ① indicates the time interval between front and rear legs, ② represents the duration when all four legs are in contact with the ground, and ③ represents the time when a single leg is lifted off the ground.

4.3.7 Parameters Within Program

The parameters for the robot dog’s movement consist of two main parts: motion parameters and gait parameters. Gait parameters include the total time in contact with the ground, the total time lifted off the ground, and the time interval between front and rear legs. Motion parameters encompass forward speed and turning speed.

When configuring, it is necessary to invoke the puppy_demo.py file, with the file path as follows: /home/pug/src/pug_driver/pug_control/scripts/pug_node.py.

Here is a detailed explanation of the parameters:

Parameter	Explanation
overlap_time	The duration of all four knee joint endpoints in contact with the ground, measured in seconds.
swing_time	The duration of both knee joint endpoints lifted off the ground, measured in seconds.
clearance_time	Time interval between front and rear legs, measured in seconds.
z_clearance	When in motion, the distance in centimeters that the knee joint lifts the endpoint above the ground.

4.4 Trot Gait Analysis and Implementation

4.4.1 Trot Gait Description

The Trot gait is a medium to low-speed dynamic gait characterized by the legs moving in a diagonal and cross pattern, simultaneously lifting and landing. This gait has a broad range of motion speeds, offering a balance between stability and speed, making it the most commonly used quadrupedal gait.

When the ROSPug is in Trot gait, the feet on the diagonal line touch the ground simultaneously. Using the diagram below as an example, let’s analyze and explain the Trot gait:

As shown in figure (a), legs 1 and 2 of the ROSPug lift and swing forward, while legs 3 and 4 support the body, ensuring the center of gravity is at the intersection of the diagonals. At this point, legs 1 and 2 are in the swing phase, while legs 3 and 4 are in the support phase.

As shown in figure (b), all four legs touch the ground simultaneously, with legs 1, 2, 3, and 4 all in the support phase.

As shown in figure (c), legs 3 and 4 lift and swing forward, while legs 1 and 2 support the body, ensuring the center of gravity is at the intersection of the diagonals. At this point, legs 3 and 4 are in the swing phase, while legs 1 and 2 are in the support phase.

As shown in figure (d), all four legs touch the ground simultaneously, with legs 1, 2, 3, and 4 all in the support phase.

When these four sets of actions (a), (b), (c), and (d) are completed, the ROSPug has completed one full cycle of movement.

4.4.2 Operation Steps

Note

The input command should be case sensitive, and users can Tab to complement keywords.

(1) Start the robot, and access the robot system desktop using NoMachine.

(2) Double-click to open the command-line terminal.

(3) Execute the command to start the game.

python3 pug/src/pug_tutorial/scripts/base_control_course/demo01_trot_gait.py

(4) If you need to terminate the game, use short-cut Ctrl+C.

4.4.3 Program Outcome

After activating the game, the ROSPug will move in a trot gait, with diagonally paired legs in coordinated motion, lifting or landing simultaneously. The phase relationship is illustrated in the diagram below:

4.4.4 Parameters Analysis

The source code of this program is located in: /home/pug/src/pug_tutorial/scripts/base_control_course/demo01_trot_gait.py

#!/usr/bin/env python3
# coding=utf8
import time
import rospy
from pug_control.msg import Velocity, Pose, Gait

rospy.init_node('trot_gait', log_level=rospy.INFO)

pose_pub = rospy.Publisher('/pug_control/pose', Pose, queue_size=1)
gait_pub = rospy.Publisher('/pug_control/gait', Gait, queue_size=1)
velocity_pub = rospy.Publisher('/pug_control/velocity_move', Velocity, queue_size=1)

time.sleep(0.1)
#roll pitch yaw height x_shift stance_x stance_y run_time
pose_pub.publish(0, 0, 0, -0.13, 0.01, 0, 0, 0.5)
time.sleep(0.5)

gait_pub.publish(0.2, 0.18, 0.02, 0.04)

velocity_pub.publish(0.1, 0, 0, False)
time.sleep(2)

velocity_pub.publish(0, 0, 0, True)

Publish Trot gait parameters using the gait_pub.publish() function.

gait_pub.publish(0.2, 0.18, 0.02, 0.04)

The parameters in parentheses are defined as follows:

(1) overlap_time: The time when all four legs are on the ground, measured in seconds.

(2) swing_time: The time when a single leg is off the ground, measured in seconds.

(3) clearance_time: The interval between the front and rear crossing leg phases, measured in seconds.

(4) z_clearance: The distance the robot dog’s toe should be raised during walking, measured in centimeters.

For the Trot gait, please note that when setting up, clearance_time must be set to 0. Furthermore, adhere to the following conditions for the Trot gait:

clearance_time = 0

overlap_time > 0

swing_time > 0

If a more detailed observation is desired, you can extend swing_time and overlap_time, but exercise caution. Increasing these times will slow down the frequency and may affect the stability of walking.

4.5 Amble Gait Analysis and Implementation

4.5.1 Amble Gait

The Amble gait is a static gait and can be considered as an accelerated version of the Walk gait. During motion, at least two legs are in the support phase, with a maximum of two legs in the swing phase.

Since the implementation process of the Amble gait is similar to that of the Walk gait, the sequence of swinging legs remains: Right Front -> Left Hind -> Left Front -> Right Hind, corresponding to the numbered sequence in the diagram: 1 -> 2 -> 3 -> 4.

The key distinction from Walk is that in the Walk gait, the second leg is lifted after the first leg is placed down. In contrast, the Amble gait begins lifting the second leg while the first leg is still in the air but not yet placed down. For example, after lifting the right front leg, the left rear leg begins to lift before the right front leg is placed down.

4.5.2 Operation Steps

Note

The input command should be case sensitive, and users can Tab to complement keywords.

(1) Start the robot, and access the robot system desktop using NoMachine.

(2) Double-click to open the command-line terminal.

(3) Execute the command below to start the game.

python3 pug/src/pug_tutorial/scripts/base_control_course/demo02_amble_gait.py

(4) If you need to terminate the game, use short-cut Ctrl+C.

4.5.3 Program Outcome

The ROSPug moves in an Amble gait, where at least two legs are in the support phase, and a maximum of two legs are in the swing phase. The Amble gait can be likened to an accelerated version of the Walk gait. The phase relationship is illustrated in the diagram below:

4.5.4 Parameters Analysis

The source code of this program is located in: /home/pug/src/pug_tutorial/scripts/base_control_course/demo02_amble_gait.py

#!/usr/bin/env python3
# coding=utf8
import time
import rospy
from pug_control.msg import Velocity, Pose, Gait

rospy.init_node('amble_gait', log_level=rospy.INFO)

pose_pub = rospy.Publisher('/pug_control/pose', Pose, queue_size=1)
gait_pub = rospy.Publisher('/pug_control/gait', Gait, queue_size=1)
velocity_pub = rospy.Publisher('/pug_control/velocity_move', Velocity, queue_size=1)

time.sleep(0.1)

pose_pub.publish(0, 0, 0, -0.13, 0.01, 0, 0, 0.5)
time.sleep(0.5)

gait_pub.publish(0.2, 0.15, 0.05, 0.04)

velocity_pub.publish(0.1, 0, 0, False)
time.sleep(2)
    
velocity_pub.publish(0, 0, 0, True)

Publish Amble gait parameters using the gait_pub.publish() function.

gait_pub.publish(0.2, 0.15, 0.05, 0.04)

The parameters in parentheses have the following meanings:

(1) overlap_time: The time when all four legs are on the ground, measured in seconds.

(2) swing_time: The time when a single leg is off the ground, measured in seconds.

(3) clearance_time: The interval between the front and rear crossing leg phases, measured in seconds.

(4) z_clearance: The distance the robot dog’s toe should be raised during walking, measured in centimeters.

For the Amble gait, it is important to note that when configuring, adhere to the conditions of 0 < clearance_time < swing_time and overlap_time > 0. If a more detailed observation is desired, you can extend the clearance_time, swing_time, and overlap_time, but be cautious. Increasing these times will slow down the frequency, potentially impacting the stability of walking.

4.6 Walk Gait Analysis and Implementation

4.6.1 Walk Gait

The Walk gait is a static gait, meaning that at any given moment, at least three legs are on the ground, and at most, one leg is in the swing phase.

The sequence of swinging legs of ROSPug is as follows: Right Front -> Left Rear -> Left Front -> Right Rear, corresponding to the numbered sequence in the diagram: 1 -> 2 -> 3 -> 4. The analysis and explanation of the Walk gait will be illustrated using the diagram below as an example:

In Figure (a), as depicted, leg number 1 of the ROSPug robotic dog is raised and swung forward, while legs 2, 3, and 4 are used to support the body. At this moment, leg 1 is in the swing phase, and legs 2, 3, and 4 are in the support phase.

As shown in Figure (b), all four legs touch the ground simultaneously, with legs 1, 2, 3, and 4 all in the support phase.

Moving to Figure (c), leg number 2 of the ROSPug robotic dog is raised and swung forward, while legs 1, 3, and 4 support the body. Here, leg 2 is in the swing phase, and legs 1, 3, and 4 are in the support phase.

In Figure (d), all four legs touch the ground simultaneously again, with legs 1, 2, 3, and 4 in the support phase.

Proceeding to Figure (e), leg number 3 is raised and swung forward, with legs 1, 2, and 4 providing support. Leg 3 is in the swing phase, while legs 1, 2, and 4 are in the support phase.

As illustrated in Figure (f), all four legs touch the ground simultaneously, with legs 1, 2, 3, and 4 in the support phase.

In Figure (g), leg number 4 is raised and swung forward, while legs 1, 2, and 3 support the body. Leg 4 is in the swing phase, and legs 1, 2, and 3 are in the support phase.

Finally, as shown in Figure (h), all four legs touch the ground simultaneously once again, with legs 1, 2, 3, and 4 in the support phase.

Upon completing these eight sets of actions (a, b, c, d, e, f, g, and h), the ROSPug robotic dog has completed one full cycle of movement.

4.6.2 Operation Steps

Note

The input command should be case sensitive, and users can Tab to complement keywords.

(1) Start the robot, and access the robot system desktop using NoMachine.

(2) Double-click to open the command-line terminal.

(3) Execute the command to start the game.

python3 pug/src/pug_tutorial/scripts/base_control_course/demo03_walk_gait.py

(4) If you need to terminate the game, use short-cut Ctrl+C.

4.6.3 Program Outcome

After initiating the game, the ROSPug will move forward in a Walk gait, ensuring that at any given moment, three legs are always on the ground, with at most one leg lifted. The phase relationship is depicted in the diagram below:

4.6.4 Parameters Analysis

The source code of this program is located in: /home/pug/src/pug_tutorial/scripts/base_control_course/demo03_walk_gait.py

#!/usr/bin/env python3
# coding=utf8
import time
import rospy
from pug_control.msg import Velocity, Pose, Gait

rospy.init_node('walk_gait', log_level=rospy.INFO)

pose_pub = rospy.Publisher('/pug_control/pose', Pose, queue_size=1)
gait_pub = rospy.Publisher('/pug_control/gait', Gait, queue_size=1)
velocity_pub = rospy.Publisher('/pug_control/velocity_move', Velocity, queue_size=1)

time.sleep(0.1)

pose_pub.publish(0, 0, 0, -0.13, 0.01, 0, 0, 0.5)
time.sleep(0.5)

gait_pub.publish(0.24, 0.12, 0.05, 0.04)

velocity_pub.publish(0.1, 0, 0, False)
time.sleep(2)

velocity_pub.publish(0, 0, 0, True)

Publish Walk gait parameters using the gait_pub.publish() function.

gait_pub.publish(0.24, 0.12, 0.05, 0.04)

The parameters in parentheses have the following meanings:

(1) overlap_time: The time when all four legs are on the ground, measured in seconds.

(2) swing_time: The time when a single leg is off the ground, measured in seconds.

(3) clearance_time: The interval between the front and rear crossing leg phases, measured in seconds.

(4) z_clearance: The distance the robot dog’s toe should be raised during walking, measured in centimeters.

In the Walk gait, it is essential to note that when configuring, adherence to the conditions clearance_time > swing_time and overlap_time > 0 is necessary. If a more detailed observation is desired, you can extend the clearance_time, swing_time, and overlap_time. However, be cautious, as increasing these times will slow down the frequency, potentially affecting the stability of walking.

4.7 Stepping in Place under Trot Gait

To get detailed introduction to Trot gait, please refer to the document saved in 4.4 Trot Gait Analysis and Implementation.

4.7.1 Operation Steps

Note

The input command should be case sensitive, and users can Tab to complement keywords.

(1) Start the robot, and access the robot system desktop using NoMachine.

(2) Double-click to open the command-line terminal.

(3) Execute the command below to start the game.

python3 pug/src/pug_tutorial/scripts/base_control_course/demo04_stay.py

(4) If you need to terminate the game, use short-cut Ctrl+C.

4.7.2 Program Outcome

After activating the game, the ROSPug will perform Trot Gait stepping in place, with diagonally paired legs in coordinated motion, simultaneously lifting or landing in the same spot. The phase relationship is illustrated in the diagram below:

4.7.3 Parameters Analysis

The source code of this program is located in: /home/pug/src/pug_tutorial/scripts/base_control_course/demo04_stay.py

#!/usr/bin/env python3
# coding=utf8
import time
import rospy
from pug_control.msg import Velocity, Pose, Gait

rospy.init_node('stay', log_level=rospy.INFO)

pose_pub = rospy.Publisher('/pug_control/pose', Pose, queue_size=1)
gait_pub = rospy.Publisher('/pug_control/gait', Gait, queue_size=1)
velocity_pub = rospy.Publisher('/pug_control/velocity_move', Velocity, queue_size=1)

time.sleep(0.1)

pose_pub.publish(0, 0, 0, -0.13, 0.01, 0, 0, 0.5)
time.sleep(0.5)

gait_pub.publish(0.2, 0.18, 0.02, 0.04)

velocity_pub.publish(0, 0, 0, False)
time.sleep(3)
velocity_pub.publish(0, 0, 0, True)

Publish movement parameters using the velocity_pub.publish() function.

velocity_pub.publish(0, 0, 0, False)

(1) The first parameter 0 is the linear velocity in the x direction, in meters per second (m/s).

(2) The second parameter 0 is the linear velocity in the y direction, in meters per second (m/s).

(3) The third parameter 0 is the angular velocity, in meters per second (m/s).

(4) The fourth parameter False is the operation state; set to True to stop movement.

Additionally, if the ROSPug robot dog shows positional drift during stationary stepping, you can correct this by adjusting its center of gravity. For detailed information on center of gravity adjustment, refer to 4.14 Center of Gravity Adjustment.

4.8 Turning under Trot Gait

For a detailed explanation of the Trot gait, please refer to the document located in the 4.4 Trot Gait Analysis and Implementation.

This program will build upon the Trot gait, allowing the ROSPug to move forward and turn by modifying the corresponding parameters.

4.8.1 Operation Steps

Note

The input command should be case sensitive, and users can Tab to complement keywords.

(1) Start the robot, and access the robot system desktop using NoMachine.

(2) Double-click to open the command-line terminal.

(3) Execute the command to start the game.

python3 pug/src/pug_tutorial/scripts/base_control_course/demo05_trot_turn.py

(4) If you need to terminate the game, use short-cut Ctrl+C.

4.8.2 Program Outcome

In the Trot gait, ROSPug will continuously turn in the specified direction.

4.8.3 Parameters Analysis

The source code of the program is saved in /home/pug/src/pug_tutorial/scripts/base_control_course/demo05_trot_turn.py

#!/usr/bin/env python3
# coding=utf8
import time
import math
import rospy
from pug_control.msg import Velocity, Pose, Gait

rospy.init_node('trot_turn', log_level=rospy.INFO)

pose_pub = rospy.Publisher('/pug_control/pose', Pose, queue_size=1)
gait_pub = rospy.Publisher('/pug_control/gait', Gait, queue_size=1)
velocity_pub = rospy.Publisher('/pug_control/velocity_move', Velocity, queue_size=1)

time.sleep(0.1)

pose_pub.publish(0, 0, 0, -0.13, 0.01, 0, 0, 0.5)
time.sleep(0.5)

gait_pub.publish(0.2, 0.18, 0.02, 0.04)

velocity_pub.publish(0.1, 0, math.radians(15), False)
time.sleep(4)

velocity_pub.publish(0, 0, 0, True)

Use the velocity_pub.publish() function to publish movement parameters:

velocity_pub.publish(0.1, 0, math.radians(15), False)

(1) The first parameter, 0.1, sets the linear velocity in the x direction, measured in meters per second (m/s).

(2) The second parameter, 0, sets the linear velocity in the y direction, measured in meters per second (m/s).

(3) The third parameter, math.radians(15), specifies the angular velocity, measured in radians per second (rad/s).

(4) The fourth parameter, False, indicates the operation state; set this to True to stop movement.

Adjust these parameters to change the robot’s speed.

Revise the Gait

Publish the Trot gait parameters using the gait_pub.publish() function.

pose_pub.publish(0, 0, 0, -0.13, 0.01, 0, 0, 0.5)

The parameters in parentheses are defined as follows:

(1) The first parameter, overlap_time, represents the duration during which all four legs of the ROSPug are on the ground, measured in seconds.

(2) The second parameter, swing_time, indicates the time when a single leg of the robotic dog is off the ground, measured in seconds.

(3) The third parameter, clearance_time, denotes the interval between the front and rear crossing leg phases of the robotic dog, measured in seconds.

(4) The fourth parameter, z_clearance, signifies the distance the robotic dog’s toe should be raised during walking, measured in centimeters.

4.9 Explanation of Pose (Static) Parameters

4.9.1 Pose Description

Pose describes the orientation and rotational state of a robot in space. For the ROSPug robot dog, pose parameters define its initial stance, including standing height, pitch angle, roll angle, and other related measurements.

4.9.2 Program-Related Parameter Explanation

You can adjust the robot dog’s initial pose by modifying the coordinates of the foot contact points for its four legs. To configure the pose, the pose parameters are sent to the pose_callback() function in the puppy_node.py script.

The file path is: /home/pug/src/pug_driver/pug_control/scripts/pug_node.py.

The definition for each parameter is as below:

Parameter	Description
stance_x	The additional distance the four legs extend along the x-axis, measured in meters. Positive values move the front legs forward and the rear legs backward.
stance_y	The additional distance the four legs extend along the y-axis, measured in meters. Positive values spread the legs outward to the sides.
height	The z-axis coordinate for each leg, measured in meters.
x_shift	The x-axis offset for all four legs, measured in meters.
pitch	The pitch angle of the body, in radians. Use the `math.radians()` function to convert degrees to radians. From the "robot dog's perspective", positive values raise its head (looking up), while negative values lower its head (looking down). The range is "-31° to +31°".
roll	The roll angle of the body, in radians. Use the `math.radians()` function to convert degrees to radians. From the "robot dog's perspective", positive values tilt the body to the right, and negative values tilt it to the left. The range is "-31° to +31°".
Run_time	The time it takes for the robot dog to achieve the desired pose, measured in seconds.

4.10 Robot Horizontal Movement

When controlling the ROSPug robot dog during walking, adjusting the movement parameters allows you to control the robot’s horizontal movement.

4.10.1 Operation Steps

Note

The entered command should be case sensitive, and Tab key can be used to complement key words.

(1) Start the ROSPug, and connect to NoMachine remote system desktop.

(2) Click to open the command line terminal.

(3) Enter command below to set the direction and speed of ROSPug’s movement.

python3 pug/src/pug_tutorial/scripts/intermediate_control_course/demo02_horizontal_move.py

(4) If want to close this game, press Ctrl+C. If it fails to close, please try again.

4.10.2 Program Outcome

After the game starts, ROSPug will move horizontally based on the set speed.

4.10.3 Program Analysis

The source code is saved in: /home/pug/src/pug_tutorial/scripts/intermediate_control_course/demo02_horizontal_move.py

#!/usr/bin/env python3
# coding=utf8
import time
import rospy
from pug_control.msg import Velocity, Pose, Gait

rospy.init_node('horizontal_move', log_level=rospy.INFO)

pose_pub = rospy.Publisher('/pug_control/pose', Pose, queue_size=1)
gait_pub = rospy.Publisher('/pug_control/gait', Gait, queue_size=1)
velocity_pub = rospy.Publisher('/pug_control/velocity_move', Velocity, queue_size=1)

time.sleep(0.1)

pose_pub.publish(0, 0, 0, -0.13, 0.01, 0, 0, 0.5)
time.sleep(0.5)

gait_pub.publish(0.2, 0.18, 0.02, 0.04)

velocity_pub.publish(0, 0.06, 0, False)
time.sleep(2)

velocity_pub.publish(0, -0.06, 0, False)
time.sleep(2)

velocity_pub.publish(0, 0, 0, True)

Publish movement parameters using the velocity_pub.publish() function.

velocity_pub.publish(0, 0.06, 0, False)
time.sleep(2)

(1) The first parameter 0 is the linear velocity in the x direction, in meters per second (m/s).

(2) The second parameter 0.06 is the linear velocity in the y direction, in meters per second (m/s).

(3) The third parameter 0 is the angular velocity, in meters per second (m/s).

(4) The fourth parameter False is the operation state; set to True to stop movement.

Here, a y-direction linear velocity of 0.06 m/s is published, with a delay of 2 seconds. Then, a y-direction linear velocity of -0.06 m/s is published, with another 2-second delay, achieving lateral movement of the robot dog. You can adjust the velocity to change the speed of lateral movement.

4.11 Omnidirectional Movement

While controlling the ROSPug robot dog, you can achieve omnidirectional movement by adjusting its motion parameters.

4.11.1 Operation Steps

Note

The entered command should be case sensitive, and Tab key can be used to complement key words.

(1) Start the robot, and access the robot system desktop using NoMachine.

(2) Click to open the command line terminal.

(3) Enter the following command to initiate the game.

python3 pug/src/pug_tutorial/scripts/intermediate_control_course/demo03_omnidirection_move.py

(4) If want to close this game, press Ctrl+C. If it fails to close, please try again.

4.11.2 Program Outcome

Once play mode is activated, the ROSPug robot dog will move to the front-left for 2 seconds, then to the back-right for another 2 seconds, following the preset directions.

4.11.3 Program Analysis

The source code of this program is saved in: /home/pug/src/pug_tutorial/scripts/intermediate_control_course/demo03_omnidirection_move.py

#!/usr/bin/env python3
# coding=utf8
import time
import rospy
from pug_control.msg import Velocity, Pose, Gait

rospy.init_node('omnidirection_move', log_level=rospy.INFO)

pose_pub = rospy.Publisher('/pug_control/pose', Pose, queue_size=1)
gait_pub = rospy.Publisher('/pug_control/gait', Gait, queue_size=1)
velocity_pub = rospy.Publisher('/pug_control/velocity_move', Velocity, queue_size=1)

time.sleep(0.1)

pose_pub.publish(0, 0, 0, -0.13, 0.01, 0, 0, 0.5)
time.sleep(0.5)

gait_pub.publish(0.2, 0.18, 0.02, 0.04)

velocity_pub.publish(0.06, 0.06, 0, False)
time.sleep(2)

velocity_pub.publish(-0.06, -0.06, 0, False)
time.sleep(2)

velocity_pub.publish(0, 0, 0, True)

Use the velocity_pub.publish() function to set movement parameters:

velocity_pub.publish(0.06, 0.06, 0, False)
time.sleep(2)

(1) The first parameter, 0.06, is the linear velocity in the x direction (m/s).

(2) The second parameter, 0.06, is the linear velocity in the y direction (m/s).

(3) The third parameter, 0, is the angular velocity (rad/s).

(4) The fourth parameter, False, indicates the running state; set it to True to stop the robot.

In this example, the robot dog moves with x and y velocities of 0.06 m/s for 2 seconds, then changes to -0.06 m/s for both directions for another 2 seconds, resulting in diagonal movement. Adjust these speed values to achieve movement in different directions.

4.12 Speed Adjustment

While controlling the ROSPug robot dog, you can adjust the movement parameters to modify its walking speed.

4.12.1 Operation Steps

Note

The entered command should be case sensitive, and Tab key can be used to complement key words.

(1) Turn on ROSPug, and access the robot system desktop using NoMachine.

(2) Click to open the command line terminal.

(3) Enter the following command to initiate the game.

python3 pug/src/pug_tutorial/scripts/intermediate_control_course/demo04_move_speed_adjustment.py

(4) If want to close this game, press Ctrl+C. If it fails to close, please try again.

4.12.2 Program Outcome

Once the play mode is activated, the ROSPug robot dog will walk at the set speed, with speed changes occurring during its movement.

4.12.3 Program Analysis

The source code of this program is saved in: /home/pug/src/pug_tutorial/scripts/intermediate_control_course/demo04_move_speed_adjustment.py

#!/usr/bin/env python3
# coding=utf8
import time
import rospy
from pug_control.msg import Velocity, Pose, Gait

rospy.init_node('move_speed', log_level=rospy.INFO)

pose_pub = rospy.Publisher('/pug_control/pose', Pose, queue_size=1)
gait_pub = rospy.Publisher('/pug_control/gait', Gait, queue_size=1)
velocity_pub = rospy.Publisher('/pug_control/velocity_move', Velocity, queue_size=1)

time.sleep(0.1)

pose_pub.publish(0, 0, 0, -0.13, 0.01, 0, 0, 0.5)
time.sleep(0.5)

gait_pub.publish(0.2, 0.18, 0.02, 0.04)

velocity_pub.publish(0.05, 0, 0, False)
time.sleep(2)

velocity_pub.publish(0.12, 0, 0, False)
time.sleep(2)

velocity_pub.publish(0.05, 0, 0, False)
time.sleep(2)

velocity_pub.publish(0, 0, 0, True)

Use the velocity_pub.publish() function to set movement parameters:

velocity_pub.publish(0.05, 0, 0, False)
time.sleep(2)

velocity_pub.publish(0.12, 0, 0, False)
time.sleep(2)

(1) The first parameter, 0.05, represents the linear velocity in the x direction (m/s).

(2) The second parameter, 0.00, represents the linear velocity in the y direction (m/s).

(3) The third parameter, 0, represents the angular velocity (rad/s).

(4) The fourth parameter, False, indicates whether the robot is running; set to True to stop the robot.

In this example, the robot dog will first move at an x-direction velocity of 0.05 m/s for 2 seconds. It will then switch to an x-direction velocity of 0.12 m/s for 2 seconds, before returning to 0.05 m/s for another 2 seconds. This sequence creates a variation in speed during the robot dog’s movement.

4.13 Height Adjustment

While controlling the ROSPug robot dog, you can adjust the parameters to modify its walking height. For detailed information on these parameters, see the document ‘4.9 Explanation of Pose (Static) Parameters’.

4.13.1 Operation Steps

Note

The input command is case-sensitive, and keywords can be auto-completed using the Tab key.

(1) Start the robot, and access the robot system desktop using NoMachine.

(2) Click to open the command line terminal.

(3) Enter the following command to initiate the game.

python3 pug/src/pug_tutorial/scripts/intermediate_control_course/demo05_move_height_adjustment.py

(4) If want to close this game, press Ctrl+C. If it fails to close, please try again.

4.13.2 Program Outcome

Once play mode is activated, the ROSPug robot dog will lower its body height while walking forward.

4.13.3 Program Analysis

The source code of this program is saved in /home/pug/src/pug_tutorial/scripts/intermediate_control_course/demo05_move_height_adjustment.py

#!/usr/bin/env python3
# coding=utf8
import time
import rospy
from pug_control.msg import Velocity, Pose, Gait

rospy.init_node('move_height', log_level=rospy.INFO)

pose_pub = rospy.Publisher('/pug_control/pose', Pose, queue_size=1)
gait_pub = rospy.Publisher('/pug_control/gait', Gait, queue_size=1)
velocity_pub = rospy.Publisher('/pug_control/velocity_move', Velocity, queue_size=1)

time.sleep(0.1)

pose_pub.publish(0, 0, 0, -0.13, 0.01, 0, 0, 0.5)
time.sleep(0.5)

gait_pub.publish(0.2, 0.18, 0.02, 0.04)

velocity_pub.publish(0.06, 0, 0, False)

for i in range(30):
    pose_pub.publish(0, 0, 0, -0.15 + i*0.001, 0, 0, 0, 0)
    time.sleep(0.1)

velocity_pub.publish(0, 0, 0, True)

Use the velocity_pub.publish() function to set movement parameters:

velocity_pub.publish(0.06, 0, 0, False)

for i in range(30):
    pose_pub.publish(0, 0, 0, -0.15 + i*0.001, 0, 0, 0, 0)
    time.sleep(0.1)

(1) The first parameter, 0.06, is the linear velocity in the x direction (m/s).

(2) The second parameter, 0.00, is the linear velocity in the y direction (m/s).

(3) The third parameter, 0, is the angular velocity (rad/s).

(4) The fourth parameter, False, indicates the running state; set to True to stop the robot.

for i in range(30):
    pose_pub.publish(0, 0, 0, -0.15 + i*0.001, 0, 0, 0, 0)
    time.sleep(0.1)

Use the pose_pub.publish() function to set pose parameters.

The height parameter is set to -0.15 + i*0.001, representing the foot position along the z-axis in meters.

In this program, the x-direction velocity is set to 0.06 m/s. While the robot moves forward, the pose parameters are adjusted to change the body height, allowing the robot dog to modify its height while moving.

4.14 Center of Gravity Adjustment

4.14.1 Center of Gravity Explanation

The center of gravity of the robot dog is located at a point that is evenly distributed from front to back and left to right of the body. Therefore, during walking, it maintains a dynamic balance and requires continuous adjustments of other body parts to keep the center of gravity within a stable threshold.

In general situations, there is no need to adjust the default center of gravity of the robot dog. However, when it needs to fulfill additional development requirements, it is essential to maintain the center of gravity within a stable threshold to avoid deviations or tilting of the body.

This can be understood similar to how a person’s center of gravity is different when walking normally compared to walking while carrying or carrying something. Thus, this illustrates the significance of adjusting the center of gravity.

4.14.2 Operation Steps

Note

The input command should be case sensitive, and users can Tab to complement keywords.

(1) Start the robot, and access the robot system desktop using NoMachine.

(2) Double-click to open the command-line terminal.

(3) Execute the command below to initiate the game.

python3 pug/src/pug_tutorial/scripts/advanced_control_course/demo01_gravity_center_adjustment.py

(4) If you need to terminate this game, you can use short-cut Ctrl+C. If the program cannot be terminated, please retry.

4.14.3 Program Outcome

After enabling the game, the ROSPug robot dog will walk forward with a forward tilt in the Trot gait.

4.14.4 Program Analysis

The source code is saved in: /home/pug/src/pug_tutorial/scripts/advanced_control_course/demo01_gravity_center_adjustment.py

#!/usr/bin/env python3
# coding=utf8
import time
import math
import rospy
from pug_control.msg import Velocity, Pose, Gait

rospy.init_node('trot_gait', log_level=rospy.INFO)

pose_pub = rospy.Publisher('/pug_control/pose', Pose, queue_size=1)
gait_pub = rospy.Publisher('/pug_control/gait', Gait, queue_size=1)
velocity_pub = rospy.Publisher('/pug_control/velocity_move', Velocity, queue_size=1)

time.sleep(0.1)

pose_pub.publish(0, 0, 0, -0.13, 0, 0, 0, 0.5)
time.sleep(0.5)

gait_pub.publish(0.2, 0.18, 0.02, 0.04)

velocity_pub.publish(0.06, 0, 0, False)

for i in range(40):
    pose_pub.publish(0, 0, 0, -0.13, 0 + i*0.001, 0, 0, 0)
    time.sleep(0.1)

for i in range(40):
    pose_pub.publish(0, 0, 0, -0.13, 0.04 - i*0.001, 0, 0, 0)
    time.sleep(0.1)

velocity_pub.publish(0, 0, 0, True)

Use the pose_pub.publish() function to send pose parameters.

for i in range(40):
    pose_pub.publish(0, 0, 0, -0.13, 0 + i*0.001, 0, 0, 0)
    time.sleep(0.1)

for i in range(40):
    pose_pub.publish(0, 0, 0, -0.13, 0.04 - i*0.001, 0, 0, 0)
    time.sleep(0.1)
    
for i in range(40):
    pose_pub.publish(0, 0, 0, -0.13, 0 + i*0.001, 0, 0, 0)
    time.sleep(0.1)
    
for i in range(40):
    pose_pub.publish(0, 0, 0, -0.13, 0.04 - i*0.001, 0, 0, 0)
    time.sleep(0.1)

(1) The parameter x_shift, defined as 0 + i*0.001, specifies the X-axis position of each leg in meters.

In this program, the robot dog moves forward at a speed of 0.06 m/s. During movement, the pose parameters adjust each leg’s position along the X-axis, creating a shifting effect where the robot’s center of gravity moves backward and then returns to its original position.

4.15 Twisting

4.15.1 Program Logic

The body twisting of the robot is mainly achieved by controlling the robot’s posture, i.e., changing the robot’s roll, pitch, and yaw angles to induce a twisting motion in the body.

Specifically, it can be divided into the following types of twisting movements:

(1) Left and right tilt (Roll): Changing the robot’s roll angle to tilt the body left or right around the forward axis.

(2) Up and down pitching (Pitch): Changing the robot’s pitch angle to make the body pitch up or down around the lateral axis.

(3) Left and right rotation (Yaw): Changing the robot’s yaw angle to make the body rotate left or right around the vertical axis.

(4) Combined twist: Simultaneously changing multiple angles to generate more complex twisting movements, for example, simultaneously changing roll and pitch to create a helical twisting effect.

(5) Dynamic twist: Continuously changing angles to create dynamic twisting movements, for example, sinusoidally changing the roll angle to achieve body oscillation.

The body’s twisting actions can fully demonstrate the flexibility of quadruped robots and are important indicators for assessing robot control performance. Good twisting capability is also crucial for the adaptability of quadruped robots in complex environments.

4.15.2 Operation Steps

Note

The input command should be case sensitive, and users can Tab to complement keywords.

(1) Start the robot, and access the robot system desktop using NoMachine.

(2) Double-click to open the command-line terminal.

(3) Execute the command below to start the game.

python3 pug/src/pug_tutorial/scripts/advanced_control_course/demo02_wave_body.py

(4) If you need to terminate this game, you can use short-cut Ctrl+C. If the program cannot be terminated, please retry.

4.15.3 Program Outcome

The ROSPug quadruped robot dog will repeatedly sway back and forth before returning to its original stance.

4.15.4 Parameters Analysis

The source code of this program locates in /home/pug/src/pug_tutorial/scripts/advanced_control_course/demo02_wave_body.py

#!/usr/bin/env python3
# coding=utf8
import time
import math
import rospy
from pug_control.msg import Pose

rospy.init_node('wave_body', log_level=rospy.INFO)

pose_pub = rospy.Publisher('/pug_control/pose', Pose, queue_size=1)

time.sleep(0.1)

pose_pub.publish(0, 0, 0, -0.13, 0, 0, 0, 0.5)
time.sleep(0.5)

for i in range(3):
    pose_pub.publish(0, math.radians(-15), 0, -0.13, 0, 0, 0, 0.5)
    time.sleep(0.5)
    pose_pub.publish(0, math.radians(0), 0, -0.13, 0, 0, 0, 0.5)
    time.sleep(0.5)

    pose_pub.publish(0, math.radians(15), 0, -0.13, 0, 0, 0, 0.5)
    time.sleep(0.5)
    pose_pub.publish(0, math.radians(0), 0, -0.13, 0, 0, 0, 0.5)
    time.sleep(0.5)

Publish pose parameters using the pose_pub.publish() function.

for i in range(3):
    pose_pub.publish(0, math.radians(-15), 0, -0.13, 0, 0, 0, 0.5)
    time.sleep(0.5)
    pose_pub.publish(0, math.radians(0), 0, -0.13, 0, 0, 0, 0.5)
    time.sleep(0.5)

    pose_pub.publish(0, math.radians(15), 0, -0.13, 0, 0, 0, 0.5)
    time.sleep(0.5)
    pose_pub.publish(0, math.radians(0), 0, -0.13, 0, 0, 0, 0.5)
    time.sleep(0.5)

(1) The pitch parameter, set to math.radians(15), defines the pitch angle of the robot dog’s body in radians.

In this program, the pitch angle is adjusted continuously, enabling the robot dog to alternate between looking up, looking straight ahead, and looking down.

4.16 IMU Calibration

4.16.1 IMU (Inertial Measurement Unit) Introduction

An IMU (Inertial Measurement Unit) measures an object’s orientation angles (angular rates) and acceleration along three axes. It includes a gyroscope and an accelerometer, offering 6 degrees of freedom to capture angular velocity and acceleration in three-dimensional space.

4.16.2 Operation Steps

When the node receives the first IMU message, it will instruct you to hold the IMU in a specific orientation and press Enter to record the measurement. After completing the measurements for all 6 orientations, the node will calculate the calibration parameters and save them to the specified YAML file. Follow the detailed steps below:

Note

The input command should be case sensitive, and you can Tab to complement the keywords.

(1) Power on the robot and connect it to the remote control software NoMachine.

(2) Click-on to open the command-line terminal.

(3) Execute the command below to initiate the app auto-start service.

sudo systemctl stop pug_bringup.service

(4) Run the following command to launch the IMU calibration.

roslaunch pug_peripherals imu_calibrate.launch

(5) When prompted, position the robot with its right side facing up and press Enter.

Note

The initial direction is defined as the front, and all subsequent orientations should be based on this initial direction.

After successfully calibrating each direction, you will see the following prompt:

(6) Position the robot with its left side facing up and press Enter.

(7) Position the robot with its rear side facing up and press Enter.

(8) Position the robot with its front side facing up and press Enter.

(9) Align the robot so that it is facing upwards. Once the robot is securely fixed in this position, press Enter.

Note

When adjusting the robot’s vertical orientation, it may become unstable or bump into objects. It is advisable to hold the robot steady to avoid damaging the depth camera or screen.

(10) Position the robot with its bottom facing up. Once the robot is securely fixed in this position, press Enter.

(11) When you see the prompt indicating calibration is complete, the process is finished.

(12) After calibration, you can enter a command to display the model’s three-axis orientation. Tilt the robot forward, backward, left, and right, and check whether the model’s tilt direction matches the robot’s tilt direction.

roslaunch pug_peripherals imu.launch debug:=true

4.17 ROS Robot Pose Detection

4.17.1 Program Logic

When controlling the state of the ROSPug, you can adjust its posture. Similarly, you can invoke the MPU6050 sensor function to detect the current posture of the robot dog.

Posture mainly includes parameters such as roll (roll angle), pitch (pitch angle), yaw (yaw angle), height (height of the robot dog), and more.

The source code is saved in: /home/pug/src/pug_tutorial/scripts/advanced_control_course/demo03_self_balance01.py

#!/usr/bin/env python3
# encoding: utf-8
import rospy
from sensor_msgs.msg import Imu
from scipy.spatial.transform import Rotation as R

class SelfBalancingNode:
    def __init__(self):
        rospy.init_node("self_balancing_node")
        rospy.Subscriber("/imu", Imu, self.imu_callback, queue_size=2)

    def imu_callback(self, imu_msg):
        try:
            q = imu_msg.orientation
            r = R.from_quat((q.x, q.y, q.z, q.w))
            x, y, z = r.as_euler('xyz', degrees=True)
            rospy.loginfo("Pitch:{:>6.2f}°, Roll:{:>6.2f}°, Yaw:{:>6.2f}°".format(x, y, z))
        except Exception as e:
            rospy.logerr(str(e))

if __name__ == "__main__":
    try:
        SelfBalancingNode()
        rospy.spin()
    except Exception as e:
        rospy.logerr(str(e))

4.17.2 Operation Steps

Note

Command input must strictly distinguish between uppercase and lowercase letters, as well as spaces.

(1) Restart the robot, and access the robot system desktop using NoMachine.

(2) Double-click to open the command-line terminal.

(3) Execute the command below to initiate the game.

python3 pug/src/pug_tutorial/scripts/advanced_control_course/demo03_self_balance01.py

(4) If you need to terminate this program, use short-cut Ctrl+C on LX terminal.

4.17.3 Program Outcome

After initiating the game, open the terminal, then you can check the current pose parameters of ROSPug robot dog.

4.17.4 Program Analysis

The source code of this program is saved in: /home/pug/src/pug_tutorial/scripts/advanced_control_course/demo03_self_balance01.py

Subscribe to IMU Topic

class SelfBalancingNode:
    def __init__(self):
        rospy.init_node("self_balancing_node")
        rospy.Subscriber("/imu", Imu, self.imu_callback, queue_size=2)

Subscribe to the /imu topic using the Subscriber() function, with self.imu_callback() as the callback function.

Callback Function

    def imu_callback(self, imu_msg):
        try:
            q = imu_msg.orientation
            r = R.from_quat((q.x, q.y, q.z, q.w))
            x, y, z = r.as_euler('xyz', degrees=True)
            rospy.loginfo("Pitch:{:>6.2f}°, Roll:{:>6.2f}°, Yaw:{:>6.2f}°".format(x, y, z))
        except Exception as e:
            rospy.logerr(str(e))

(1) Use imu_msg.orientation to obtain the quaternion data from the IMU.

(2) Convert the quaternion to a rotation matrix using the from_quat() function.

(3) Then, calculate the angles from the rotation matrix using the as_euler() function.

4.18 ROS Robot Pose Self-Balancing

4.18.1 Program Logic

The overall process of this program is as follows:

(1) Firstly, it is necessary to initialize the posture parameters of the robot dog, including roll (roll angle), pitch (pitch angle), yaw (yaw angle), height (height of the robot dog), and other parameters, to determine the initial posture of the robot dog.

(2) Then, call the QMI8658 sensor function to obtain real-time posture parameters of the robot dog and save the records.

(3) Finally, based on the changes in posture parameters, reconfigure the pitch and roll angles of the robot dog. Through inverse kinematics calculations, obtain the rotation angles of the servo motors, and control the servos to rotate to the specified angles, thus completing the self-balancing function of the robot dog.

4.18.2 Operation Steps

Note

Command input must strictly distinguish between uppercase and lowercase letters, as well as spaces.

(1) Restart the robot, and access the robot system desktop using NoMachine.

(2) Double-click to open the command-line terminal.

(3) Then execute the command to initiate the pose self-balancing program.

python3 pug/src/pug_tutorial/scripts/advanced_control_course/demo04_self_balance02.py

(4) If you need to terminate this program, use short-cut Ctrl+C on LX terminal.

4.18.3 Program Outcome

Note

When tilting the robot dog, the pitch angle range is from -25° to 25°, and the roll angle range is from -20° to 20°.

Once you start the process, IMU calibration will begin. The calibration is complete when you hear the buzzer signal. During this period, ensure that the ROSPug remains stationary.

After calibration, the ROSPug will automatically adjust its posture according to the surface’s tilt to maintain an upright stance.

4.18.4 Program Parameters Analysis

The source code of this program is saved in: /home/pug/src/pug_tutorial/scripts/advanced_control_course/demo04_self_balance02.py

#!/usr/bin/env python3
# encoding: utf-8
import time
import math
import rospy
from pug_sdk import buzzer, pid
from sensor_msgs.msg import Imu
from pug_control.msg import Pose
from scipy.spatial.transform import Rotation as R
from ros_robot_controller.msg import BuzzerState

class SelfBalancingNode:
    def __init__(self):
        rospy.init_node('self_balancing')
        self.pid_pitch = pid.PID(0.1, 0.0, 0.0002)
        self.pid_roll = pid.PID(0.13, 0.0, 0.0002)
        self.pitch = 0
        self.roll = 0
        self.init_finish = False
        self.pitch_init = 0
        self.roll_init = 0
        self.count = 0
        self.imu_sub = rospy.Subscriber('/imu', Imu, self.imu_callback, queue_size=2)
        self.pose_pub = rospy.Publisher('/pug_control/pose', Pose, queue_size=1)
        self.buzzer_pub = rospy.Publisher('/ros_robot_controller/set_buzzer', BuzzerState, queue_size=1)
        time.sleep(0.1)
        self.pose_pub.publish(0, 0, 0, -0.13, 0, 0, 0, 1)
        time.sleep(1.5)

Initialize Publishers and Subscribers

        self.imu_sub = rospy.Subscriber('/imu', Imu, self.imu_callback, queue_size=2)
        self.pose_pub = rospy.Publisher('/pug_control/pose', Pose, queue_size=1)

(1) Subscribe to the /imu topic using the Subscriber() function, with self.imu_callback() as the callback function.

(2) Publish pose messages to the /pug_control/pose topic using the Publisher() function.

Calculate Orientation Angles

            q = imu_msg.orientation
            r = R.from_quat((q.x, q.y, q.z, q.w))
            x, y, z = r.as_euler('xyz')

(1) Obtain the IMU data quaternion from imu_msg.orientation.

(2) Convert the quaternion to a rotation matrix using the from_quat() function.

(3) Calculate the angles from the rotation matrix using the as_euler() function.

Control Robot Self-Balancing

            if self.init_finish:
                self.pid_pitch.SetPoint = self.pitch_init
                self.pid_roll.SetPoint = self.roll_init
                if abs(x - self.pitch_init) < 0.04:
                    x = self.pitch_init
                self.pid_pitch.update(x)
                if abs(y - self.roll_init) < 0.02:
                    y = self.roll_init
                self.pid_roll.update(y)
                try:
                    self.pitch -= self.pid_pitch.output
                    self.roll += self.pid_roll.output
                    pitch_limit = 0.35
                    roll_limit = 0.30
                    self.roll = roll_limit if self.roll > roll_limit else (-roll_limit if self.roll < -roll_limit else self.roll)
                    self.pitch = pitch_limit if self.pitch > pitch_limit else (-pitch_limit if self.pitch < -pitch_limit else self.pitch)
                    self.pose_pub.publish(self.roll, self.pitch, 0, -0.13, 0, 0, 0, 0)

(1) Use the PID algorithm to calculate the roll and pitch angles.

(2) Publish the pose message with self.pose_pub.publish(self.roll, self.pitch, 0, -0.13, 0, 0, 0, 0) to control the robot’s self-balancing.

(3) The parameters self.roll and self.pitch represent the roll and pitch angles, calculated using PID.

4.19 Wireless Handle Control

4.19.1 Preparation

Note

Please ensure that the handle receiver is inserted before turning on the wireless handle. If it is already plugged in, disregard this step. Also, when inserting the battery, pay attention to distinguish between the positive and negative terminals.

(1) Insert the handle receiver into any USB interface on ROSPug. Generally, the handle receiver has already been inserted to the robot before leaving the factory.

(2) Please bring your own two AAA dry batteries. And insert them into the battery slot.

4.19.2 Device Connection

(1) Start ROSPug. After the robot starts successfully, turn on the handle. At this time, two LED lights will flash simultaneously.

(2) Please wait for a while. Then the robot will pair with the handle automatically. After successful pairing, the green light will keep lighting up and red LED light goes out.

Note

If the handle doesn’t connect to the robot within 30s or there is no operation on the handle within 5 minutes after turning on, it will enter sleep mode. And you can press “START” to activate the handle.

4.19.3 Button Function

Below are the functions of the buttons and joysticks, presented from the perspective of ROSPug.

Note

A gentle push on the joystick in any direction can achieve slow-motion movement.

Button	Function
START	Restore to Initial Pose
↑	Move Forward
↓	Move Backward
←	Turn Left
→	Turn Right
△	Raise Body Height
×	Lower Body Height
◻	Move Left
○	Move Right
L1	Tilt Body Upward
L2	Tilt Body Downward
R1	Twist Body Left
R2	Twist Body Right
Left joystick	Control the robot to go forward, backward, translate left and right
Right joystick	Turn left and right