Kaya robot uses cmd_vel of ROS to control the left front movement error. Has anyone encountered the same problem? Can you help me solve it?

@mgussert Hey, this is my newly created thread, below is my USD, you can try to make kaya go forward or backward, after a while, kaya will deviate. I would like to know how to fix this bug, thank you very much for your help

kaya_ogn_gamepad.zip (11.8 KB)

Hello again 1365351984!

I was able to open your stage and there is a bigger problem I think we need to look at first :) and that is the version of Isaac Sim you’re using. There was a reference on the stage to the kaya asset with a path on the local host to a version of Isaac sim from 2022. It might be an old asset folder, but just in case, please make sure you are using the most recent release of Isaac Sim! This should be 2023.1.1 at the time of this writing.

Another hint that there might be a version issue is the graph in the file

It looks like there are several nodes missing, which means they were probably depreciated, refactored, or replaced.

My suggestion would be to update Isaac-Sim, get things working again, and post here. I think I see a similar deviation when I work with the Jetbot, but for me this is coming from the control mode (effort vs velocity). I see deviation when using effort controls but I think this comes from the additional numerical integration step that comes as a consequence. Velocity controls appears to make the robot drive qualitatively straight (i haven’t measured it)

Looking forward to your reply! hope this helps!

Gus

Thank you very much for your timely reply. I created one again using the 2023.1.1 version, but the same problem still occurs. What do you mean by using speed control? How can I change to using speed control? Sorry, I don’t know much about this aspect.Below is the USD file I recreated. @mgussert

kaya_test.zip (13.3 KB)

kaya_test_simplified.zip (694.1 KB)

I simplified the graph for testing, removing the ros2 nodes and replacing the command with a constant vector. You are already using velocity commands (the values coming out of the holonomic controller) so that’s not the issue.

Tl,Dr; What you have found is not actually a bug, but one of the many joyful consequences of open loop control and numerical optimization! Removing these deviations means “closing” the control loop by measuring / computing some correction to the deviation and applying the relevant actions.

# Details

In the test stage attached, you can explicitly command the robot by modifying the double3 being used as an input to the “velocity command” of the controller. If the robot starts at (0,0,0) and I command it with a velocity of(v,0,0), then after a time we should see it at some position (vt,0,0) but instead we see some tiny value in the Y position as well! If we command (0, v, 0) we instead see deviation in the X position. I assume this is the deviation you note in your question.

Why does this happen?

Commanding the robot to move with some velocity means figuring out how quickly each wheel needs to turn such that the total resulting force applied to the robot sums to the desired command velocity. We can go through all the mechanics and write down Newton’s second law for the system

**F**net = m **a**

Where **F**net is the sum of all the forces from the contacts between the wheels and the ground, while **a** is the acceleration of the center of mass of the robot. Up to a scale factor, you can think of this as a bunch of little arrows (the net force) adding up to one big arrow (the acceleration).

We can rephrase this problem by moving everything to one side of the equation

**F**net - m **a** = **0**

Let’s rephrase this equation slightly. We want to know what forces to apply given a command, or more explicitly, a desired acceleration. The total force being applied to the robot at any given time is unlikely to be aligned with the command, and so this difference wont be zero most of the time.

The difference between the current total force acting on the robot and the necessary total force to follow the command is called the Residual, **R**.

**F**net - m **a**_command = **R**

Our problem is now an optimization problem! We need simply and systematically choose the forces that sum to **F**net such that **R** is **0**. Better still, because we only care about the magnitude of **R** we can equivalently minimize the following instead

(**F**net - m **a**_command)^2 = R^2

And this is a well understood problem known as Least Squares Minimization. In the case of a rigid robot, where the position of the wheels are fixed, we can rephrase the problem yet again into a Quadratic Program, which allows us to guarantee specific convergence properties and improve performance. We make use of the Operator Splitting Quadratic Program detailed here and for a specific example of least squares, go here. These are just optimization details however. There’s nothing that says you couldn’t also train a neural network via gradient descent to do the same, for example.

The point here is there’s an uncertainty to this numerical optimization step. How “close” is “exact”? Those uncertainties contribute to the final total deviation, but they are only one part of the story! There are additional terms in this step explicitly to ensure that the total applied joint efforts to the wheels are normalized, which adds a structural factor to the resulting solution.

But it goes even deeper! The way that you command the robot matters! This is what I was referring to velocity vs force commands. If the joint drives receive target forces, there is an additional integration step that needs to occur outside of the controller! In fact there are many MANY parts of the physics simulation that require additional optimization steps between the individual ticks of the simulation in order to compute the next step.

all of these errors compound together to result in the deviation you noted. In reality, we don’t have access to the “ground truth” of the “simulation” and so control theory has developed around the idea of controlling with and without feedback (closed loop control vs open loop control). If you absolutely needed the robots velocity to match the command velocity at all times, you could close this loop with ground truth measures in the sim, but that’s a whole other project.

It should be noted that a real robot would also not move in perfect alignment, simply because the controller would not be perfect due to the nature of the solver alone, but further still, even with perfect commands and a perfect solver, we cannot expect perfect results out of the simulation. We can guarantee that the sim is deterministic (that you’ll get the same result every time) but we cannot guarantee that the sim is perfect (that it will exactly match reality) as this is beyond the speed of light.

Hope this helps!