《Robotics, Vision and Control — Fundamental Algorithms in MATLAB》第二章课后习题

《Robotics, Vision and Control — Fundamental Algorithms in MATLAB》第二章课后习题

1.探讨一下负值的横滚-俯仰-偏航角的效果。从RPY角转换到旋转矩阵，然后再转换回RPY角，看看转换结果是否像欧拉角那样与初始值不同。
Ans：From RPY angles to rotation matrix,and then back to RPY angles from the rotation matirx,will get the same value.like the following :

 R1=rpy2r(-0.1,0.2,-0.3);
 back_rpy=tr2rpy(R1);
 trplot(R1,'frame','1','color','r');
 view(30,30);

and compare the value of RPY angles:

    R1=rpy2r(-0.1,0.2,-0.3)
      0.9363    0.2896    0.1987
      -0.3130    0.9447    0.0978
      -0.1593   -0.1538    0.9752

    back_rpy=tr2rpy(R1)
     -0.1000    0.2000   -0.3000

2.探索一下与函数trplot相关的更多选项。
Ans：2.trplot Draw a coordinate frame
trplot2 draws a 2D coordinate frame
tranimate Animate a coordinate frame

3.使用函数tranimate演示以下相对于各种轴的坐标系平移与旋转动作。
Ans：translated along the X axis:
tranimate(transl(1,0,0))
translated along the Y axis:
tranimate(transl(0,1,0))
translated along the Z axis:
tranimate(transl(0,0,1))
rotate about the X axis:
tranimate(rotx(pi/2))
rotate about the y axis:
tranimate(roty(pi/2))
rotate about the z axis:
tranimate(rotz(pi/2))

4.制作一个翻滚立方体的动画：
a)编写一个函数来绘制一个中心在原点的立方体。
b)修改这个函数使其接受一个齐次变换的参数，该齐次变换将作用在立方体的顶点上。
c)动画出绕x轴的旋转。
d)动画出绕所有轴的旋转。
Ans:
a)

    function cube()
    v=[-10 -10 -5;-10 10 -5;10 10 -5;10 -10 -5;-10 -10 5;-10 10 5;10 10 5;10 -10 5];
    f=[1 2 3 4;2 6 7 3;4 3 7 8;1 5 8 4;1 2 6 5;5 6 7 8];
    patch('Faces',f,'Vertices',v,'FaceColor','b');
    view(30,30)

b)

 function B=cube_input(x,y,z,roll,pich,yaw)
    T1=transl(x,y,z);
    t2=rpy2r(roll,pich,yaw);
    T2=r2t(t2);
    T=T1*T2;
    A1=[-10 -10 -10];
    A2=[-10 10 -10];
    A3=[10 10 -10];
    A4=[10 -10 -10];
    A5=[-10 -10 10];
    A6=[-10 10 10];
    A7=[10 10 10];
    A8=[10 -10 10];
    v=[A1;A2;A3;A4;A5;A6;A7;A8];
    f=[1 2 3 4;2 6 7 3;4 3 7 8;1 5 8 4;1 2 6 5;5 6 7 8];
    patch('Faces',f,'Vertices',v,'FaceColor','b');
    view(30,30);
    a1=A1';
    a2=A2';
    a3=A3';
    a4=A4';
    a5=A5';
    a6=A6';
    a7=A7';
    a8=A8';
    A=[a1,a2,a3,a4,a5,a6,a7,a8];
    B=zeros(4,8);
    C=zeros(3,8);
    D=zeros(8,3);
    for i=1:8
            B(:,i)=T*[A(:,i);1];%each B's Columns represents the homogeneous transformed vertex value of A
            C(:,i)=h2e(B(:,i));
            D(i,:)=C(:,i)';
    end
    v1=[D(1,:);D(2,:);D(3,:);D(4,:);D(5,:);D(6,:);D(7,:);D(8,:)];
    hold on;
    patch('Faces',f,'Vertices',v1,'FaceColor','b');
    view(30,30);

inputcube_input(1,2,5,0.2,0.3,0.4)

c)

function cude_rotation()
    while 1
    v=[-10 -10 -10;-10 10 -10;10 10 -10;10 -10 -10;-10 -10 10;-10 10 10;10 10 10;10 -10 10];
    f=[1 2 3 4;2 6 7 3;4 3 7 8;1 5 8 4;1 2 6 5;5 6 7 8];
    %patch('Faces',f,'Vertices',v,'FaceColor','b');
    %view(30,30)

    v1=zeros(8,3);
    for step=pi/10:2*pi
        for i=1:8
            v1(i,:)=v(i,:)*rotx(step);
        end

    patch('Faces',f,'Vertices',v1,'FaceColor','b');
    getframe;
    view(30,30);
    cla;
    end
    end

d)

function cude_rotation_about_x_y_z_axis()
    while 1
    v=[-10 -10 -10;-10 10 -10;10 10 -10;10 -10 -10;-10 -10 10;-10 10 10;10 10 10;10 -10 10];
    f=[1 2 3 4;2 6 7 3;4 3 7 8;1 5 8 4;1 2 6 5;5 6 7 8];
    %patch('Faces',f,'Vertices',v,'FaceColor','b');
    %view(30,30)

    v1=zeros(8,3);
    for step=pi/10:2*pi
        for i=1:8
            v1(i,:)=v(i,:)*rotx(step);
        end

    patch('Faces',f,'Vertices',v1,'FaceColor','b');
    getframe;
    view(30,30);
    cla;
    end
        for step=pi/10:2*pi
        for i=1:8
            v1(i,:)=v(i,:)*roty(step);
        end

    patch('Faces',f,'Vertices',v1,'FaceColor','b');
    getframe;
    view(30,30);
    cla;
        end
        for step=pi/10:2*pi
        for i=1:8
            v1(i,:)=v(i,:)*rotz(step);
        end

    patch('Faces',f,'Vertices',v1,'FaceColor','b');
    getframe;
    view(30,30);
    cla;
    end
    end

5.用方程（2.21）展示TT^-1=I
Ans:
%TT^-1=(R t;0 1)(R^T -R^T*t;0 1)=(R*R^T R(-R^T*t)+t;0 1)=(1 0;0 1)

T0=eye(4,4);
T1=transl(0,1,1)*trotz(0.5)*transl(4,0,1);
T2=inv(T1);
T3=T1*T2;
trplot(T0,'frame','O','color','r');
hold on;
trplot(T3,'frame','1','color','b');

6.绘制如图2.11所示的序列图。
Ans：

%T1=rotx(pi/2)*roty(pi/2)
subplot(2,2,1),trplot(rotx(pi/2))
subplot(2,2,2),trplot(rotx(pi/2)*roty(pi/2),'frame','2','color','b')
%T2=roty(pi/2)*rotx(pi/2)
subplot(2,2,3),trplot(roty(pi/2),'frame','3','color','r')
subplot(2,2,4),trplot(roty(pi/2)*rotx(pi/2),'frame','4','color','b')

7.笛卡尔的头骨在哪里？
Ans：Descartes’ skull was collected in the Museum of Humanity in Paris.

8.创建一个向量-四元数类，它支持复合、取负和点变换。
Ans：

classdef Quaternion_Jiajie

    properties (SetAccess = private)
        quaternion
        vector
    end

    methods
        function f=Quaternion_Jiajie(q,v)
            f.quaternion=q;
            f.vector=v;
        end

        function z1_plus_z2=vq_plus(z1,z2)
        %z1=[t1,q1] t1=[1,2,3]' q1=0.995 < 0.099833, 0, 0 >    
        %z2=[t2,q2] t2=[4,5,6]' q2=0.98877 < 0, 0.14944, 0 >
        t1=z1.vector;
        t2=z2.vector;
        q1=z1.quaternion;
        q2=z2.quaternion;
        T=t1+q1*t2;
        Q=q1*q2;
        z1_plus_z2 = [T,Q];
        end

        function neg_z=negative(z1)
            t=z1.vector;
            q=z1.quaternion;
            T=-1*(inv(q)*t);
            Q=inv(q);
            neg_z=[T,Q];
        end

        function point_x=translate(z1,point_y)

            point_x=z1.quaternion*point_y.vector+z1.vector;
        end

    end
end

Input

q1=Quaternion(rpy2tr(0.1,0.2,0.3))
q2=Quaternion(rpy2tr(0.4,0.5,0.6))
t1=[1,2,3]'
t2=[4,5,6]'
z1=[q1,t1]
z1=Quaternion_Jiajie(q1,t1)
z2=[q2,t2]
z2=Quaternion_Jiajie(q2,t2)
p_y=[2,5,9]'
p_y=Quaternion_Jiajie(0,p_y)
vq_plus(z1,z2)
negative(z1)
translate(z1,p_y)

Result

>> z1=Quaternion_Jiajie(q1,t1)

z1 = 

  Quaternion_Jiajie with properties:

    quaternion: [1×1 Quaternion]
        vector: [3×1 double]

>> z2=Quaternion_Jiajie(q2,t2)

z2 = 

  Quaternion_Jiajie with properties:

    quaternion: [1×1 Quaternion]
        vector: [3×1 double]

>> p_y=Quaternion_Jiajie(0,p_y)

p_y = 

  Quaternion_Jiajie with properties:

    quaternion: 0
        vector: [3×1 double]

>> vq_plus(z1,z2)

ans = 


0 < 4.489, 7.3884, 8.9826 >         
0.7939 < 0.31116, 0.27118, 0.44651 >

>> negative(z1)

ans = 


0 < -1.0842, -2.0612, -2.9285 >           
0.98186 < -0.064071, -0.091158, -0.15344 >

>> translate(z1,p_y)

ans =

    3.2125
    6.4689
   12.2268

以上内容均根据个人理解所写，不代表标准答案，仅供参考，欢迎反馈交流，O(∩_∩)O谢谢。