首先,关于我的设置:我有一个机械臂,可以将点和运动模式编程到其中。目标是让它根据我们投入的输入以某种方式移动。
与机械臂同在一张桌子上的是另一只手臂,它在人力的作用下移动,可以感知它在太空中的位置。这两个手臂已经共享一个坐标系,但是我在进行特定计算时遇到了麻烦,这让我很头疼。
当前的目标是专门用传感臂取三个点,然后将其转换为穿过这三个点的半椭圆弧。这条弧线应该从第一个点开始,在第二个点达到顶点,在第三个点结束,必要时穿过所有三个维度。这三个点通过 Visual Studio 输入,然后被放入 MATLAB 并变成一个 99 xyz 坐标的数组。
除了 MATLAB 函数之外,我们每个步骤都在工作。这些点与实际坐标相去甚远,尽管它们之间的关系似乎还不错。谁能告诉我代码有什么问题?
以下是我们目前所拥有的:
function P = getEllipticalPath(h0,hl,hr)
%define center of ellipse
center = (hl+hr)/2;
%want everything centered at (0,0,0)
h0 = h0 - center;
hl = hl - center;
hr = hr - center;
%xz plane direction between h0 and center
d = [h0(1),0,0]/49;
%now get the major/minor axis of the ellipse
%minor axis(along z axis)
a = h0(3);
b = hr(2);%arbitrary with hr
%set increment of orbit
incr = (pi)/99;
%by symmetry, only need to compute first half of orbit
%allocation
Pf = zeros(99,3);
for i=1:99
if(i < 50)
newpt = [0, b*cos(i*incr), a*sin(i*incr)] + (i*d);
else
newpt = [0, b*cos(i*incr), a*sin(i*incr)] + (99 - i)*d;
end
Pf(i,:) = [newpt(1), newpt(2), newpt(3)];
end
P = addOffset(Pf,-h0);
end
%simply adds a fixed translational offset to the given list of vectors
%(n*3 matrix). Assumes a matrix that is longer than 3.
function P = addOffset(points,offset)
newpoints = zeros(length(points),3);
for i=1:length(points);
newpoints(i,:) = points(i,:) + offset;
end
P = newpoints;
end
编辑:忘记输入输出信息;这是一个例子:
输入:
>> h0 = [-10.06 14.17 0.53 ]
h0 =
-10.0600 14.1700 0.5300
>> hl = [-45.49 7.87 1.07 ]
hl =
-45.4900 7.8700 1.0700
>> hr = [-4.52 -20.73 1.02 ]
hr =
-4.5200 -20.7300 1.0200
>> P = getEllipticalPath(h0,hl,hr)
输出: