Monday, August 13, 2012

Simulation of a humanoid robot

Today, I'll show some of the work I did last year in my university. I worked in a project titled "Stability Control of a Humanoid Robot". The project goal was to test a stability control algorithm on a humanoid robot in a virtual environment. This project was a first step toward the actual implementation of a humanoid robot.

I'm going to show the steps I followed to achieve a full simulation environment. And I hope these steps allow other people to do simulations of robots (not necessarily bipedal robots).

Mechanical design

The project started with the mechanical design of the humanoid robot. Since my faculty had servomotors, they were used as part of the design. The mechanical design was developed on Solidworks, and was inspired on various commercial designs. The robot has 18 degrees of freedom, 5 per leg and 4 per arm, and has no head!

Solidworks render of the mechanical design

The solidworks design provides pretty much all the necessary information (dimensions, mass and inertia) for a dynamic simulation.

Mass properties from Solidworks

The simulation environment

Next task was selecting a simulation environment, at that time my team and I only knew the Simulink environment from MATLAB, so the choice was limited. The Simmechanics toolbox was used to simulate the multibody dynamics of the robot.

The simulink environment

Bodies and joints

Simmechanics provides body and joint blocks to construct various mechanical assemblies. A body holds the inertial information necessary for a dynamic simulation. And two bodies can be connected via a joint, this joint can add zero or more degrees of freedom between the bodies.

6 DOF Joint between the humanoid torso and the ground (absolute reference point)

The bodies and joints are referenced to each other via Coordinates Systems (CS), which can be absolute or relative. Also, sensors and actuators can be attached to the bodies or to the joints for control applications.

Torso (body) properties: Inertial parameters and CS

There is a plugin for Solidworks called Simmechanics Link, that allows the conversion from a Solidworks assembly to a Simulink model. However, the bodies and joints were laid out manually for greater flexibility and control.

Modeling the floor

The problem with the Simmechanics toolbox was the lack of object collision, which is crucial for the simulation, as the normal and friction forces between the floor and the robot are key for gait.

To solve this problem, the floor was modeled as a PD controller. This  controller exerted normal forces on a few selected points of the robot sole, only when these points were on or below the floor level.

8 support points selected per foot.

The error input of this PD controller was the deviation of these points from the floor level. High proportional and derivative gains were used to minimize the sinking of the feet on the floor. The floor also exerted viscous friction forces, when the feet was in contact with the floor.

Floor controller block diagram.

Modeling the servomotor

For the servomotor model, specifications like the maximum angular speed and the maximum torque were used to produce an accurate DC motor model. For the servomotor controller model, a position + speed controller with gravity compensation and angular speed feedforward was used. Full block diagram is below.

Servomotor model

Each servomotor was attached to each joint via a joint actuator and joint sensors, as shown in the following image.

Actuated joint

Putting it all together

After modelling the servomotor and the floor, the rest of the work consisted in assigning each body its inertial parameters and its 3d model (.stl) and then gluing everything together with the servomotors. The final result is shown below.

High level assembly


The simulation of this whole system can take a considerable amount of time, depending on the number of degrees of freedom, CPU power, etc. Here's a tip about the animation, you can speed up a little the animation (not the simulation), by accessing the menu Simulation > Control Animation Speed and reducing the "Delay per frame" and tweaking the "Visualization sample time".

Controlling the animation speed

Making it move

Moving the robot is as simple as giving some references to the servomotors, but making it walk is a different history. The references that can be send to the servomotors are angular positions (denoted as Q), but usually you want the robot to follow trajectories in the XYZ space.

The process of going from the Q space or articular space to the XYZ space is called forward kinematics, and the reverse process is called inverse kinematics. The forward kinematics is usually easier to compute, and is required to compute the inverse kinematics.

Forward kinematics

To compute the forward kinematics, the Denavit Hartenberg approach was used. I won't go into much detail but basically, the Denavit Hartenberg transformation matrices can map positions and orientations from one coordinate system to another.

With these Denavit Hartenberg transformation matrices, the position of any body part seen from any other body part (generally one of the feet) can be easy computed.

Inverse Kinematics

Using the forward kinematics, an analytic expression of the position of any body part seen from any other body part can be derived, the inputs of this expression are the joint angles (Q) and the output is the XYZ position. Computing the inverse of this expression is impossible, as there are many solutions.

Since no analytical expression for the inverse kinematics can be computed, an iterative method name Damped Least Squares (DLS) was used. I won't cover the mathematics of the DLS method here, but it uses the pseudo jacobian to approach the desired position iteratively.

The trajectory

The chosen gait trajectory consist of a spline for the foot trajectory and another spline for the hip trajectory for the single support phase, i.e. when only one foot is in contact with the floor.

Gait trajectory

This continuous trajectory was discretized in 50 points, and these points were transformed to the Q space using the inverse kinematics. The Q coordinates computed were finally fed at 50Hz to the servomotors.

The stability indicator

Trying to walk just by using the previous computed references will result in a failure, it's necessary to implement a stability control. And for that we need to select a stability indicator.

For this project, we used the Zero Moment Point (ZMP) indicator, which is basically an extension of the concept of center of mass in a dynamic context. You might know that a static object can stay in equilibrium as long as the projection (along the gravity axis) of its center of mass is inside its support polygon.

The ZMP takes in consideration the center of mass and the acceleration of the body and must reside inside the support polygon to guarantee the stability of the body.

The ZMP indicator computation can be reduced to the cart table problem, as sketched in the following image, for each plane of interest.

Cart table model for the XZ plane.

The stability control

Our problem reduces to keeping the ZMP inside the support polygon, to achieve this we implement two separate controls.

  • Feet control, maintains the foot parallel to the floor maximizing the support polygon. 
 Left: Feet control disabled. Right: Feet control enabled
  • Hip control, move the ZMP inside the support polygon.
Hip control in action, with disturbance at t = .25s.
Yellow: ZMP, Cyan/Magente: Support polygon

The final result

Here is your reward, for getting this far.


  • Prof. Jose Oliden, our advisor
  • Victor Paredes and Santiago Cortijo, team members
  • Control systems and Artificial Intelligence Research Group (GISCIA) 
  • National University of Engineering (UNI)



  1. Hey, your project is excellent! I am also using Simmechanics toolbox for simulation. Thus, would you mind sharing your matlab files with me, especially the part of simmechanics. Thus, I can use it as my reference. My e-mail address is Thank you.

    1. Hi anon,

      I'll need to discuss with my team, whether I can or can't release the MATLAB / Simmechanics files.

      What I can do is a tutorial on how to simulate multibody systems and how to simulate a "floor" in Simmechanics.

      Stay tuned.

    2. Hi Jorge

      Thanks for your reply! My name is Ryan. I am also doing a research on bipedal walking.

      You have mentioned Denavit Hartenberg transformation matrices for deriving the kinematic model of bipedal robot, right? I have a question:

      Have you guys derived two kinematic model in right support phase and left support phase respectively?

      Or, just a kinematic model which can describe both right support phase and left support phase?

      Since I am the 1st case, then I don't know how to model it in simmechanics.

    3. Hello Ryan,

      We used SimMechanics to model the DYNAMICS (the kinematics alone are not really useful) of the system (the robot). To model the dynamics you don't need the Denavit Hartenberg (DH) transformation matrices, you only need distances, masses, inertias and the SimMechanics building blocks: bodies and joints.

      You can use either of these two approaches to model the system:

      + You fix either foot, which will become the support foot, to the ground. This is, you use a weld joint between the foot and the ground.

      (This is the easier approach. It's not really useful as the robot will never walk, because it won't be able to switch its support foot.)

      + Or you don't fix any foot, but you model a "floor". This is you model the contact forces between the foot and the "floor".

      (This is how we simulated the system in the video)

      Regarding the kinematics, which are only useful to go from angles (Q) to positions (XYZ) and viceversa:

      We've used various kinematic chains, but the most important ones are:

      Left Foot -> Hip (and its inverse: Hip -> Left Foot)
      Right Foot -> Hip (and its inverse: Hip -> Right Foot)

      From these you can derive the Left Foot -> Right Foot and Right Foot -> Left Foot chains.

      Here, I'll recommend working with the transformation matrices instead of deriving analytic expressions (like X = d * cos(theta) * sin(alpha) + ...), which can become very large resulting in slow evaluation times.

      The tutorial post might come out by the next weekend...

  2. Hi Jorge

    Thanks for your reply again!

    (My approach)

    Part 1

    + By using D-H notation, I have developed the forward kinematic model and inverse dynamic model. These two model are used for finding a set of optimized joint trajectories. Since a set of joint trajectories is found, I do not need any inverse kinematic model. Also, This part is done.

    Part 2

    + Now, I would like to use the simmechanics to model a forward dynamic model in order to control the robot in a virtal environement. Since I do not need any inverse kinematic model, according to your experience, then what I need to do is to model the ground and friction?

    I look forward to your tutoiral =)

  3. Excellent work,
    I am looking forward your tutorial.
    Now I am trying to do same work with you. My robot is kondo robot khr-3hv (22dof)
    I completely design my robot on solidwork 2012,
    and a also converted to simmechanics 2 genaration (matlab 2012b),
    Can you give me some tutorials,

  4. Excellent work,
    I am looking forward your tutorial.
    Now I am trying to do same work with you. My robot is kondo robot khr-3hv (22dof)
    I completely design my robot on solidwork 2012,
    and a also converted to simmechanics 2 genaration (matlab 2012b),
    Can you give me some tutorials,


  5. Hola Jorge,
    Estoy trabajando en la simulación de un exoesqueleto y he encontrado tu trabajo muy interesante a la vez que instructivo. De todas formas hay todavía algunas cosas que no acabo de entender y me preguntaba si podríamos comunicarnos via e-mail y discutir sobre el asunto.

    Muchas gracias.

  6. How about if you have a pet robo???
    Meet the multi-talented Humanoid Robot- AIHRO...!!!
    Do Watch and Share

  7. Hi Jorge, i'm trying to do the same, but it's not working. Did You use the gravity vector in the enviromental block? When I'm doing this, the whole robot is just falling down, and I don't know what to do. could You give me some advices?

  8. Hi, your project is amazing,
    I have a Question in the PD servo model, If the gear ratio has for example 100 how do you insert this gain in the pd servo model? I will try to implement a pd position control of a dc motor I have all constants but I don't know how to put the gear ratio . Regards

  9. Hi, your project is amazing,
    I have a Question in the PD servo model, If the gear ratio has for example 100 how do you insert this gain in the pd servo model? I will try to implement a pd position control of a dc motor I have all constants but I don't know how to put the gear ratio . Regards

  10. Hola amigo, que Buen trabajo, Felicitaciones, Favor me podrías brindar algu archivo de Simulación (matlab) o más informacion que pueda desarrollar. Muchas Gracias,Saludos desde Perú.
    MI correo:
    Pd.La Investigación es la clave del desarrolo!!

  11. This comment has been removed by the author.

  12. Dear Mr.Jorge ,
    Can you tell me how I can calculate moment of inertia, if I don't use Simmechanics.
    Thanks in advance.