You still haven't posted the (full) code that gives the results you've presented; when I run your snippet I don't the results you posted. Instead, I get:
[xdfb] = FDfb(sphere, xfb, [], [], [])
xdfb =
0
-2.5000
0
0
0
-1.0000
-1.0000
0
0
0
0
5.0000
-14.8066
Here xdfb
is the time-derivative of your input system sphere
and the initial conditions xfb
. It's a 13-element vector made up of the derivative of the things that comprise xfb
, which means:
- You have a quaternion derivative,
0
-2.5000
0
0
- You have a Cartesian velocity, and
0
-1.0000
-1.0000
- You have a spatial acceleration.
0
0
0
0
5.0000
-14.8066
Note that here the spatial acceleration I have is not what you've presented in your question, so I'm not positive that we're doing the same thing.
You can use constant-acceleration equations to calculate future positions. For your y-values, there's no acceleration, so:
$$
y = y_0 + v_y \Delta t + \frac{1}{2} a_y \Delta t^2 \\
$$
becomes
$$
y = y_0 + v_y \Delta t \\
$$
For your starting position of $y_0 = 0$ and starting speed of $v_y = -1$, after 30 seconds you should have:
$$
y = 0 + (-1)*30 \\
$$
for the trivial result of $y = -30$.
For z, you have a constant acceleration of gravity and a starting speed of -1, so you should have:
$$
z = 0 + (-1)*30 + (0.5)*(-9.8066)*(30^2) \\
$$
for a result of $z = -4,443$.
If I run the following after running your code:
t0 = 0;
tMax = 30;
dT = 0.001;
time = t0:dT:tMax;
nSamples = numel(time);
pos = zeros(3,nSamples);
for i=1:nSamples
% Forward Dynamic
[xdfb] = FDfb(sphere, xfb, [], [], []);
q = xfb(1:4);
dq = xdfb(1:4);
r = xfb(5:7);
v = xdfb(5:7);
v_old = xfb(8:13);
a = xdfb(8:13);
q = q + dq*dT;
q = q./(norm(q)); % re-normalize the unit quaternion
r = r + v*dT;
v = v_old + a*dT;
pos(:,i) = r;
xfb = [q;r;v];
end
plot3(pos(1,:),pos(2,:),pos(3,:));
Then I get:
>> pos(:,end)
ans =
1.0e+03 *
0
-0.0300
-4.4431
There's a slight difference in the z-value from integration/rounding errors, but the y-value is correct. There's no problem in the output.
I think the issue for you is kind of the same issue I had with the spatial vector formulation - it was really hard for me to conceptualize. I studied Featherstone's method for a while on my own and eventually gave up because of difficulties I had in trying to implement anything based on his work. I didn't (don't) have any formal classroom training on his spatial maths and so didn't have anyone to ask for help and didn't have the ability to get feedback on whether anything I was doing was correct or not.
You've given the sphere a rotational speed of -5
about the x-axis, and this is how you're getting a y-axis term in the spatial acceleration.
Frankly speaking, I've never had any education with regards to screw theory, Plücker coordinates, etc., so again I don't have any intuition here to be able to illuminate anything more for you. I'd like to say it's Coriolis forces or some other fictional force, but it's all a guess for me.
You can look at the source code for FDfb
and see the conversion from your spatial velocities to local floating-body velocities. When I plot them, everything looks as expected - the sphere is spinning, so I see the speed oscillating between y- and z-axes, and it's in free-fall, so I see the magnitude of speed (on both axes) increasing.
So, tl;dr - I can't explain how the spatial velocities or accelerations are working, but the orientations and positions should all be coming out correct.
I guess Featherstone's library has been sufficiently tested so it must be my inputs. So what is wrong with my inputs?
What are your inputs? Could you please edit your post to include the Matlab commands you're using to generate that output? $\endgroup$