I’m training a robot in Isaac Gym using the legged_gym code base. In this video you can see that at a standstill (target angles all = 0) with gravity on but base fixed in the air, the joints are all idle and close to their target position. However, their velocities are completely erroneous.
This makes it 1) impossible to use DOF_DRIVE_MODE effort with a PD controller where d_gains are not zero and 2) impossible to use dof_vel in reward functions.
Things I tried:
Tuned gains many many many times
Read through replies to Inaccuracy of DOF state readings – tried all combinations of simulation-device // pipeline-device (CPU // CPU, CPU // GPU, GPU // CPU, GPU // GPU) and the bug persisted in all of them.
Finite differencing: approximating dof_vel as (self.last_dof_vel - self.dof_vel) / self.dt. On average the approximation is better – for example, here are some comparisons for one of the legs:
Joint
Avg. Vel. Approx. (rad/s)
Avg. Vel. Reported by DOF State (rad/s)
body to leg
-7.386e-08
-1.239e-06
leg link 1 to link 2
-0.213
0.774
leg link 2 to link 3
-0.655
2.618
However, this is still not good enough to use for effort drive mode or reward functions
Read ‘Damping’ and ‘Stiffness’ parameters effect on DRIVE_MODE_EFFORT – learned that for DOF_MODE_POS and DOF_MODE_VEL there is a built-in PD controller that tracks target positions or target velocities using the stiffness and damping of the DOF set through gym.set_actor_dof_properties as p-gains and d-gains respectively.
In legged_gym examples DOF_DRIVE_MODE is effort. There is no built-in PD controller for this drive mode so they explicitly write out their own PD controller in compute_torques and never set stiffness or damping when they call gym.set_actor_dof_properties.
Damping and friction of a DOF can also be read from the tag of the URDF file, but all of the examples have URDFs with no damping or friction in their tags – it works without tuning any of this
Switched to DOF_MODE_POS – actually setting stiffness and damping through gym.set_actor_dof_properties
At this point, when I turn off base_link fixed and allow the robot to fall to the ground and make contact with the plane, I can get to something like this:
Joint
Target Angle
Actual Angle
Reported Velocity
Actual Velocity
body to leg
0
0.0032
-0.0001
0
leg link 1 to link 2
0
0.2765
-0.1445
0
leg link 2 to link 3
0
0.0595
-0.1
0
What else can I try to eliminate this velocity issue? I can only really tell how bad it is when the target velocity is 0, but when the joints are moving I’m sure it may be even worse.
I didn’t get your question completely, why velocity should be zero? at what point? there are always settling time, overshoot, offset and …with any controller. when the robot is floating the gravity and controller working on the legs and make it oscillates, but it should converge to a steady state after a few seconds. same for the time the robot lands on the ground.
While in motion the velocity should be nonzero (when settling, or during oscillations). But in my example, whether on the ground or floating in the air, the steady state it converges to is nonzero while the joints are clearly at a standstill.
There’s a bump at the beginning as everything settles and the robot falls to the ground plane. The joint position converges to some steady state and does not change but you can see the joint velocity continues to measure around -0.2.
That’s for joint 2 of 3 for one leg, you can see for joint 3 of 3 it’s even more pronounced, velocity converging to around -0.25
I’m using the legged_gym code base. We get dof_state_tensors off the GPU at initialization and refresh them at each time step with gym.refresh_dof_state_tensor
Like this?
self.dof_state = gymtorch.wrap_tensor(dof_state_tensor)
self.dof_pos = self.dof_state.view(self.num_envs, self.num_dof, 2)[…, 0]
self.dof_vel = self.dof_state.view(self.num_envs, self.num_dof, 2)[…, 1]
I see, so you mean when working on GPU the derivative of pos is not equal to the velocity?
It’s persistent no matter what device I use for sim_device and pipeline – CPU // CPU, CPU // GPU, GPU // CPU, GPU // GPU. Here is the bug with CPU sim_device and CPU pipeline:
I assume that you are using the TGS solver. If that’s the case, what you are seeing is not a bug. This has already been explained in the previous thread that you linked to. What is happening behind the scenes is that each iteration is effectively a sub-step of the simulation, where the constraints are re-projected, solved and then the bodies are integrated. This approach leads to a much faster converging iterative solver than taking a single large step and iterating over constraints N times. However, it does have the artefact that the returned joint velocities are no longer the same as the change in joint velocity/dt. Instead, they are the velocities that were generated in the final time-step of size dt/N by the constraint solver. It’s highly likely that, if we were to chart the position and velocity values over all iterations, that you would see a set of values that are oscillating at an extremely high frequency.
We can see from your joint position graph that it’s converging on a steady state positional error, but that the joint velocities are not zero - instead, they are a small value that reflects the corrective forces applied by the solver in the final sub-step to get to/maintain the drive states in the face of external forces like gravity, contacts etc.
If the velocities being reported from the simulation (which are the true current joint velocities) are not sufficient for your controller’s requirements, you can of course compute a new set of joint velocities by finite differencing the joint positions. This is the set of values that you are expecting. Unfortunately, they would not be an accurate reflection of the internal state of the sim - they would just be a smoothed approximation based on sampling the joint positions. The actual velocity values being reported by the solver are an exact snapshot of the simulation at the end of the frame. It’s not a weird artefact or a bug - the simulation is genuinely applying the forces from the solver at each sub-stepping and this is the resulting state that is produced.
You could produce the same effect if you ran the simulation with a single solver iteration at a time-step of dt/N and sampled joint position and joint velocity every N steps. You should see that the rate of change of the joint position over those N steps is not the same as the joint velocity on the Nth step because the joints errors are oscillating around the steady state value.
Thanks for the explanation, so I’m wondering why I got very good results for velocity and Maxime didn’t, as you can see in my velocity graph it’s zero, is it because of smaller time steps? or lower number of joints( 1 in my case)? or not using tensors? In my test I put the number of sub steps to 1 and dt=0.001.
Hear “that the returned joint velocities are no longer the same as the change in joint velocity/dt.” I think you meant “joint position/dt”
It’s likely due to the lower number of joints and lack of any external constraints - it’s basically a simple case where a single solver iteration already gets the exact required answer.
When we have cases like a balancing quadruped, there are multiple contacts and joints involved, so a single solver iteration will be unlikely to reach a global solution for this system. Therefore, we end up in a state where the solver is continuously revising the forces being applied by the contacts and joints in a feedback loop. As these forces propagate through the system and the system is integrated, small positional or velocity errors accumulating, which result in additional forces being applied by the constraints next iteration to correct them.
If the solver is given more iterations, it should result in less error both in terms of positional and velocity error.
Thanks, very helpful, one last thing, does the drive mode has any effect on this? how about friction and damping amount? I’m guessing higher Kp or stiffness makes it worse, and higher friction and damping should make it better.
Right, higher friction/damping should drive the velocity closer to zero and will reduce oscillation, at the expense of increasing the positional error that a steady state is reached.
