Example 4: Falling Brick - ihmcrobotics/ihmc-open-robotics-software-tutorials GitHub Wiki

Introduction

In this tutorial we will create a simulation of a brick falling on a cluttered ground. You will gain more experience with specifying ground contact, terrain, and damping.

If you haven't done so already, visit Importing Simulation Construction Set Examples Project to set up the project in your development environment.

Understanding the Contents of the Project

Now let's browse the falling-brick project. There are three Java files here:

1. FallingBrickSimulation.java, which declares the simulation entry point. This sets up a Simulation Construction Set (SCS) instance to simulate the robot defined in the FallingBrickDefinition.java.
2. FallingBrickDefinition.java defines the model of the falling brick.
3. ClutteredGroundDefinition.java, defines the cluttered ground setup.

Let's take a deeper look at the contents of each file, starting with FallingBrickSimulation.java.

FallingBrickSimulation.java

This class has a few new SCS tools implemented that we didn't see in simple-pendulum:

• scs.addYoEntry(String varname): This method adds an entry box for the specified variable varname. Entry boxes appear at the bottom of the variable and registry section of the GUI.
• requestCameraRigidBodyTracking(String robotName, String rigidBodyName): The camera will move to keep the selected rigid body of the selected robot in focus.

For a more thorough explanation, I invite you to read through the code in the file. The code in each project of this tutorial has been notated heavily for user clarification, and to fully understand how the project works, it would be good practice to read through the provided class files before or after running the simulations.

FallingBrickDefinition.java

FallingBrickDefinition extends RobotDefinition and allows to define the properties of the falling brick.

In the constructor, there are some components that we have seen in the previous tutorial: creation of the robot joints and links (rigid bodies). The new components that we are going to focus on are the addition of ground contact points to the brick and the instantiation of the ground contact model.

Let's start by taking a look at the GroundContactPointDefinition class. In this example, this class is used to add ground contact points to the brick. It is an extension of ExternalWrenchPointDefinition, which is an extension of KinematicPointDefinition. Let's understand the purpose and use of each of these to fully understand was GroundContactPointDefinition does.

The creation of a point definition requires a name and an offset. The offset refers to the vector from the joint that the point will be attached to using the addKinematicPointDefinition(), addExternalWrenchPointDefinition(), or addGroundContactPointDefinition() methods of JointDefinition. A real robot model is generated from this definition and added to the simulation:

FallingBrickDefinition fallingBrick = new FallingBrickDefinition();
SimulationConstructionSet2 scs = new SimulationConstructionSet2(SimulationConstructionSet2.contactPointBasedPhysicsEngineFactory(contact));
scs.addRobot(fallingBrick);

This generated robot model will hold contact points according to the definitions: KinematicPoint, ExternalWrenchPoint, or GroundContactPoint. Such a contact point will move with the Joint in the same way that a link will move with the joint. The three types of points specified above are as follows:

• KinematicPoint: can monitor the cartesian position and velocity of a point on the robot. When a KinematicPoint is created, YoVariables for position (name+"X", name+"Y", name+"Z"), orientation (name+"Yaw", name+"Pitch", name+"Roll"), velocity (name+"LinearVelocityX", name+"LinearVelocityY", name+"LinearVelocityZ"), angular velocity (name+"AngularVelocityX", name+"AngularVelocityY", name+"AngularVelocityZ"), and offset (name+"OffsetX", name+"OffsetY", "name"+OffsetZ,"name"+OffsetYaw, name+"OffsetPitch", name+"OffsetRoll") are automatically created. These variables get updated whenever the robot is updated.
• ExternalWrenchPoint: an extension of KinematicPoint that can apply an external wrench to the monitored point. When an ExternalWrenchPoint is created, the YoVariables for force (name+"ForceX", name+"ForceY", name+"ForceX") and moment (name+"MomentX", name+"MomentY", name+"MomentZ") are created in addition. These variables can be set by the user to simulate a force acting on the robot at that point, or can be set by a ground contact model, or other object if the ExternalWrenchPoint is being used to model a contact or other event.
• GroundContactPoint: an extension of ExternalWrenchPoint that can model ground contact at the monitored point. A GroundContactPoint has additionally the YoVariables for touchdown pose (name+"TouchdownX", name+"TouchdownY", name+"TouchdownZ", name+"TouchdownQx", name+"TouchdownQy", name+"TouchdownQz", name+"TouchdownQs"). The touchdown pose refers to the position and orientation where the GroundContactPoint first contacts the ground. Furthmore, there is a boolean indicating if slip is detected (name+"IsSlipping") and a boolean indicating if the point is in contact/collision (name+"InContact"). Also the surface normal at the contact point is provided (name+"ContactNormalX", name+"ContactNormalY", name+"ContactNormalZ").

To create a new GroundContactPointDefinition, we use the following method:

GroundContactPointDefinition(String name, Vector3D offset)

where offset is the Vector3D from the parent joint. The forces applied to a GroundContactPoint are determined by the contact-point based physics engine to prevent penetration into the environment. The environment is set by adding a TerrainObject to the simulation

scs.addTerrainObject(new ClutteredGroundDefinition());

Notice that the class ClutteredGroundDefinition extends TerrainObjectDefinition.

ClutteredGroundDefinition.java

This class is responsible for defining the terrain. Different objects are added to the terrain. Each object consists of a GeometryDefinition (such as a box, a sphere, or the like), a VisualDefinition, and a CollisionShapeDefinition added to this geometry. The object's pose can be defined by passing a RigidBodyTransform to the constructor of the VisualDefinition.

RigidBodyTransform boxPose = new RigidBodyTransform();
boxPose.getRotation().setToPitchOrientation(Math.toRadians(45));
boxPose.getTranslation().set(new Vector3D(0.1, 0.2, 0.8));
GeometryDefinition tiltedBoxOnGround = new Box3DDefinition(0.2, 0.5, 0.80);
addVisualDefinition(new VisualDefinition(boxPose, tiltedBoxOnGround, new MaterialDefinition(ColorDefinitions.DarkBlue())));
addCollisionShapeDefinition(new CollisionShapeDefinition(boxPose, tiltedBoxOnGround));

Run the Simulation

Open FallingBrickSimulation.java. In the newly opened Java editor, right click anywhere on the code then in the context menu select Run As > Java Application. Soon enough, SCS should appear. Now press the Simulate button.

Experiment with the Simulation

Change the Velocity and Acceleration of the Brick

Try experimenting with the variables in SCS. For example, set qd_rootJoint_world_x and qd_rootJoint_world_z to 5.0. Click the Simulate button to observe the brick jump. Comment out the following line that sets the simulation in real-time: scs.setRealTimeRateSimulation(true);

Change the Stiffness of the Ground

The properties of all environment surfacse are defined in the ground contact parameters that are passed to the constructor for the SimulationConstructionSet2 when the simulation is created:

ContactPointBasedContactParameters contact = ContactPointBasedContactParameters.defaultParameters();
contact.setKxy(40000.0);
contact.setBxy(100.0);
contact.setKz(500.0);
contact.setBz(250.0);
SimulationConstructionSet2 scs = new SimulationConstructionSet2(SimulationConstructionSet2.contactPointBasedPhysicsEngineFactory(contact));

Try experimenting with the damping and spring constants. Note that as you lower the stiffnesses, greater penetration into the ground will occur. As you lower the damping constants, vibrations occur longer. However, if you increase the stiffness or damping constants too much, instabilities can occur due to numerical instabilities. As a general rule of thumb, ground stiffness and damping are tuned experimentally until an acceptable ground penetration and bounce is achieved. Try tuning the ground parameters for different ground penetration and bounce.

Change the Profile of the Terrain

Try adding more clutter to the ground or into the air.

When you're ready, continue learning with the Whole-Body-Controller-Core tutorial