Does anyone have a solution to using Motion Profile with Mecanum wheels?
Or does anyone have the Position Estimation kinematic equations for mecanum drive?
I have field-centric control working with Velocity Closed loop control. Now I would like to have some pre-canned flights of the robot that included translation and rotation. If something already exists, I would love to not have to derive the equations and build a path planner myself.
You probably want to look into using follower omnis for pos estimation, mecanum wheels have a really stong tendancy to slip (I mean they kinda have to when pushing their limits).
1986 2017 has a great example of this. After estimating x and y you could use a controller such as pure pursuit (or your other favorite of choice) to actually track that path.
Velocity control on the talons will come in handy for following a path and you will need a higher level controller if you want to do any kind of tracking from position estimation.
Depending on the amount of time and experience you have with closed loop control, there are a couple options that my team has explored. Since you already have closed loop velocity control, you might be interested in exploring a logistic model or cubic motion profile over different time intervals (logistic and cubic models drive your target velocity once differentiated). We run an independent function for each axis of motion (x,y, rotational (with a gyro)).
Last summer we did rough experimenting with an open loop logistic model, and a calibration sequence to find the velocity at a given output, with accuracy of ± 3". With a closed loop velocity control system, this could easily be reduced.
Regarding position, a simple algorithm can solve for position based on velocity. Since velocity is the rate of change of position, you can solve for the distanced traveled between each cycle of your robot code. If you find the x and y displacement between every cycle and add them all up you will get your total displacement. dx = ((Vi + Vf)/2 )*t
Another option that may interest you, is the PathWeaver class that is new with the 2019 WPI Libraries. For the 2020 season software documentation has been moved to https://docs.wpilib.org. Documentation for KOP items can still be found here. | FRC KOP Documentation
Logistic Model Graph: Motion Profiling | Desmos
Cubic Model Graph: Motion Profiling: cubic model | Desmos
Motion Profiling Project.pdf (491.5 KB)
The error from slip should be tolerable for my needs. With proper Acceleration and Jerk values, slip should conform with kinematic equations of the mecanum drive. I just would prefer to find proven equations instead of deriving them myself.
The only issue with PathWeaver is that it’s for differential drive robots. I need to create paths for a mecanum system that can translate in x&y and rotate all together.
Using the logistic or cubic models will do that for you. What we did is create 3 different profiles for x, y, and rotation. So if you wanted to go 10 ft forward and to the left, use the model to control your x component vector and y component vector. When you drive your robot with macanum drive, you are inputting x, y, and rotation components separately and they get added together to give you your velocity vector. The motion profiling models control each component separately (mimicking a joystick) to control the output velocity. To make complex curves, you just change the time interval in which they operate. Unfortunately, I do not have any clean, well documented code examples for this.
https://research.ijcaonline.org/volume113/number3/pxc3901586.pdf
Maybe this is the kind of thing your looking for?
In regards to path planning I would recommend just generating a central path using pathweaver or alike and then doing the vector math yourself to follow the path. You could achieve this by setting you wheelbase in pathfinder to something very small like .0001.
Our team is working on the same thing…
I found
And the above linked paper. I am still working on the actual pose estimation. I have the x and y velocity of the robot working decently.
Our code:
Still not sure why our pose estimation is wayy off. I think it has to do with the timestamp.
I’ll have to look at this when I get home from work. They block GitHub.
How off is way off and is it over or under? How clean is your rotational position signal? are you getting angle or do you have to integrate your gyro output directly?
I’ve been looking at this for a couple days. It’s all in velocity kinematics. From what I understand about Motion Profile streams, you need a position value in addition to everything else.
Right now, I am not using the angle generated from the kinematics at all. I’m just using the yaw from the gyro directly.
And when I mean off, I mean there is a bug in the code so it isn’t working at the moment 
I honestly have no idea where its coming from at the moment. If you look at it id appreciate a PM if you see anything off haha.
I see what your saying. And yes, the point model kinematics is the first step. The problem I’m running into is calculating the encoder positions for the 4 motors. The velocity control part I have a fairly strong grasp on. I guess I can just integrate the velocities since i have a fixed time interval…
If you paste your code for position estimation, I can look at it. Is this a mecanum drive system?
RigidTransform2D::Delta DrivebaseSubsystem::MecanumForwardKinematics(RigidTransform2D::Delta& flVelocity, RigidTransform2D::Delta& frVelocity, RigidTransform2D::Delta& blVelocity, RigidTransform2D::Delta& brVelocity) {
double xVelocity = (GetWheelSpeed("fl") + GetWheelSpeed("fr") + GetWheelSpeed("bl") + GetWheelSpeed("br") * (kWHEEL_DIAMETER / 2) / 4);
double yVelocity = (-GetWheelSpeed("fl") + GetWheelSpeed("fr") + GetWheelSpeed("bl") - GetWheelSpeed("br") * (kWHEEL_DIAMETER / 2) / 4);
double yawRate = (-GetWheelSpeed("fl") + GetWheelSpeed("fr") - GetWheelSpeed("bl") + GetWheelSpeed("br") * (kWHEEL_DIAMETER / 2) / (4 * (kWHEEL_BASE_LENGTH + kWHEEL_BASE_WIDTH)));
SmartDashboard::PutNumber("xVel", xVelocity);
SmartDashboard::PutNumber("yVel", yVelocity);
return RigidTransform2D::Delta::fromDelta(xVelocity, yVelocity, yawRate, flVelocity.GetDt());
}
As I pasted it, I realized Im not actually using any of the input parameters, so that might have something to do with it haha. Once I get access to a robot and make some changes I’ll update the thread.
are xVel and yVel off? Those look like the correct equations. The return line confuses me as to what you are doing.
Given the task of creating a motion profile, you should not need to have a position value. Instead of using your position as a reference value, you can use time as a reference. The FPGA in the roborio provides an very accurate reading of time, and match time is also available to you.
The problem with integrating velocity to find your position is that every integration you do will increase the noise of your data. The mecanum wheels will only amplify this effect.
If you do need to have position for another reason, the point kinematics is a solid and simple choice. If you check for velocity every time you robot code cycles, you are calculating a new position point several times a second. In a 2 and a half min. match, the error will be negligible.
Otherwise as mentioned earlier on this thread by CptJJ, follower omnis would work.
OK. Some updates. We seem to be calculating the wheel positions and velocities correctly. What is strange to me is that our x and y field positions are off by a factor of 3. I have no idea why at this point.
Here is our new forward kinematics equation:
What does RigidTransform2D::Delta::fromDelta(…) do?
It generates a “change in velocity” object. And we got it working. We had to add a division by PI somewhere. It doesn’t have horrible drift and we are on a slippery ground with sawdust all over it. I want to try it at higher speed.
Have you gotten this mecanum Path planner to run? I can’t get this to compile.