Data -.inf drive_module_steering_angle_controller about zinger_swerve_controller HOT 22 OPEN

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??

from zinger_swerve_controller.

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

from zinger_swerve_controller.

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??

from zinger_swerve_controller.

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.

from zinger_swerve_controller.

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

from zinger_swerve_controller.

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

from zinger_swerve_controller.

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

from zinger_swerve_controller.

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.

from zinger_swerve_controller.

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

from zinger_swerve_controller.

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

from zinger_swerve_controller.

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

from zinger_swerve_controller.

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 :

from zinger_swerve_controller.

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.

from zinger_swerve_controller.

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

from zinger_swerve_controller.

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

from zinger_swerve_controller.

here is a link to my joint_state publisher omni_controller

from zinger_swerve_controller.

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

from zinger_swerve_controller.

The controller code subscribes to the joint_states here:

And then uses them here:

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.

from zinger_swerve_controller.

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?

from zinger_swerve_controller.

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?

from zinger_swerve_controller.

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

from zinger_swerve_controller.

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.

from zinger_swerve_controller.