Theory of dynamics

 

Step 1: Extract Robot Parameters from URDF

Your URDF describes the robot's geometry, mass, inertia, joints, and visuals—perfect to retrieve needed physical parameters for SRBD.

How to extract key parameters:

• Import your URDF into a robotics framework to read kinematics and inertial properties, e.g., using MATLAB Robotics Toolbox (importrobot), Python with urdfpy or ROS tools.

MATLAB Example for loading URDF and extracting mass & inertia:

matlab
robot = importrobot('your_robot.urdf');
bodyNames = {robot.Bodies.Name};
for i = 1:length(robot.Bodies)
    body = robot.Bodies{i};
    fprintf('Body %s has mass %.3f\n', body.Name, body.Mass);
    disp('Inertia tensor (about body frame origin):');
    disp(body.Inertia);
end

What to extract:

• Total mass (m): sum of all body masses or use trunk mass if legs are negligible.

• Inertia tensor (I) of the trunk: combine all body inertias referred to the center of mass frame. This is critical for the rotational dynamics.

• Center of mass (CoM) position of the robot or trunk.

• Foot positions relative to CoM: you may compute foot positions from forward kinematics on your robot.

Step 2: Define State and Variables for SRBD

The SRBD model states commonly include:

• ${\mathbf{p}}_{CoM}$: 3D position of center of mass (world frame)

• ${\mathbf{v}}_{CoM}$: linear velocity of CoM

• $\mathbf{R}$: 3x3 rotation matrix representing base orientation

• $\mathit{\omega }$: angular velocity of the robot's trunk (body frame)

Inputs are the contact forces at the feet:

• ${\mathbf{F}}_i$, for each foot $i$ currently in contact.

Step 3: Formulate Newton-Euler Equations for SRBD

Use:

• Translational:

  $$m{\ddot{\mathbf{p}}}_{CoM}=\sum _{i=1}^N{\mathbf{F}}_i+m\mathbf{g}$$

• Rotational:

  $$\mathbf{I}\dot{\mathit{\omega }}+\mathit{\omega }\times (\mathbf{I}\mathit{\omega })=\sum _{i=1}^N{\mathbf{r}}_i\times {\mathbf{F}}_i$$

Here,

• $\mathbf{I}$ = inertia tensor of the trunk about CoM in the body frame.

• ${\mathbf{r}}_i$ = vector from CoM to foot $i$ in the body frame.

• ${\mathbf{F}}_i$ = foot contact forces in world or body frame (transform forces as needed).

Step 4: Implement the Newton-Euler Equations for Integration

Coding sketch in Python

python
import numpy as np

def skew(vec):
    """ Return skew symmetric matrix for cross product """
    return np.array([
        [0, -vec[2], vec[1]],
        [vec[2], 0, -vec[0]],
        [-vec[1], vec[0], 0]
    ])

def srbd_step(m, I_body, r_feet, F_feet, g, p_com, v_com, R, omega, dt):
    """
    Perform one integration step of the SRBD Newton-Euler equations
    
    Inputs:
    - m: robot mass
    - I_body: inertia tensor of body frame (3x3)
    - r_feet: list of foot positions relative CoM in body frame (Nx3)
    - F_feet: list of contact forces at each foot in world frame (Nx3)
    - g: gravity vector in world frame (3,)
    - p_com: position of CoM in world frame (3,)
    - v_com: velocity of CoM in world frame (3,)
    - R: rotation matrix (body -> world) (3x3)
    - omega: angular velocity of body in body frame (3,)
    - dt: timestep
    
    Returns updated p_com, v_com, R, omega
    """
    # Sum forces in world frame
    F_total = np.sum(F_feet, axis=0) + m * g
    a_com = F_total / m  # linear acceleration in world frame

    # Compute torques in body frame
    tau = np.zeros(3)
    for r_i, F_i in zip(r_feet, F_feet):
        # Convert F_i to body frame
        F_i_body = R.T @ F_i
        tau += np.cross(r_i, F_i_body)

    # Angular acceleration in body frame
    omega_dot = np.linalg.inv(I_body) @ (tau - np.cross(omega, I_body @ omega))

    # Integrate linear
    v_com_new = v_com + a_com * dt
    p_com_new = p_com + v_com * dt + 0.5 * a_com * dt**2

    # Integrate angular velocity
    omega_new = omega + omega_dot * dt

    # Integrate orientation matrix using Rodrigues' formula approximation
    omega_skew = skew(omega_new)
    R_new = R @ expm(omega_skew * dt)  # use scipy.linalg.expm or approximate for small dt

    return p_com_new, v_com_new, R_new, omega_new

Note: For integration of the rotation matrix, you can use the matrix exponential of the skew symmetric omega vector or quaternion integration methods.

What to provide from URDF or robot interface:

• m: total trunk body mass

• I_body: inertia tensor at CoM (adjust frame if needed)

• r_feet: foot positions relative to CoM in body frame (compute from URDF fixed transforms or FK)

• F_feet: contact forces at current timestep (from sensors, MPC solver or estimates)

• Initial conditions: p_com, v_com, R, omega

Step 5: Things to Consider / Checklist for Proper Application

Step	Key Points & Checks
Parameter Accuracy	Extract precise mass, CoM location, and inertia tensor from URDF. Cross-check with CAD or measurements.
Frame Consistency	Confirm all vectors (forces, positions, velocities) are expressed in consistent frames (world or body). Use correct transforms.
Contact Forces Model	Your control or estimation method must produce physically realistic foot forces respecting friction and contact constraints.
Numerical Integration	Choose appropriate numerical methods and time steps to avoid instability. Quaternion-based or matrix-based orientation integration.
Validation	Validate your SRBD results by comparing simulation vs real robot motions and/or higher fidelity dynamic models.
Handling Contacts	Switch foot contacts on/off during gait cycles correctly to avoid force discontinuities.
Gravity Vector	Ensure gravity vector matches ROS/Gazebo's convention used in your URDF and environment.
Initial Conditions	Initialize states (pose, velocity) accurately from your robot's sensors or simulator.
Unit Consistency	All units (mass, length, forces) must be consistent (e.g., SI units like kg, m, N).

Step 6: Integration into Your ROS Gazebo Pipeline

1. Extract and parse URDF parameters as above.

2. Implement Newton-Euler SRBD module as a ROS node or part of control code.

3. At each control step:

   • Get robot pose and velocity estimation.

   • Compute or receive estimated/measured foot contact forces.

   • Calculate accelerations and integrate to predicted states.

4. Use this model prediction in your MPC or controller to generate inputs.

5. Test in simulation (Gazebo with your URDF robot first).

6. Tune parameters (mass, inertia, friction) based on observed behavior.