Quadruped Basics

Things are getting clearer now.

The analysis of forward and reverse kinematics and etc. will be supplemented in the future from manual scripts.

2D - Modelling

tbc

3D - Modelling

tbc

Pitch

Roll

Yaw

Trajectory Planning

Cycloid

Bazier

Dynamics Basics

Previously, the modelling of legs is done, however, only the kinematics part is not enough. As the robot is in motion, which is a time-variant system. During the process, velocity and acceleration may change with time. Natrually, force is involved with acceleration, and the analysis of force requires dynamics.

1.Velocity and static force

Starting with rigid motion.

1.1 Time-variant representations

Differential of position vector

When the mass of an object can be concentrate into one point, the rotation of the object itself can be ignored, and only linear velocity needs to be considered.

Define a point $Q$ in reference coodinate $B$, the position can be represent as $^B P_Q$, and the velocity as $^B V_Q$

The following relation exists:

$^B V_Q=\frac{d^B P_Q}{dt}=\lim_{\triangle t\rightarrow0 }\frac{^B P_Q(t+\triangle t)- {^B P_Q(t)}}{\triangle t}$

The reference frame is very important, as the frame itself may be moving.

Similar to transformation of position, the velocity can also be changed between coordinate.

$^A(^B V_Q)=^A(\frac{d^B V_Q}{dt})=^A_B R^B V_Q$

In most cases, there’ll be discussion about the relative velocity of origin to the world reference frame, without considering the relative velocity to any other coordinate:
$v_C={^U V_{CORG}}$

Angular velocity

When the object cannot be regard as an mass point, the rotation needs to be considered, which will usually be represented as $\Omega$.

Assume to observe point $Q$ from coordinate $\{B\}$, and the position is invariant to time, while $\{B\}$ is rotating relatively to $\{A\}$, compute the velocity of $Q$ in $\{A\}$,

$^A V_Q = {^A\Omega_B} \times {^A P_Q}$

The cross product equation: $a\times b=(l,m,n)\times(o,p,q)=(mq-np,no-lq,lp-mo)$

In which,

$^A P_Q = {^A_B R^B P_Q}$

Normally, $Q$ is changing relatively to $\{B\}$, thus the linear velocity compoent.

$^A V_Q={^A_B R {^B V_Q}}+{^A\Omega_B}\times{^A_B R_BP_Q}$

Adding velocity of origin,

$^A V_Q={^A V_{BORG}}+{^A R_B {^B V_Q}}+{^A\Omega_B}\times{^A_B R_BP_Q}$

2. Link motion

Acording to D-H parameters, calculate angular velocity:

$^i\omega_{i+1}={^i\omega_{i}}+ {^i_{i+1}R\dot{\theta}_{i+1}}{^{i+1}}\hat{Z}_{i+1}$

where:

$\theta_{i+1}{^{i +1}}\hat{Z}_{i+1}={^{i+1}}\left[\begin{array}{c}0\\0\\\dot{\theta}_{i+1}\end{array}\right]$

To note that, in DH parameter, the revolute joints are defined as $Z$ axis, hence
$\theta_{i+1}{^{i+1}\hat{Z}_{i+1}}$

As the angular velocity of $(i+1)-th$ link is produced by joint motion, so its angular velocity with respect to $i-th$ link, equals the raw angular velocity of link $i$ plus the joint speed of link $i+1$

Multiply $^{i+1}_i R$ on both sides to change observation coordinate,

$^{i+1}\omega_{i+1}=^{i+1}_i R^i \omega_i+\dot{\theta}_{i+1}{^{i+1}}\hat{Z}_{i+1}$

For linear velocity

$^i v_{i+1}={^iv_i}+{^i\omega_i}\times{^iP_{i+1}}$

Similarly,

$^{i+1}v_{i+1}={^{i+1}_i}R(^iv_i+{^i\omega_i}\times{^iP_{i+ 1}})$

Dynamics Modelling

1. Review

The equation for revolute joint are presented above. As for prismatic joints, the idea is about the same:

$^{i+1}\omega_{i+1}={^{i+1}_iR{^i\omega_i}}$ $^{i+1}v_{i+1}={^{i+1}_iR(^iv_i+{^i\omega_i\times{^iP_{i+1}}})}+\dot{d}_{i+1}{^{i+1}\hat{Z}_{i+1}}$

2. Rigid body acceleration

2.1 Linear acceleration

Same as the previous process, differentiate the velocity to obtain acceleration.

$^BA_Q=\frac{d^BV_Q}{dt}=\lim_{\triangle t\rightarrow0}\frac{^BV_Q(t+\triangle t)-^BV_Q(t)}{\triangle t}$

Thr transformation between coordiantes are

$^AA_Q={^AA_{BORG}}+^A\dot{\Omega}_B\times{^A_BR^BP_Q}+{^A\Omega_B\times({^A\Omega_B\times{^A_BR^BP_Q}})}+2{^A\Omega_B\times{^A_BR^BV_Q}}+{^A_BR^BA_Q}$

2.2 Angular acceleration

$^A\dot{\Omega}_B=\frac{d^A\Omega_B}{dt}=\lim_{\triangle t\rightarrow0}\frac{^A\Omega_B(t+\triangle t)-{^A\Omega_B(t)}}{\triangle t}$

Transformation between coordinate is as follows, asusme the reference frame $\{B\}$ is rotating at an angular speed $^A\Omega_B$ with respect to reference frame $\{A\}$, and frame $\{C\}$ is rorating at the speed $^B\Omega_C$ with respect to frame $\{B\}$

$^A\Omega_C={^A\Omega_B}+{^A_BR^B\Omega_C}$ $^A\dot{\Omega}_C={^A\dot{\Omega}_B}+{^A_BR^B\dot{\Omega_C}}+{^A\Omega_B}\times{^A_BR^B\Omega_C}$

3. Parallel axis theorem

The theorem can be used to determine the moment of inertia or the second moment of area of a rigid body about any axis, given the body’s moment of inertia about a parallel axis through the object’s center of gravity and the perpendicular distance between the axes.

$^AI={^CI}+m(P^T_CP_CI_3-P_CP^T_C)$

where $P_C=[x_C\ y_C\ z_C]^T$, representing the center of mass in coordinate $\{A\}$.

4. Newton / Euler equation

In Newton’s equation, $m$ is the mass of rigid body.

$F=m\dot{v}$

In Euler’s equation, $\dot{\omega},\ \omega$ are angular acceleration and angupar speed respectively, $^CI$ represent the moment of inertia in rigid mass center corrdinate.

$N={^CI\dot{\omega}}+\omega\times{^CI\omega}$

5. Iterative Newton-Euler dynamics

To calculate the inertia force applied to the link, the angular speed, linear speed and angular acceleration is required.

5.1 Iteration of Acceleration

Angular acceleration

To differentiate the angular speed equation, to obtain angular acceleration.

$^{i+1}\dot{\omega}_{i+1}={^{i+1}_iR{^i\dot{\omega}_i}}+{^{i+1}_iR}{^i\omega_i}\times\dot{\theta}_{i+1}{^{i+1}\hat{Z}_{i+1}}$

Linear acceleration

$^{i+1}a_{i+1}={^{i+1}_i}R({^{i+1}_i}a_i+{^i\dot{\omega_i}\times{^iP_{i+1}+{^i\omega_i}({^i\omega_i}\times{^iP_{i+1}}}})$

Acceleration of mass center

$^ia_{c_i} = {^ia_i}+{^i\dot{\omega}_i\times{^iP_{c_i}}}+{^i\omega_i}\times({^i\omega_i\times {^iP_{c_i}}})$

5.2 Iteration of force and torque

$^if_i={^iF_i}+{^i_{i+1}R{^{i+1}}f_{i+1}}$ $^in_i={^i_{i+1}R^{i+1}n_{i+1}}+{^iN_i} + {^iP_{c_i}}\times{^iF_i} + {^iP_{i+1}}\times {^i_{i+1}R^{i+1}f_{i+1}}$

In static force, the joint torque can be derived via the torque applied to adjacent one by link on the $\hat{Z}$ component.

$\tau={^in^T_i}{^i\hat{Z}_i}$

Unitree Gazebo

Previously, I tried once fore. But, at that time, I was in a hurry and barely had time to read the instructions thoroghouly.

The main problem was about the SKD called unitree_ros_to_real, in which the legged_msgs is required for simulating model A1, but I’ve been having isssues compiling the other one designed for manipulating real robots.

I was not aware of this, and the simplest solution was to remove that package.

Kinda dumb.

So, according to the offical readme.md

Robots and joints controller can be loaded in Gazebo, in order to perform low-level control (e.g. torque, position and angular velocity of the joints), while the high-level control, namely wakling is not supported.

Dependencies

ROS Melodic or Kinetic
Gazebo
unitree_legged_msgs: It’s a package under unitree_ros_to_real

Build

For Melodic

1	sudo apt-get install ros-melodic-controller-interface ros-melodic-gazebo-ros-control ros-melodic-joint-state-controller ros-melodic-effort-controllers ros-melodic-joint-trajectory-controller

In the file unitree_gazebo/worlds/stairs.world. At the end:

1
2
3

<include>
	<uri>model:///home/unitree/catkin_ws/src/unitree_ros/unitree_gazebo/worlds/building_editor_models/stairs</uri>
</include>

Change the path of building_editor_models/stairs to real path.

Then use catkin_make to build:

1 2	cd ~/catkin_ws catkin_make

unitree_ros_to_real

Basic message function: unitree_legged_msgs
The interface between ROS and real robot: unitree_legged_real

Build

Download the package, throw it in to source directory and then

1 2	cd ~/ws catkin_make

unitree_legged_sdk

It’s really strange that the different versions of SDK supports different product.

Dependencies

Boost
CMake
g++

Build

mkdir build
cd build
cmake ../
make

One more thing

Remember to source the workspace in order to get things linked.

Details of Packages

The description of robots

It contains the description of Go1, A1, Aliengo and Laikago, including mesh, urdf and xacro files of robot.

Model can be checked by

1	roslaunch model_description model_rviz.launch

unitree_legged_controll

It contains the joints controllers for Gazebo simulation, which allows users to control joints with position, velocity and torque.

Refer to unitree_ros/unitree_controllers/src/servo.cpp for joint control examples in different modes.

unitree_gazebo & unitree_controller

Launch gazebo simulation with the following command

1	roslaunch unitree_gazebo normal.launch rname:=a1 wname:=stairs

Stand controller

After launching the gazebo, you can start to control the robot:

1	rosrun unitree_controller unitree_servo

External disturbances is also available

1	rosrun unitree_controller unitree_external_force

Position and pose publisher

Here we demonstrated how to control the position and pose of robot without a controller, which should be useful in SLAM or visual development.

Then run the position and pose publisher in another terminal:

1	rosrun unitree_controller unitree_move_kinetic

The robot will turn around the origin, which is the movement under the world coordinate frame. And inside of the source file move_publisher.cpp, we also provide the method to move using the robot coordinate frame. You can change the value of def_frame to coord::ROBOT and run the catkin_make again, then the unitree_move_publisher will move robot under its own coordinate frame.

Quadruped Basics

Table of Contents

2D - Modelling

3D - Modelling

Pitch

Roll

Yaw

Trajectory Planning

Cycloid

Bazier

Dynamics Basics

1.Velocity and static force

1.1 Time-variant representations

Differential of position vector

Angular velocity

2. Link motion

Dynamics Modelling

1. Review

2. Rigid body acceleration

2.1 Linear acceleration

2.2 Angular acceleration

3. Parallel axis theorem

4. Newton / Euler equation

5. Iterative Newton-Euler dynamics

5.1 Iteration of Acceleration

Angular acceleration

Linear acceleration

Acceleration of mass center

5.2 Iteration of force and torque

Unitree Gazebo

Dependencies

Build

unitree_ros_to_real

Build

unitree_legged_sdk

Dependencies

Build

One more thing

Details of Packages

The description of robots

unitree_legged_controll

unitree_gazebo & unitree_controller

Stand controller

Position and pose publisher

Estus