Some more help with the physics module

Technical Python
3

Thank you. That is what I tried first, but the encoder numbers do not seem to update. I must be doing something else wrong. Do you have any thoughts?

Here are my documents. They are a bit messy, but you can see where I tried to set it in physics.py (ll 45-49, then 71-72), and get it in robotpy (ll 67-74, and 94).

4

You have to compute and set them in your physics.py, they don’t update automagically.

1 Like
5

Thank you. I figured that. So, how do I compute them, the same way as previously, which seems like taking the speed from the wheel and deviding it by counts/revolution then passing that back to the encoder object? Or, some other way.

6

Yes, wheel speeds is what you need. https://robotpy.readthedocs.io/projects/pyfrc/en/stable/physics.html#pyfrc.physics.drivetrains.MecanumDrivetrain.wheelSpeeds

1 Like
7

Thank you. That is what I was looking for. I still need to work out the formula, but the method is…
self.drivetrain.wheelSpeeds.frontLeft
or whichever wheel I am pulling.

Edit…
Here is the relevant code in the physics.py utilizing the math from the old code. It seems to be accurate…

def __init__(self, physics_controller):
    self.frontLeftEncoder = hal.simulation.EncoderSim(0)
    self.encoderDistanceCalculation = (360 / (0.5 * math.pi)) #360 degrees per revolution, .5 for 6 inch wheel, pi because circles.
    self.frontLeftDistance = 0
def update_sim(self, now, tm_diff):
    self.frontLeftDistance += self.drivetrain.wheelSpeeds.frontLeft
    self.frontLeftEncoder.setCount (int(self.encoderDistanceCalculation * self.leftEncoderDistance ))
8

Now that I understand odometry and kinematics a bit more, I thought I would take a look at trajectories. I read through the WPILIB tutorial and I have a few more questions.

  1. Non-sim related. Is it appropriate or wise to use trajectories with the timed robot framework?
    I would like to stay in this framework for now as it is simpler for the team to understand.
    Edit: I found the answer to this over here

  2. The tutorial uses voltage compensation for the feedforward gains and the process variable in the controller. Is it difficult to mimic these in the simulator? In absence of this, is it appropriate to use the wheel speed instead (again, merely for the sim)?

~Mr. R^2

9
  1. RobotPy does not support CommandsV2 yet, so TimedRobot is fine
  2. As I understand it, the motivation for using voltage instead of speed is to provide a consistent speed to the wheels (which is a generally difficult problem in real life because of friction, battery voltage, and tons of physical constraints). In simulation we actually have the opposite problem – it’s difficult to make the wheels not have a consistent speed.
    So… play with both, and see what feels right to you. My intuition says that since voltage is just a proxy for speed, you should be able to use voltage in the simulation.
1 Like
10

Thanks that helps. I am learning a lot here. I love how well the docs are written (both the WPilib tutorials and the robotpy docs). They are helping me understand how to implement these concepts that for years I understood conceptually, but was having trouble implementing myself let alone explain tot the students.

That is not to say I will be able to do this without asking more questions (because I probably won’t), so thank you for your direct support as well. We are forever grateful for robotpy and the support of you and the entire team.

11

I thought all was working okay until I tried to actually use the simulated encoder values in the Odometry to get the pose, and I realized it was not updating. So, I started to look at the values for Encoder.getRate(), and I do not think they are accurate.

The values are similar to… 2.60921748755193e-310 Also, when the robot stops, the method still returns the last number (I would assume it would be 0).

So, I went back to the drawing board looking just at the encoder. I used the tankdrive model.
Here is the code I am working with. Am I doing something wrong, or is this the expected output of encoder.getRate()?

12

I believe you would need to compute the rate yourself and … call setPeriod. Looks like the implementation is:

encoder->distancePerPulse / SimEncoderData[encoder->index].period

Probably needs a helper function for this, filed a bug at https://github.com/wpilibsuite/allwpilib/issues/2453

1 Like
13

Thank you. I was close, I was just pulling from the wrong point (I was trying to set it in robot.py).
So, I tried to implement it in Physics.py and if I am interpreting it correctly (which I am not),
Should I do something like this?
self.LeftEncoder.setPeriod(((1/360)*6*math.pi) / self.LeftEncoder.getPeriod())
when I do this, the data bounces between

1.6104334333141489e+308
2.60921748755193e-310

Yet, when I try… self.LeftEncoder.setPeriod(((1/360)*6*math.pi) / SimEncoderData[0].period
I receive the following error…
NameError: name 'SimEncoderData' is not defined
I assumed that since self.LeftEncoder was an instance of SimEncoderData that I self.LeftEncoder.getPeriod() would work, but I think I am still off.

14

I pasted in the C++ API implementation, so your call to setPeriod would set the value of SimEncoderData[encoder->index].period (which is some internal C++ object not exposed in the API). Your rate would be whatever distancePerPulse is (there’s probably an API for that) divided by whatever you give to setPeriod.

1 Like
15

Reference: https://github.com/wpilibsuite/allwpilib/blob/1c28b729ad1ce54716416cc39c94c20ffcc80949/hal/src/main/native/sim/Encoder.cpp#L239

1 Like
16

Thank you. So, could I use something as simple as my self.leftEncoder.getDistancePerPulse()/tm_diff?
Also, I am still noticing that when I do that and call self.leftEncoder.getRate() the returned value keeps increasing even when the robot is stopped. Is that supposed to happen?

17

I may have made some progress. I got much more stable results simply by setting self.leftEncoder.getPeriod(1) (which after reading the WPILib Docs again seems to be 1 second) in the physics.py module. I am not certain how accurate they are, but I get a float value out of self.leftEncoder.getRate() and it stops when the robot stops.

A follow-up question as far as the physics module goes. Is there a way to set the scale or size of the field (or at least see it)? I know the javascript files are no longer read by the physics engine.

18

I believe the size is in the settings dialog for it.

1 Like
19

Yep, I cannot believe I did not see that. Thanks again.

20

Thank you all again for your help.
I tried a lot of things, and could not get the encoder.getRate() to work, so I ended up doing this in robot.py.

    def autonomousInit(self):
        """Called when autonomous mode is enabled"""

        self.timer = wpilib.Timer()
        self.timer.start()
        self.oldTime=0.0
        self.oldDistance=0.0

    def autonomousPeriodic(self):
        newTime = self.timer.get()
        newDistance = self.leftEncoder.getDistance()
        speed = (newDistance-self.oldDistance)/(newTime-self.oldTime)
        self.oldDistance = newDistance
        self.oldTime=newTime

I am certain I am messing something up, but this works and will get us through.
On our real robot, we have Neo Drive motors anyway, and so the rev.CANEncoder.getVelocity will be a bit easier.
This could easily be its own method now that I think about it.

21

From what I understand you ended up getting everything working but if you wanted another reference I’ve been working on trajectory tracking in the python simulation and have it pretty much fully functional. The code is very messy and I still have some more commits I need to make but this should work.

1 Like
22

Thank you. I just finished getting Odometry working (with a basic Proportion FeedForward). I am going to begin actually getting trajectories tomorrow.
I will post back when finished or if I have more questions.
Edit: I just downloaded the code and ran it. It worked beautifully, thank you. I love the Utils. They solve many of the issues I have been having. Do you mind if I borrow them for our project?