Data -.inf drive_module_steering_angle_controller

[peterheim1]

Hi
very impressed with your work
I tried the zinger_swerve_controller for my swerve robot but when you stop the robot the
drive_module_steering_angle_controller/commands data shows

layout:
  dim: []
  data_offset: 0
data:
- -.inf
- -.inf
- -.inf
- -.inf

if /cmd_vel is not 0 then no inf
trying to convert them to a INT I get
OverflowError: cannot convert float infinity to integer

[pvandervelde]

That sounds like I have created some subtle bugs. Do you have a reproduction case. Does it always happen when you stop the robot?
I can try to reproduce in the next few days or so, I'm currently kinda busy with a new job and home renovations.

[peterheim1]

Yes
when i send a /cmd_vel with all zeros i get the error if i add a very small value to X like 2.5e-07(0.000001) the error goes away and steering data is 0.0 lower than that number the error comes back
I just finished kitchen renovation i feel for you

[Togn3K]

Hello!
I noticed the same issue and atm I fixed it by avoiding publishing the message if there are some inf values: Togn3K@2829a94#diff-4c0e67df744088b1220b5634ff45b5cc3e49a55d97823b5363b27e0379a6d85fR573

I made a PR for this fix and another fix regarding the TF: #2

[peterheim1]

hi I just tried the Togn3k version but i noticed once the vel and steering stop publishing sending a /cmv_vel all 0 dose not reset the angle or reduce the last vel. changing the angular vel alone will change the linear vel but the angular vel will not change (no messages published )and with a small linear vel(0.00001) angular vel works as expected

Peter

[peterheim1]

after digging in the code i found this in control_model.py at line 231
reverse_steering_angle = float("-infinity")
I changed -infinity to 0.0
and the error went away
or did i break something else??

[pvandervelde]

So if I remember correctly the float("infinity") here and the float("-infinity") here when the cmd_vel is set to zero is because at zero we don't know what the steering angle should be, essentially any steering angle is valid. If we set the steering angle to 0 then when we come to a stop from a linear movement at, say 45 degrees (crab motion), then in the last moment in time the steering angle changes from 45 degrees to 0 degrees. Given that we're dealing with floating point numbers we can't really use any valid number to say "I don't know what the steering angle should be, pick one", so I use infinity ...

I think(??) there's some code somewhere that if the steering angle is infinity it should just keep the angle at what ever it was, but there might well be a bug there.

[peterheim1]

I have been playing with it all day now and it seems to work fine with and i like the way when it stops the wheels are a zero
where do i find what order the wheel angle and velocity are published linear motion works well but rotation is a bit jerky

[pvandervelde]

What do you mean with the order the wheel angle and velocity are published?

[peterheim1]

never mind i solved it
i was look at the wheel angle publish order ie front left, front right, rear left, rear right
but it now drive around i just have to get odometery working

[Togn3K]

you're right @peterheim1 : ignoring the inf values cause the robot to not correctly stop when i send 0.0
at the moment I substitute the "inf" with 0.0 in control_model like you suggested
i searched for a method to keep the last value but i cant find any way to implement easily, the only referenced to that is the comment :

zinger_swerve_controller/zinger_swerve_controller/control_model.py

Line 199 in dae8dd9

# In either case we just keep the position of the wheel where it was

but it seems to not do any check later neither to have access to the last value inside that method, so it need some work to actually implement in that point of the code

[pvandervelde]

There's a check in one of the pieces of code, but not in all of them (doh). I should really update the controller code with the code I have from https://github.com/pvandervelde/basic-swerve-sim. It's got a controller that should be an improvement.

[peterheim1]

I have a strange problem
When 1 start to drive forward for about 1 to 2 seconds the steering angles fluctuates with no rotation input. its worse with a slow acceleration (like with a joystick).

Rotate on the spot also causes a lot of fluctuations for the first 1 to 2 seconds with the slower acceleration if I publish a angle or velocity from the topic publisher in RQT it works

if I mix X and Z values in normal driving the wheel angles will fluctuate some time as much as 100deg but only 1 wheel
I change my joystick to control the X and Z on separate joy sticks to avoid cross selection

When running navigation every thing works until the wheels fluctuates then the robot gets lost
this shows the the transition from ) to 0.08 m/s i have just scaled you vel out put to match my motor controller
and converted radians to degrees

[omni_controller.py-9] [INFO] [1718146029.418808249] [omni_controller]: m 0 0 0 0 2 2 2 2 [omni_controller.py-9] [INFO] [1718146029.438790053] [omni_controller]: m 2 -2 -2 2 2 2 2 2 [omni_controller.py-9] [INFO] [1718146029.458785634] [omni_controller]: m 7 -6 -9 10 2 2 1 1 [omni_controller.py-9] [INFO] [1718146029.478786693] [omni_controller]: m 6 -5 -7 8 3 3 2 2 [omni_controller.py-9] [INFO] [1718146029.498762286] [omni_controller]: m 10 -7 -10 15 3 3 2 2 [omni_controller.py-9] [INFO] [1718146029.518797848] [omni_controller]: m 9 -6 -8 12 3 3 2 2 [omni_controller.py-9] [INFO] [1718146029.539112873] [omni_controller]: m 5 -5 -7 6 4 4 3 3 [omni_controller.py-9] [INFO] [1718146029.558924913] [omni_controller]: m 3 -3 -4 3 3 3 2 2 [omni_controller.py-9] [INFO] [1718146029.578833827] [omni_controller]: m 8 -11 -17 13 4 4 4 4 [omni_controller.py-9] [INFO] [1718146029.599060302] [omni_controller]: m 6 -7 -10 8 5 5 3 3 [omni_controller.py-9] [INFO] [1718146029.618878218] [omni_controller]: m 5 -10 -14 7 8 8 6 6 [omni_controller.py-9] [INFO] [1718146029.638893436] [omni_controller]: m 4 -8 -10 5 7 7 5 5 [omni_controller.py-9] [INFO] [1718146029.659045024] [omni_controller]: m 5 -9 -12 6 9 10 7 7 [omni_controller.py-9] [INFO] [1718146029.678927454] [omni_controller]: m 4 -7 -10 5 9 9 6 6 [omni_controller.py-9] [INFO] [1718146029.698700723] [omni_controller]: m 6 -8 -12 8 10 10 8 8 [omni_controller.py-9] [INFO] [1718146029.718846020] [omni_controller]: m 5 -7 -9 7 9 10 7 7 [omni_controller.py-9] [INFO] [1718146029.738820022] [omni_controller]: m 4 -5 -7 5 11 11 9 9 [omni_controller.py-9] [INFO] [1718146029.758872440] [omni_controller]: m 3 -4 -5 4 14 14 11 11 [omni_controller.py-9] [INFO] [1718146029.778849945] [omni_controller]: m 3 -4 -4 3 17 17 14 14 [omni_controller.py-9] [INFO] [1718146029.798808097] [omni_controller]: m 2 -3 -4 2 19 19 17 17 [omni_controller.py-9] [INFO] [1718146029.818686839] [omni_controller]: m 2 -3 -3 2 22 22 20 20 [omni_controller.py-9] [INFO] [1718146029.838864320] [omni_controller]: m 2 -2 -3 2 25 25 22 22 [omni_controller.py-9] [INFO] [1718146029.858892775] [omni_controller]: m 1 -2 -2 1 27 27 25 25 [omni_controller.py-9] [INFO] [1718146029.878849602] [omni_controller]: m 1 -2 -2 1 30 30 28 28 [omni_controller.py-9] [INFO] [1718146029.898796200] [omni_controller]: m 1 -1 -2 1 33 33 31 31 [omni_controller.py-9] [INFO] [1718146029.918893663] [omni_controller]: m 1 -1 -1 1 36 36 33 33 [omni_controller.py-9] [INFO] [1718146029.938940863] [omni_controller]: m 1 -1 -1 1 38 38 36 36 [omni_controller.py-9] [INFO] [1718146029.958830504] [omni_controller]: m 1 -1 -1 1 41 41 39 39 [omni_controller.py-9] [INFO] [1718146029.978827041] [omni_controller]: m 1 -1 -1 1 44 44 42 42 [omni_controller.py-9] [INFO] [1718146029.998815721] [omni_controller]: m 0 -1 -1 0 46 46 44 44 [omni_controller.py-9] [INFO] [1718146030.018848272] [omni_controller]: m 0 -1 -1 0 49 49 47 47 [omni_controller.py-9] [INFO] [1718146030.038831158] [omni_controller]: m 0 -1 -1 0 52 52 50 50 [omni_controller.py-9] [INFO] [1718146030.058805581] [omni_controller]: m 0 0 -1 0 54 54 53 53 [omni_controller.py-9] [INFO] [1718146030.078809937] [omni_controller]: m 0 0 0 0 57 57 55 55 [omni_controller.py-9] [INFO] [1718146030.098849749] [omni_controller]: m 0 0 0 0 60 60 58 58

[pvandervelde]

Eeehhh that's not ideal. Do you have a reproduction case anywhere? Any code that I can try to run?

[peterheim1]

On my real robot that publishes joint_states at 50 hz it will happen all the time but it gets worse as time goes on.
I can see a flash of steering movement if i publish a blank joint_states at 50hz and use RQT robot steering to drive forward and backwards with the slider about .3m/s of between positive and negative values

my joint_state publisher listens to steering and velocity topic pass that info to the MCU on the robot steering is looped back through to joint_states and velocity is taken from the robot actual wheel velocity
if i publish at less than 50 hz the result is less but the odometery is not reliable

[peterheim1]

here is a link to my joint_state publisher omni_controller

[pvandervelde]

So is your omni_controller using my libraries? Or are you only seeing this with my code but not with yours? Are you seeing this in a simulation or only on the real robot?

I just tried running Zinger in Ignition:

• Using Nav I do see some fluctuations. The ones I'm seeing are pretty small relative to the orientation change of the wheels so they could be due to the fact that the ros2 control construct I'm using only does linear changes, i.e. I give it a new value to be at every x time and the JointGroupPositionController and JointGroupVelocityController instantly set to that position and velocity.
• Using a cmd_vel message I don't see any fluctuations

[peterheim1]

I'm using a real robot. my controller only reads the steering angle and velocity commands and publishes a joint_state message commands to the MCU(teensy 3.5)are done through serial. the steering angles are passed straight back to joint states and the velocity is the actual wheel velocity. the steering is done with RC servos so i expect a bit of error. The robot moves as expected ie 1 meter forward is reflected in odom and a 90 degrees turn as well
I only see this error when joint_states is published. a blank joint_state only produces a small error. but a full message with angles and velocities always produces the steering jitter. the strange thing is after about 20 to 30 min of driving the steering angle is totally out of wack and no more wheel angles or velocity are published. after a few mins its fine.
if it was controller then the angles would still be published. I really don't understand how the joint_state affect the main program??

[pvandervelde]

The controller code subscribes to the joint_states here:

zinger_swerve_controller/zinger_swerve_controller/swerve_controller.py

Line 156 in 2b4facd

joint_state_topic = "joint_states"

And then uses them here:

zinger_swerve_controller/zinger_swerve_controller/swerve_controller.py

Line 392 in 2b4facd

def joint_states_callback(self, msg: JointState):

At the end of that function it notifies the steering_controller that there is a new set of drive module states, which then get translated into a new body state. The body state is heavily used when calculating the next values for the drive modules. Especially if it uses one of the different control profiles.

If I would have to take a guess (and it's very much a guess without any data to look at) this might be caused by the code restarting the control profiles each time, or it is trying to correct for the natural jitters that are caused by real life physics.

[peterheim1]

so if the joint_state message where to be corrupted would that result in strange wheel angles ie 10r 2 wheels turning by a large amount?

[pvandervelde]

That is my current hypothesis but I have no proof. Is there anyway we can grab some data? Can we grab a ROS bag with the steering inputs and the joint states. I would imagine that if we send the joint states through a simulation then we should see something?

[peterheim1]

here is a link to some bags i recorded today ros bags
let me know if you need more

[pvandervelde]

Sweet thanks. I just grabbed them and had a look at your first dataset. It's very jittery :( It's going to take me a bit to figure out what's going on to be honest.