Related
I have a followup question from this post : Extracting Yaw from a Quaternion .
Alike the OP, I want to move away from Euler rotations and use Quaternions. My scenario is as follows:
I am wearing an Apple smartwatch on my left hand and performing the bicep curls exercise. I do a few reps in the first set, and stop. I then rotate my body 90 degrees to the right in my room, and do another set of a few repetitions. I do this 4 times, and store the quaternions to disk using the xArbitraryCorrectedZVertical frame. When I use Euler rotations to describe pitch, using below code, I see a sinusoidal shape. When I want to reproduce the same pattern using quaternions, I get close, except the signal drifts. Here is my code:
clc;
clear;
quaternions_table = readtable(strcat("quaternions"), 'Delimiter',',');
attitude = quaternion(quaternions_table.w, quaternions_table.x, quaternions_table.y, quaternions_table.z);
quats = zeros(length(attitude), 1);
eulers = zeros(length(attitude), 1);
for i = 1:length(attitude)
[w,x,y,z] = parts(attitude(i) * quaternion(0,0,0,1));
quats(i) = rad2deg(atan2(z,w));
[w,x,y,z] = parts(attitude(i));
eulers(i) = rad2deg(asin(2*(y * w - z * x)));
end
figure(1);
clf;
hold on;
plot(quats, '.r');
plot(eulers, '.b');
legend('Q', 'E');
hold off;
Here is a plot of the difference:
How can I fix my code so that the quaternion generated pattern is equivalent to the euler's pitch, regardless of my "body" rotation?
Some sample data: https://filebin.net/02k2323ccbct1n38/quaternions
DISCLAIMER 1: this is a first attempt at putting together an answer. What follows is by no means conclusive. Any feedback aimed at improving this answer is sincerely welcome.
DISCLAIMER 2: I don't have access to any toolboxes featuring functions related to quaternions, so I relied on what's available on Mathworks's File Exchange, to which I added a function that builds quaternions by extracting their components from a table. Using the quaternion-related functions provided in Matlab's toolboxes, the OP should be able to replicate the calculations showcased hereinafter.
DISCLAIMER 3: the selection of the point to which the rotations are applied matters. Picking a different P will affect the resulting plot. In particular, with points far from P(1,0,0) the 3D map of the successive positions in space doesn't appear to be compatible with the physics of the OP moving the smart-watch in the way described in the question.
With the sample data provided by the OP, I was able to apply the quaternions to the point with coordinates P(1,0,0) i.e. a point on the global x-axis, using (pseudo-code):
% Pseudo code
rotatedP = Qi * P * conjugate(Qi);
where Qi is the i-th quaternion extracted from the sample data.
Here's a plot showing the positions in space assumed by the point after being rotated:
Once the coordinates of the point are available, calculating the pitch could be done by computing the acos of the dot product between the unit vector parallel to OP (O being the global origin) and the unit vector parallel to the z-axis (with components (0,0,1)), in pseudo-code:
% Pseudo code
pitch = acos(OP.z/norm(OP))
Here's a plot of the pitch shifted using the first value of the array, which is extremely close to what was obtained by the OP using Euler's angles:
Matlab code
Here's the code I used to generate the plots. As mentioned erlier, I relied on Przemyslaw Baranski's Quaternions. Please, note that my code is a very crude attempt at tackling the problem, with basically no optimization. Uncommenting the lines for the 3D plot slows down the process considerably:
function draw_quats()
% point to be rotated
P = [ 1, 0, 0 ];
% Import quaternion components
q_tab = readtable(strcat("quaternions_data"), 'Delimiter',',');
pitch = [];
pp = 1;
for k = 1:1:numel(q_tab.w)
% create a quaternion for rotation
Qrot = qGetQuaternionFromComponents(q_tab.x(k), q_tab.y(k), q_tab.z(k), q_tab.w(k));
% rotate point
Prot = qRotatePoint( P, Qrot );
Pnorm = sqrt(Prot(1)^2 + Prot(2)^2 + Prot(3)^2);
% % display axes
% quiver3( 0,0,0, 1,0,0 );
% hold on;
% quiver3( 0,0,0, 0,1,0 );
% quiver3( 0,0,0, 0,0,1 );
%
% % display point
% plot3( Prot(1), Prot(2), Prot(3), 'b.' );
%
% % setup the plot
% grid on;
% axis equal;
% axis([-1 1.0 -1 1.0 -1 1.0]);
% xlabel( 'x' );
% ylabel( 'y' );
% zlabel( 'z' );
%
% view( 45, 20 );
% drawnow;
% Compute pitch
pitch(pp) = acos(Prot(3)/Pnorm)*180/pi;
pp = pp + 1;
end
figure
plot(pitch-pitch(1),'.k');
end
And this is the definition of qGetQuaternionFromComponents:
function Q = qGetQuaternionFromComponents( x, y, z, w )
Q = [w x y z]';
end
In the event you have some of the toolboxes by MATLAB, you can do something like this:
for i = 1:length(attitude)
qPoint = quaternion(0,-1,0,0);
q = attitude(i);
[~,x,y,z] = parts(q * qPoint * conj(q));
Pnorm = sqrt(x^2 + y^2 + z^2);
quats(i) = asind(z/Pnorm);
end
References
Euclideanspace.com
Mathworks' FileExchange
The problem scenario is defined as follows problem statement (Click here)
The information what happens in one single iteration is provided here in this link
I am supposed to get one small arc touching the circle at both ends (curve 1 rotates circle about point A and then rotates touching circle at point B so both ends of arc touch the circle).
I am getting the result when I do this manually where I find intersection by using polyxpoly() , place the arc on intersection and keep rotating it till it touches the circle. But when I try to automate the process for many arcs by using a for loop I do not get desired result.
Since rotation of arc takes place , I have given small rotation step size but I get different results . Sometimes the for-loop works normally and gives desired result and sometimes gives abnormal results for certain iteration. I am unable to figure out the the problem with my approach.
Results are as follows click here to see the result
These are additional details which shows difference between desired result and undesired result
Desired Result Vs Undesired Result for theta=500 & i=2 ,3
Desired Result Vs Undesired Result for theta=1800 & i=4
It can be seen for theta = 500 & i = 2 the arc does not rotate and since it dose not rotate it gives same result for theta = 500 & i = 3
but when theta = 1800 it gives different results but fails at iteration 4 (for theta = 1800 i = 1,2,3 gives desired result but fails at i = 4). Check the result image.
The parameter theta can be found in rotation()
The code I used is as follows but For-loop is giving abnormal results for certain iteration depending on theta
clc,clear
r = 10; % arc length
R = 55; % radius of a circle
aa = 60*pi/180; % arc angle
ap = 0*pi/180; % arc position angle
k=0;
t = linspace(0,pi/2);
[x,y] = pol2cart(t,R); % circle data
t1 = linspace(0,aa)-aa/2+ap;
[x1,y1] = pol2cart(t1,r); % arc data
% Intersection of x axis (y=0) and quarter circle
x_axis_range=1:1000;
y_val_on_x_axis=zeros(1,length(x_axis_range));
[x1rc,y1rc]=polyxpoly(x_axis_range,y_val_on_x_axis,x,y);
xc_s(1)=x1rc;
yc_s(1)=y1rc;
% x1rc=55;
% y1rc=0;
for z=1:3
[x1_p,y1_p]= placement(x,y,x1,y1,x1rc,y1rc);
[x1_rotated,y1_rotated,xc,yc]=rotate(x,y,x1_p,y1_p,x1rc,y1rc);
%[x1_rotated,y1_rotated,xc,yc]=rotate(x,y,x1,y1,x1rc,y1rc);
x1rc=xc;
y1rc=yc;
end
% havent written the plot command
Placement()- The purpose of placement () is to shift the new arc to point of intersection of previous arc and the circle
function [x1_p,y1_p] = placement(x,y,x1,y1,x1rc,y1rc)
xcen=x1rc;
ycen=y1rc;
% shifting arc-lower-end to (t(1),0) or (
delx=xcen-x1(1); % Finding the x difference between arc-lower-end x-coordinate & x(1)[circle]
dely=ycen-y1(1); % Finding the y difference between arc-lower-end y-coordinate & y(1) [circle]
x1_p=x1+delx;
y1_p=y1+dely;
end
The rotation function():
function [x1_rotated,y1_rotated,xc,yc] = rotate(x,y,x1_p,y1_p,x1rc,y1rc)
% [x1_p,y1_p]= placement(x,y,x1,y1,x1rc,y1rc);
theta =linspace(0,pi,500);
i=1;
xc=[];
yc=[];
xcen=x1rc;
ycen=y1rc;
while isempty(xc)||xc==xcen && isempty(yc)||yc==ycen
% create a matrix of these points, which will be useful in future calculations
v = [x1_p;y1_p];
% choose a point which will be the center of rotation
x_center = xcen;
y_center = ycen;
% create a matrix which will be used later in calculations
center = repmat([x_center; y_center], 1, length(x1_p));
% define a theta degree counter-clockwise rotation matrix
R = [cos(theta(i)) -sin(theta(i)); sin(theta(i)) cos(theta(i))];
% do the rotation...
s = v - center; % shift points in the plane so that the center of rotation is at the origin
so = R*s; % apply the rotation about the origin
vo = so + center; % shift again so the origin goes back to the desired center of rotation
% this can be done in one line as:
% vo = R*(v - center) + center
% pick out the vectors of rotated x- and y-data
x1_rotated = vo(1,:);
y1_rotated = vo(2,:);
[xc,yc] = polyxpoly(x1_rotated,y1_rotated,x,y);
[xc1,yc1] = polyxpoly(x1_p,y1_p,x,y);
if length(xc)==2
xc=xc(2);
yc=yc(2);
end
i=i+1;
end
end
I tried modifying the "if statement" as following but it seems after a certain iteration it dosen't work. The new modification in if statement are as follows
if length(xc)==2
ubx=xc(1); lbx=xc(2);
uby=yc(2);lby=yc(1);
xc=xc(2);
yc=yc(2)
ub=[ubx uby];
lb=[lbx-4 lby];
end
Then I was trying to return the ub and lb and got the following error message
Output argument "ub" (and maybe others) not
assigned during call to "rotate". after fifth iteration of for loop . The if statement worked fine for 1,2,3,4 iteation
It's a floating point comparison problem.
Long story short, both 3*(4/3 -1) == 1 and sin(pi)==0 return false (Avoiding Common Problems with Floating-Point Arithmetic).
Change your while condition to something like this:
while (isempty(xc)||abs(xc-xcen)<=10*eps(xcen)) && (isempty(yc)||abs(yc-ycen)<=10*eps(ycen))
BTW, your assumption that when polyxpoly returns 2 point, the second point is your result, is not true. Try to find the furthest point from start point of the arc:
if length(xc)>1
[~, i] = max((xc-xcen).^2+(yc-ycen).^2);
xc=xc(i);
yc=yc(i);
end
I have this code here below. It basically rotates a box in 3D. I know I can rotate the camera using camorbit but I actually want to rotate the object, not the camera around it. So the strategy is to create an element in so(3) and take the exponential map to SO(3) as the element is varied continuously over time.
% Create object
yo = unique([-1 -1 -1; perms([1 1 -1]); perms([1 -1 -1]); 1 1 1], 'rows');
yo2 = delaunay(yo(:,1), yo(:,2), yo(:,3));
box=tetramesh(yo2, yo);
% Create prescribed rotation
seedMat=skewdec(3,1)*skewdec(3,2)*skewdec(3,3); % so(3)
t=linspace(0,20,500);
r=zeros(1,4,length(t));
reqTraj=zeros(3,3,length(t));
for i=1:length(t)
reqTraj(:,:,i)=expm(seedMat*t(i)); % SO(3)
r(:,:,i) = vrrotmat2vec(expm(seedMat*t(i))); %Axis angle form
end
% Freeze axis
axis vis3d
axis([ -2 2 -2 2 -2 2])
% Rotate
for i=1:length(t)
rotate(box,r(1,1:3,i),r(1,4,i))
pause(0.001)
end
My question is why is this animation jerky? Is there something that I can do to fix it?
EDIT: Changing the last part as follows, made the buggy/jerky behavior go away, but resulted in an entirely new rotation behavior. So i am not sure if it is the correct approach or not. Can someone comment?
% Rotate
for i=2:length(t)
rotate(box,r(1,1:3,i),r(1,4,i-1)-r(1,4,i))
pause(0.001)
end
I find that data points that lie on or near the axes are difficult to see. The obvious fix, of course, is to simply change the plot area using axis([xmin xmax ymin ymax]), but this is not preferable in all cases; for example, if the x axis is time, then moving the minimum x value to -1 to show activity at 0 does not make sense.
Instead, I was hoping to simply move the x and y axes away from the plot area, like I have done here:
left: MATLAB generated, right: desired (image editing software)
Is there a way to automatically do this in MATLAB? I thought there might be a way to do it by using the outerposition axes property (i.e., set it to [0 0 0.9 0.9] and drawing new axes where they originally were?), but I didn't get anywhere with that strategy.
The answers here already show you most of the way - here is the last step to separate the x and y axle as per the example you put together.
f = figure ( 'color', 'white' );
% create the axes and set some properties
ax = axes ( 'parent', f, 'box', 'off', 'nextplot', 'add', 'XMinorTick', 'on', 'YMinorTick', 'on' );
% plot some data
plot ( ax, 0:10, [0:10].^2, 'rx-' )
% modify the x and y limits to below the data (by a small amount)
ax.XLim(1) = ax.XLim(1)-(ax.XTick(2)-ax.XTick(1))/4;
ax.YLim(1) = ax.YLim(1)-(ax.YTick(2)-ax.YTick(1))/4;
% Set the tick direction
ax.TickDir = 'out';
% draw the plot to generate the undocumented vertex data var
drawnow()
%% R2015a
% X, Y and Z row of the start and end of the individual axle.
ax.XRuler.Axle.VertexData(1,1) = 0;
ax.YRuler.Axle.VertexData(2,1) = 0;
%% R2015b
% extract the x axis vertext data
% X, Y and Z row of the start and end of the individual axle.
vd = get(ax.XAxis.Axle,'VertexData');
% reset the zero value
vd(1,1) = 0;
% Update the vertex data
set(ax.XAxis.Axle,'VertexData',vd);
% repeat for Y (set 2nd row)
vd = get(ax.YAxis.Axle,'VertexData');
vd(2,1) = 0;
set(ax.YAxis.Axle,'VertexData',vd);
Edit: The vertex is something that Matlab recreates whenever the axes/figure changes size or if you zoom or pan for example.
You can try to counteract this (remember you are using undocumented features here) by adding a listener to attempt to capture this. We can use the MarkedClean event which is called quite a lot of times.
addlistener ( ax, 'MarkedClean', #(obj,event)resetVertex(ax) );
Where you resetVertex function is something like: (R2015b shown only)
Edit 2 added the code to turn off the minor ticks below 0.
function resetVertex ( ax )
% extract the x axis vertext data
% X, Y and Z row of the start and end of the individual axle.
ax.XAxis.Axle.VertexData(1,1) = 0;
% repeat for Y (set 2nd row)
ax.YAxis.Axle.VertexData(2,1) = 0;
% You can modify the minor Tick values by modifying the vertex data
% for them, e.g. remove any minor ticks below 0
ax.XAxis.MinorTickChild.VertexData(:,ax.XAxis.MinorTickChild.VertexData(1,:)<0) = [];
ax.YAxis.MinorTickChild.VertexData(:,ax.YAxis.MinorTickChild.VertexData(2,:)<0) = [];
end
Note: this uses undocumented features -> so may only work in certain versions of Matlab (I have added the code for r2015a & r2015b) and Matlab may recreate the vertex data depending on what you do with the plots..
Here is a simple way for achieving that:
% some data:
x = 1:100;
f=#(x) 5.*x;
y=f(x)+rand(1,length(x))*50;
close all
% plotting:
f1 = figure('Color','white');
ax = axes;
plot(ax,x,y,'o');
% 'clean' the data area a little bit:
box off
ax.TickDir = 'out';
% pushing the axis a bit forward:
lims = axis;
pos = ax.Position;
axis([lims(1)-ax.XTick(2)/5 lims(2)+0.1 lims(3)-ax.YTick(2)/5 lims(4)+0.1])
% Create lines
firstXtick = 0.013; %this value need to be adjusted only once per figure
firstYtick = 0.023; %this value need to be adjusted only once per figure
lx = annotation(f1,'line',[pos(1) pos(1)+firstXtick],...
[pos(2) pos(2)],'Color',[1 1 1],'LineWidth',1);
ly = annotation(f1,'line',[pos(1) pos(1)],...
[pos(2) pos(2)+firstYtick],'Color',[1 1 1],'LineWidth',1);
Which yields this figure:
The only thing to adjust here, once per type of figure, is firstXtick and firstYtick values, that have to be fine tuned to the specific axis. After setting them to the correct value the figure can be resized with no problem. Zoom and pan require a little fixes.
You can start your axes from less than zero and then remove the less than zero ticks from your plot. e.g.
plot(0:3:30,0:3:30); %Some random data for plotting
h = gca;
axis([-1 30 -1 30]); %Setting the axis from less than zero
box off; %Removing box
h.TickDir = 'out'; %Setting Direction of ticks to outwards
h.XTickLabel(1)= {' '}; %Removing the first tick of X-axis
h.YTickLabel(1)= {' '}; %Removing the first tick of Y-axis
With this code, you'll get this result:
This may have a drawback, sometimes, that zero ticks may also get removed (as you can see in above figure). This is because the plot had set the first ticks of axes equal to zero. This can be avoided using if condition. So, the code can be modified as below:
plot(0:3:30,0:3:30);
h = gca;
axis([-1 30 -1 30]);
box off;
h.TickDir = 'out';
if str2num(cell2mat(h.XTickLabel(1))) <0
h.XTickLabel(1)= {' '};
end
if str2num(cell2mat(h.YTickLabel(1))) <0
h.YTickLabel(1)= {' '};
end
The above code will yield the following result:-
Also note that, for your case, since your axes ticks are very less, -1 may not be much suitable for the starting value of axes and you may need to use -0.1 instead i.e. axis([-0.1 30 -0.1 30]);
With a slight modification of #matlabgui's answer you can track the (major) tick limits:
ax = gca();
% Set the tick direction
ax.TickDir = 'out';
% Make sure this stays when saving, zooming, etc
addlistener ( ax, 'MarkedClean', #(obj,event) change_ticks(ax) );
% Draw the plot to generate the undocumented vertex data variable
% and call callback for the first time
drawnow();
The callback
function change_ticks( ax )
% Modify ruler
ax.XRuler.Axle.VertexData(1,1) = ax.XTick(1);
ax.XRuler.Axle.VertexData(1,2) = ax.XTick(end);
ax.YRuler.Axle.VertexData(2,1) = ax.YTick(1);
ax.YRuler.Axle.VertexData(2,2) = ax.YTick(end);
end
I haven't test extensively but seems to work for custom ticks too. This nicely cuts the rulers not only on zero but beyond the fist and last tick. This was tested in Matlab 2019a on Windows and ax.XRuler.Axle.VertexData works just fine. Note this is only for major ticks!
I'm making a function which generalizes the cylinder function so that the cylinder has caps, can be any size and orientation. However on the look of the cylinder I am running into a jam. To get the caps to look right the curved part needs one set of shading and the caps need another. (And before you ask making 3 surfaces is not an option)
Here is the relevant code:
surface(xSurf,ySurf,zSurf,c,'EdgeColor','none','FaceLighting','phong');
and in case you want to see the whole code.
Thank you for your help,
John
function varargout = DrawCylinder(x,y,z,r,h,aVec,bVec,cVec,ccolor, npts)
% DrawCylinder Generate a three-dimensional cylinder
%
% DrawCylinder(x,y,z,a,b,c,aVec,bVec,CVec,ccolor, npts)
% creates a surface plot of a cylinder whose center is at (x,y,z), has
% semiaxes of length a, b, and c. The unit vectors associated with each
% semixis are aVec, bVec, and cVec and must be size 3 x 1 (column vector)
% with size of npts + 1.
%
% H = DrawCylinder(...) creates the surface plot and returns the handle H to each
% graphical object created.
%
% [X Y Z] = DrawCylinder(...) does not generate the surface plot and returns
% the data necessary to create the surface using:
% SURF(X,Y,Z);
%
% [X Y Z C] = DrawCylinder(...) does not generate the surface plot and returns
% the data necessary to create the surface using:
% SURF(X,Y,Z,C,'cdataMapping','direct');
%CREATE SURFACE FOR CYLINDER
[xCyl,yCyl,zCyl]=cylinder(1,npts);
xSurf=[zeros(1,max(size(xCyl)));xCyl;zeros(1,max(size(xCyl)))];
ySurf=[zeros(1,max(size(yCyl)));yCyl;zeros(1,max(size(yCyl)))];
zSurf=[zeros(1,max(size(zCyl)));zCyl;ones(1,max(size(zCyl)))] - 0.5;
xSurf = xSurf*r;
ySurf = ySurf*r;
zSurf = zSurf*h;
%ROTATE CYLINDER
%Make sure aVec,bVec, and cVec are column unit vectors:
if all(size(aVec)==[1,3])
aVec=aVec';
end
if all(size(bVec)==[1,3])
bVec=bVec';
end
if all(size(cVec)==[1,3])
cVec=cVec';
end
aVec=aVec/norm(aVec); %Make unit vectors
bVec=bVec/norm(bVec);
cVec=cVec/norm(cVec);
rot = [aVec,bVec,cVec]; %The rotation matrix
[iMax, jMax] = size(xSurf);
for i=1:iMax
for j=1:jMax
rotatedPt = rot*[xSurf(i,j);ySurf(i,j);zSurf(i,j)];
xSurf(i,j) = rotatedPt(1);
ySurf(i,j) = rotatedPt(2);
zSurf(i,j) = rotatedPt(3);
end
end
%TRANSLATE CYLINDER
xSurf = xSurf + x;
ySurf = ySurf + y;
zSurf = zSurf + z;
c = ccolor*ones(size(xSurf));
if nargout == 0
surface(xSurf,ySurf,zSurf,c,'EdgeColor','none','FaceLighting','phong');
elseif nargout == 1
varargout = {surface(xSurf,ySurf,zSurf,c,'EdgeColor','none','FaceLighting','phong');};
elseif nargout == 3
varargout = {xSurf ySurf zSurf};
elseif nargout == 4
varargout = {xSurf ySurf zSurf c};
end
end
Edit 8/18/12:
Just so you can see.
This is what I am getting ...
And this is what I want ...
I'm pretty sure that by "shade" you simply mean the colors of the end caps and not a more complicated effect. If so that's pretty straightforward, I'll give an example in gray scale
Change
c = ccolor*ones(size(xSurf));
to
c = (ccolor/255)*ones(size(xSurf));
c([1 3],:)=max(0,(ccolor-10))/255;
the first line initializes the c matrix with a normalized ccolor (expecting an 8 bit greyscale ccolor input, it normalizes to 0..1). The second line changes the caps (rows 1 and 3) to be a slightly darker color bottoming out at 0 and leaves the cylinder surfaces (rows 2 and 4) alone.
In order to make sure you see the results correctly, you need to alter the nargout==0 condition so that it looks like this
surface(xSurf,ySurf,zSurf,c,'EdgeColor','none','FaceLighting','phong');
colormap(gray(256));
caxis([0 1]);
The colormap just sets the colormap, similar to an 8 bit gray scale. The caxis command is fairly critical. According to Matlab's surface documentation
MATLAB performs a linear transformation on this data to obtain colors from the current colormap
For our purposes that is bad. Since we only have two values the lowest would be changed to 0 and the highest to 1. That effectively ignores our ccolor input and gives a white cylinder with two black caps. Using caxis([0 1]) preserves the full scale and ccolor's position within it.
Update:
Sounds like I misunderstood what you wanted, the easiest way to achieve something very close to the effect you want is to set 'MeshStyle' to 'row', like this:
surface(xSurf,ySurf,zSurf,c,'EdgeColor','k','FaceLighting','phong','MeshStyle','row');
This will give you the following result :
There's still a central point, but it's by far the simplest way to produce that effect.