MATLAB not detecting any of my functions from methods
3 次查看(过去 30 天)
显示 更早的评论
This was the basic code that I wrote and I am having trouble overriding the operator.
g1 = SE3([1;2;3],[1 0 0;0 0 -1; 0 1 0]);
g2 = SE3([2;-1;-1],[sqrt(2)/2 0 -sqrt(2)/2;0 1 0; sqrt(2)/2 1 sqrt(2)/2]);
g3 = g1 * g2
The function in questions is basically the mtimes operator where I am override the * operator.
And then my error basically turns into Operator '*' is not supported for operands of type 'SE3'.
%================================== SE3 ==================================
%
% class SE3
%
% g = SE3(d, theta)
%
%
% A Matlab class implementation of SE(2) [Special Euclidean 2-space].
% Allows for the operations written down as math equations to be
% reproduced in Matlab as code. At least that's the idea. It's about
% as close as one can get to the math.
%
%================================== SE3 ==================================
classdef SE3 < handle
properties (Access = public)
M; % Internal implementation is homogeneous.
end
%
%========================= Public Member Methods =========================
%
methods
%-------------------------------- SE3 --------------------------------
%
% Constructor for the class. Expects translation vector and rotation
% angle. If both missing, then initialize as identity.
%
function g = SE3(d, R)
if (nargin == 0)
g.M = eye(4);
else
g.M = [R, d; 0 0 0 1];
end
end
%
%------------------------------ display ------------------------------
%
% Function used by Matlab to "print" or display the object.
% Just outputs it in homogeneous form.
%
function display(g)
disp(g.M);
end
%-------------------------------- plot -------------------------------
%
% Plots the coordinate frame associated to g. The figure is cleared,
% so this will clear any existing graphic in the figure. To plot on
% top of an existing figure, set hold to on. The label is the name
% of label given to the frame (if given is it writen out). The
% linecolor is a valid plot linespec character. Finally sc is the
% specification of the scale for plotting. It will rescale the
% line segments associated with the frame axes and also with the location
% of the label, if there is a label.
%
% Inputs:
% g - The SE2 coordinate frame to plot.
% label - The label to assign the frame.
% linecolor - The line color to use for plotting. (See `help plot`)
% sc - scale to plot things at.
% a 2x1 vector, first element is length of axes.
% second element is a scalar indicating roughly how far
% from the origin the label should be placed.
%
% Output:
% The coordinate frame, and possibly a label, is plotted.
%
function plot(g, flabel, lcol, sc)
if ( (nargin < 2) )
flabel = '';
end
if ( (nargin < 3) || isempty(lcol) )
lcol = 'b';
end
if ( (nargin < 4) || isempty(sc) )
sc = [1.0 0.5];
elseif (size(sc,2) == 1)
sc = [sc 2];
end
d = getTranslation(g);
R = getRotation(g);
ex = R*[sc(1);0;0]; % get rotated x-axis.
ey = R*[0;sc(1);0]; % get rotated y-axis.
ez = R*[0;0;sc(1)]; % get rotated z-axis.
isheld = ishold;
pts = [d , d+ex];
plot3(pts(1,:), pts(2,:), pts(3,:),lcol); % x-axis
hold on;
pts = [d , d+ey];
plot3(pts(1,:), pts(2,:), pts(3,:),lcol); % y-axis
pts = [d , d+ez];
plot3(pts(1,:), pts(2,:), pts(3,:),lcol); % z-axis
plot3(d(1), d(2), d(3), [lcol 'o'],'MarkerSize',7); % origin
if (~isempty(flabel))
pts = d - (sc(2)/sc(1))*(ex+ey+ez);
text(pts(1), pts(2), pts(3),flabel);
end
if (~isheld)
hold off;
end
%------------------------------- inv -------------------------------
%
% Returns the inverse of the element g. Can invoke in two ways:
%
% g.inv();
%
% or
%
% inv(g);
%
%
function invg = inv(g)
invg = SE3(); % Create the return element as identity element.
%invM = WHAT_WHAT; % Compute inverse of matrix.
invg.M = inv(g.M); % Set matrix of newly created element to inverse.
end
%------------------------------ times ------------------------------
%
% This function is the operator overload that implements the left
% action of g on the point p.
%
% Can be invoked in the following equivalent ways:
%
% >> p2 = g .* p;
%
% >> p2 = times(g, p);
%
% >> p2 = g.times(p);
%
function p2 = times(g, el)
p2 = g.leftact(el);
end
%------------------------------ mtimes -----------------------------
%
% Computes and returns the product of g1 with g2.
%
% Can be invoked in the following equivalent ways:
%
% >> g3 = g1 * g2;
%
% >> g3 = g1.mtimes(g2);
%
% >> g3 = mtimes(g1, g2);
%
function g3 = mtimes(g1, g2)
g3 = SE3(); % Initialize return element as identity.
% MISSING LINE HERE TO PERFORM PROPER MULTIPLICATION.
g3.M = g1.M*g2.M;; % Set the return element matrix to product.
end
%----------------------------- leftact -----------------------------
%
% g.leftact(p) --> same as g . p
%
% with p a 2x1 specifying point coordinates.
%
% g.leftact(v) --> same as g . v
%
% with v a 3x1 specifying a velocity.
% This applies to pure translational velocities in
% homogeneous form, or to SE3 velocities in vector forn.
%
% This function takes a change of coordinates and a point/velocity,
% and returns the transformation of that point/velocity under the
% change of coordinates.
%
% Alternatively, one can think of the change of coordinates as a
% transformation of the point to somewhere else, e.g., a displacement
% of the point. It all depends on one's perspective of the
% operation/situation.
%
function x2 = leftact(g, x)
if ( (size(x,1) == 3) && (size(x,2) == 1) )
% two vector, this is product with a point.
x2 = g.M(1:3, :)* [x;ones(1, size(x,2))];
elseif ( (size(x,1) == 4) && (size(x,2) == 1) )
% three vector, this is homogeneous representation.
% fill out with proper product.
% should return a homogenous point or vector.
x2 = g.M*x;
end
end
%----------------------------- adjoint -----------------------------
%
% h.adjoint(g) --> same as Adjoint(h) . g
%
% h.adjoint(xi) --> same as Adjoint(h) . xi
%
% Computes and returns the adjoint of g. The adjoint is defined to
% operate as:
%
% Ad_h (g) = h * g2 * inverse(h)
%
function z = adjoint(g, x)
if (isa(x,'SE3'))
% if x is a Lie group, then deal with properly.
elseif ( (size(x,1) == 6) && (size(x,2) == 1) )
% if x is vector form of Lie algebra, then deal with properly.
elseif ( (size(x,1) == 4) && (size(x,2) == 4) )
% if x is a homogeneous matrix form of Lie algebra, ...
end
end
%-------------------------------- log --------------------------------
%
% Compute the log of a Lie group element. Returns the vector form.
%
function xi = log(g, tau)
if ((nargin < 2) || (isempty(tau))) % No tau, assume unity.
tau = 1;
end
% REST OF CODE HERE!
end
%
%--------------------------- getTranslation --------------------------
%
% Get the translation vector of the frame/object.
%
%
function T = getTranslation(g)
T = g.M(1:3, 4);
end
%------------------------- getRotationMatrix -------------------------
%
% Get the rotation or orientation of the frame/object.
%
%
function R = getRotationMatrix(g)
R = g.M(1:3, 1:3);
end
end
end
%
%======================= Static (Helper) Methods =======================
%
% These methods are helper functions for the class. Typically they
% do not involve actual SE(3) Lie group elements, but are functions
% that are related to the SE3() Lie group. Even though they do not
% take elements of the class, they still may return elements of the
% class.
%
% They get run by invoking as follows:
%
% output = SE3.funcName(input);
%
methods(Static)
%-------------------------------- hat --------------------------------
%
% Hat a vector element representation of the Lie algebra se(3).
%
function xiHat = hat(xiVec)
%TO BE DONE WITH FIRST PROBLEM ON THIS.
end
%------------------------------- unhat -------------------------------
%
% Unhat a homogeneous matrix element of the Lie algebra se(3).
%
function xiVec = unhat(xiHat)
%TO BE DONE WITH FIRST PROBLEM ON THIS.
end
%-------------------------------- exp --------------------------------
%
% Takes in an element of the Lie algebra and compute the exponential
% of it. Should return an actual SE3 element.
%
function expXi = exp(xi, tau)
if ((nargin < 2) || (isempty(tau)))
tau = 1;
end
%TO BE DONE WITH FIRST PROBLEM ON THIS.
end
%-------------------------------- RotX -------------------------------
%
% Takes an angle and generates rotation matrix about that angle,
% with respect to x-axis.
%
function Rmat = RotX(theta)
%TO BE DONE WITH FIRST PROBLEM ON THIS.
end
%-------------------------------- RotY -------------------------------
%
% Takes an angle and generates rotation matrix about that angle.
% with respect to y-axis.
%
function Rmat = RotY(theta)
%TO BE DONE WITH FIRST PROBLEM ON THIS.
end
%-------------------------------- RotZ -------------------------------
%
% Takes an angle and generates rotation matrix about that angle.
% with respect to z-axis.
%
function Rmat = RotZ(theta)
%TO BE DONE WITH FIRST PROBLEM ON THIS.
end
%---------------------------- EulerXYZtoR ----------------------------
%
% Generates a rotation matrix given the x-y-z Euler angle convention.
%
function Rmat = EulerXYZtoR(thX, thY, thZ)
% Should be RotX * RotY * RotZ
%TO BE DONE WITH FIRST PROBLEM ON THIS.
end
%---------------------------- RtoEulerXYZ ----------------------------
%
% Generates a rotation matrix given the x-y-z Euler angle convention.
%
function Rmat = RtoEulerXYZ(thX, thY, thZ)
% Should be RotX * RotY * RotZ
% TO BE DONE LATER, AS PART OF INVERSE KINEMATICS.
end
end
end
0 个评论
回答(1 个)
Matt J
2021-10-10
You have a missing 'end' on your plot() method and an extra 'end' on your public methods block.
0 个评论
另请参阅
类别
在 Help Center 和 File Exchange 中查找有关 Create System Objects 的更多信息
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!