This is the second of a two-part blog on creating virtual robot environments in Simscape. If you have not read part 1 yet, we encourage you to check it out here.

In the first half, we discussed importing your CAD into Simscape Multibody, and linking your parts together with joints. In this post we are going to be discussing adding physics, object interactions to your robot, and finally how to leverage Simulink program your robot. Enjoy!

Implementing Physics

Once you have verified that your main robot assembly simulates correctly, the next step is to implement the physics that will cause the robot to behave like it would in real life. The first step to do this is to create other objects for the simulation. For instance, the ability to use the wheel friction against the ground for moving requires we have a ground or floor. We did this by simply creating a very large rectangular floor with the ‘Brick Solid’ block.

Notice that the ‘Export Entire Geometry’ option is checked. We will use this exported geometry once we add the interactions between different environment components.

Adding Physics – Driving in the Ground

Gravity

Gravity affects all real-life robots, so naturally we need to make sure our simulation also accounts for it. After creating the ground, you need to directly attach this block to 3 blocks: Solver Configuration, World Frame, and Mechanism Configuration. These three blocks are essential for any Simscape Multibody model, and within the Mechanism Configuration block we can set the desired gravity for the simulation.

The Ground is attached directly to World Frame since we want it to be always static, while the robot is first attached to the ground using a 6-DOF joint. A 6-DOF joint allows the robot to translate and rotate in any direction relative to World Frame which is what we want since we want to make drive across the floor.

In order to make the robot move across the floor, we need to simulate both the normal force and the friction of the wheels when touching the floor. Luckily both these forces can be modeled with one block: the ‘Spatial Contact Force’ block.

Remember when we pointed out the check box for exporting the geometry of the ground block? This is where it’s going to come into play.

Adding Spatial Contact Forces

To model forces and movement, we need to introduce “geometry connections”. These connections are represented by dashed/doted lines. A ‘Spatial Contact Force’ block connects the geometry of two different parts to model the contact between them.

Simscape uses what is called a ‘convex hull representation’ to model the geometry of our imported CAD. This can lead to strange physical representation when working with parts that have a lot of features. Rather than worry about this, we decided to instead use a sphere to model the geometry of the wheel. This approach is called using a “proxy geometry” and can be very useful when dealing with complex geometries.

The sphere has the same radius and density as the wheel. After checking the ‘Export Entire Geometry’ box, a port labeled ‘G’ appears for the sphere. This connects to one side of a Spatial Contact Force Block, and the other side is connected to the ‘G’ port on the Ground block, which we made available earlier. Congrats! Now you have added a contact force!

Torque

After doing this for all four wheels, it is time add a driving torque to the wheels. This is essentially adding the rotational force that would come from motors in the real robot. The robot we are modeling is Front Wheel Drive, so we are going to add a torque input to both front wheels:

Open the revolute joint, expand the ‘Actuation’ settings, and switch the ‘Torque’ setting to ‘Provided by Input’, and the ‘Motion’ setting to ‘Automatically Computed’. This will create another input port labeled ‘t’. For testing, you can start with a constant input, and use a Simulink-PS Converter block to turn it to a physical signal. Note that you will have to use this block to set the signal units to N*m or your desired units. You can look at the files for this example to see how we implemented some of these connections

Damping

After setting a torque input you will need to set damping values for the wheels. Damping is the internal energy dissipation of the joint that resists changes in speed. Some damping is involved in every real-life contact collision and movements, and without it your robot’s motion would be erratic and unstable.

Tuning the damping values for each joint in the model was probably the hardest part of this process. If you are looking for a high-performance simulation just like we were for this example, then consider commenting out some of the robot’s subassemblies and incrementally adding more complexity to the model to keep track of when the simulation starts slowing down substantially. In general, here are my two main takeaways:

1. Add some damping to almost all joints.
2. Add significantly more damping to joints with input torque, or joints that experience a high load.

For example, the optimal damping values for the front wheels (which have an input torque) was 20 N*m/(deg/s), whereas for the rear wheels (no input torque) it was 1 N*m/(deg/s).

The damping values in the attached model should be a good starting point for robots of similar weight and dimensions. To tune the damping values, I recommend running the model with the Simulink Solver Profiler. The solver profiler will record events that substantially slow down simulations as ‘Solver Exceptions’. If a joint is causing a lot of solver exceptions, the damping value you have chosen can probably be adjusted by at least an order of magnitude.

Tuning Contact Parameters – Friction and Stiffness

After tuning your damping values, the last step to producing realistic driving in Simscape Multibody is to tune your friction and stiffness parameters for the ‘Spatial Contact Force’ blocks.

Here are my main takeaways from the friction/stiffness tuning:

1. If friction coefficients are too high, high forces can cause robots to literally “fly” off the screen.
2. If friction coefficients are too low, you can see the wheels slip while driving
3. If stiffness is too high, then gravity causes the robot to “bounce” on the ground.
4. Coefficient of static friction should always be larger than the coefficient of dynamic friction.
5. If you are feeling adventurous or having trouble finding good values, read this post on how to set these parameters for a more deterministic approach.

If you have followed the steps above, you should now have a robot that can drive around in a Simscape Environment! You can add Simulink Dashboard blocks to control the values of constant blocks that are driving actuated joints, this will help you test for correct robot movement using simulated remote controls.

Forces Between Robots and Objects

Now that you have a model that can drive, the final step is to give the robot a task. But in this case first we need to implement movement for the arm and the claw in order to enable object grasping.

The VEX V5 Clawbot motors are capable of both torque and position control. We decided that torque control would be the best choice for the wheel input, but that position control works better for the claw and arm input. This also gave us a chance to introduce you to a different actuation method for joints.

Arm Control

We will start with how to control the arm position.  Looking at the Clawbot model, we see that the motor is attached to a set of gears that drive the lower arm. Rather than worry about modeling the gear ratios in Simscape, we modeled the motor as being directly attached to the lower arm:

Since we are working with position control, we are going to change the actuation settings to that ‘Motion’ is set to ‘Provided by Input’ and ‘Torque’ is set to ‘Automatically Computed’.

Motion input for Simscape joints is slightly different that torque input, in that it requires the position, velocity, and acceleration all together in one physical signal. You can achieve this with a Simulink-PS converter block by using the options shown above. Also make sure to set the units to ‘radians’ or your preferred choice, and add some damping for the arm and claw joints in the same way as we outlined previously.

The ‘armPosition’ inport block you see here is attached to a Simulink Dashboard switch, which switches between the arm being horizontal (at 0-degree rotation), and down to the ground (37-degree rotation). However, if we switch directly from 0 to 37 or vice-versa, the calculated velocity for the Simscape physical signal will be infinity! This is going to cause an error in the model.  How do we fix this?

We use a Rate Limiter Block!

The rate limiter block limits the rising or falling rate of the signal to +/- 25 deg/s. This is then converted to radians by a gain block, and lastly connected to a ‘Zero-Order Hold’ block, which “holds” the input value at a constant number until the next block downstream needs it. In general, these are two useful Simulink blocks to keep in mind.

Claw Contact and Control

Before adding position input to the claw, we first need to prepare the forces between the claw and the cubes it is picking up.

For our robot, picking up a block with the claw can be achieved by adding forces between the claw and the block that can help fight gravity. We can do all of this with… you guessed it… a ‘Spatial Contact Force’ block. However, just like adding a sphere coincident with the wheels, we must figure out how to add “geometry connections” to the claw. We did this by adding three rectangles to each side of the claw to serve as our contact proxies (6 total).

In the Simscape Environment, the left half of the claw looks like this:

This might look complicated, but it’s simply a compilation of several other techniques we have used so far in this blog. Let’s break it down:

L1, L2, and L3 are 3 differently sized rectangles created using the ‘Brick Solid’ block. I used the CAD of the claw to measure out the thickness and length of each one. Once you have enough contact proxies to account for your desired contact points, you need to attach them to the pincher’s Center of Mass and place them in the correct locations. I estimated these locations based on the visuals.

Since each contact point has a separate geometry, they need their own Spatial Contact Force block. For both sides of the claw, that means there are 6 total force blocks, and they all are going to be connected to the target object. For your robot you can choose however many contact points are needed, in this case we wanted the ability to pick up round objects for future simulations.

The block labeled ‘PickUp’ is the cube that the Clawbot is being told to pick up in this virtual environment. The PickUp block is connected to the Claw2Block1 signal, which comes from the 6 Spatial Contact Force blocks in the claw.

There are two other force blocks (see the arrows above) connected to the cube. These force blocks link the cube to the ground and the ‘DropOff’ block, which is the cylinder the robot is trying to deliver the cube to. That means the cube has contact forces associated with 8 different objects in Simscape! In this case the object interacts with the floor, target cylinder, and 6 contact points in the pinchers. But depending on the simulation you are trying to build you can certainly have more or less of these contact blocks.

The cube is connected to the World Frame, Ground, and a Planar and Prismatic joint. Initially we used a 6-DOF joint just like for the Clawbot and Ground, but including the extra degree of freedom to account for the tilting of the cube adds complexity and therefore the simulation runs slower. The planar and prismatic joints allow us to pick up and move the block around which is great; the block simply cannot tilt forward or sideways for our simulation but depending on the simulation fidelity/realism you are looking you could change these to another 6-DOF joint.

And that brings us to the end of our model! If you followed along until this point you should have been able to create a simulation for your robot so it can pick up a cube and transport it to a matching target. In our case we made a model where we could drive the robot with some controls.

In the next section we will discuss 2 methods to help you make your robots autonomous!

Adding Autonomy – Designing Control Algorithms

So far, we’ve built a model for manual control of robots in a Simscape environment. There are two more models here which implement autonomous control. One assumes that the controller knows only the initial position of the cube and goal, while the other implements perception by actively tracking the locations of the cubes and goals.

In both instances, we used Stateflow to process the robot position and orientation, compare that with the target location, and calculate the necessary trajectories.

To illustrate the value of perception, I added some controls to move the target mid-simulation:

Do you want to learn more about designing a control algorithm for an autonomous robot? Check out our mobile robotics training video series. If you want to learn more about the Stateflow algorithm we developed for the Clawbot, check the complete files here and post questions in the comments!

Assess Performance of your Design/Control Algorithm

This virtual environment is a great method for assessing the performance of an autonomous control algorithm before a competition. Changing your code and re-running your model is considerably faster than working with the real hardware and you can evaluate the efficiency of different robot trajectories, changes to your mechanical design, and much more without taking your robot apart.

The biggest point of uncertainty in this exercise is that it is impossible to exactly model the physics of a robot in the real world. The damping, stiffness, and friction tuning we did here was mostly based on simulation speed and there could be additional disturbances. So, model as much as you think necessary and you can always adjust your simulations further with information from real world tests.

However, even without the most accurate model, simulations will add great value to your development process. Maybe your wheels have higher traction in real-life and the robot moves twice as fast? That’s fine, design closed loop controls using sensors, so your algorithm is not dependent on time, only position! For instance, we used our simulation to make sure our autonomous logic would withstand rapid changes to the target location. Additionally, you can always go back and adjust your simulation parameters if you want to refine your model and keep testing in parallel using simulations.

Thank you for following along with us in this journey, and please share with us your models and any questions you have!