I'll demonstrate how to simulate a floor in Simmechanics, this floor will prevent free falling (6 DOF) objects from falling below a threshold level ("the floor level"), i.e. I will simulate a collision/interaction between the free falling object and the floor.
I'll assume you have a basic understanding of how Simmechanics works. If you don't, you might want to check this post.
If you have used Simmechanics before, then you know it's an excellent toolbox for simulation of multibody dynamics. You can easily work with robotic arms, which are always fixed to the ground. But what happens when you want to work with legged robots, that can't be fixed to the ground. Well, then you run in a problem, if you don't fix any part of the robot to the ground, then the robot will fall and fall by action of the gravity. Removing the gravity is not an option, because you want to design the controllers of your robot with gravity compensation, even worse your robot will float if you simulate a zero gravity environment.
Let's develop a minimal model to illustrate the problem: a 6 DOF ball.
Illustrating the problem
We'll start by drawing the floor. Implement the following block diagram in Simulink.
Block diagram to visualize the floor.
Modify the Floor block parameters to match the following image, and also on the Visualization tab change the Body geometry option to "Equivalent ellipsoid from mass properties".
Parameters to draw a 1 meter radius floor.
Next, start a simulation. Activate the the Isometric View and the "Enable Automatic Expanding Fit".
Animation showing the floor as a 1 m radius disk.
Now, we add the free falling ball. Add more blocks to the model to match the following diagram.
Added a free falling ball to the model.
Next, modify the parameters of the Above Ground block and the Ball block, to match the following images. On the Visualization tab of the ball block also select "Equivalent ellipsoid from mass properties" on the Body geometry option.
Above Ground parameters
Now simulate the system and observe the problem.
Ball falling through the floor.
Known the problem, we must now create a solution. Let's begin by analyzing how the floor behaves. Ideally the floor is a rigid body that exerts enough force over adjacent bodies to prevent them from falling through. Now, perfect rigid bodies doesn't exist in the real world, all bodies deform when stressed or compressed. This means our floor model must be elastic, this can be modeled as a spring.
Our floor model can't be modeled only as a spring, otherwise will end with a trampoline. When we fall to the ground, we don't jump back to the sky, instead our potential energy dissipates as heat or as permanent deformation. This means our floor model must also have a dissipation element.
A mass-spring-damper system.
That description resembles a lot to a mass-spring-damper system. As you might know those system tend to vibrate, unless they are overdamped. This means our floor model must behave as an overdamped mass-spring-damper system. The final condition is that the free falling object must be the mass of the system, this means that our floor model doesn't really has a mass parameter. Other way to think of it is that the mass is detachable, and this mass is only affected by the spring and the damper when is at or below the floor level.
Implementing the floor interaction
First, let's enable some sensor/actuator ports on the free falling ball. These two ports must be located at the same point, at the bottom of the ball.
Adding sensor/actuator ports to the free falling ball.
The floor control diagram can be implemented as follows:
Floor Control Diagram.
The body sensor is configured to output the position in meters, while the body actuator is configured to exert force in newtons over the ball. Using a demuxer, we extract the Y component of the position, then we compare it to the floor level. If the Y position of the bottom of the ball is below the floor level, then we activate the spring-damper system represented by the gains 'k' and 'c', otherwise the floor exerts no force over the ball. The force developed by the spring damper system is exerted over the ball but only in the Y direction, thanks to a muxer and the body actuator.
The final step is selecting 'k' and 'c'. You need to considerate the following things:
- The damping ratio "c / 2 / sqrt(m * k)" should be greater than 1, to guarantee overdamping.
- The free falling body will sink a distance "m * g / k" into the ground, so you might want to increase 'k', right?
- But Increasing 'k', also increases the computation load of the simulation.
How do I select the parameters 'k' and 'c'? I first select 'k' balancing how much sinking and computation load can I tolerate. Then I compute 'c' using the biggest mass and choosing a damping ratio of 1.
I'll leave you with the simulation output.
Floor control enabled.
Ball interacting with the floor. (Y position)
As a closing remark, I must say that I have used this method in the simulation of a humanoid robot with positive results.