zoukankan      html  css  js  c++  java
  • 三维坐标变换习题

    最近在做《先进机器人控制》作业,题目如下

     笔者答案以及python3计算如下

    #方法一
    
    import numpy as np 
    
    F=np.mat([[0,-1,0,10],[1,0,0,20],[0,0,1,1],[0,0,0,1]])
    u=np.mat([[3],[2],[2],[1]])
    
    v=F*u
    print(v[0:3])
    
    Ry90_homo=np.mat([[0,0,1,0],[0,1,0,0],[-1,0,0,0],[0,0,0,1]])
    Tx20_homo=np.mat([[1,0,0,20],[0,1,0,0],[0,0,1,0],[0,0,0,1]])
    Ry90=Ry90_homo[0:3,0:3]
    Tx20=Tx20_homo[0:3,3]
    
    F1=Tx20_homo*(Ry90_homo*F)
    print(F1)
    
    R1=F1[0:3,0:3]
    p1=F1[0:3,3]
    p1_inverse=-R1.transpose()*p1
    temp1_inverse=np.hstack((R1.transpose(),p1_inverse))
    c=np.mat([[0,0,0,1]])
    F1_inverse=np.vstack((temp1_inverse,c))
    
    u1=F1_inverse*v
    print(u1[0:3])
    

    #方法二
    
    import numpy as np 
    
    R=np.mat([[0,-1,0],[1,0,0],[0,0,1]])
    p=np.mat([[10],[20],[1]])
    u=np.mat([[3],[2],[2]])
    
    v=R*u+p
    print(v)
    
    Ry90=np.mat([[0,0,1],[0,1,0],[-1,0,0]])
    Tx20=np.mat([[20],[0],[0]])
    
    R1=Ry90*R
    p1=Ry90*p+Tx20
    temp=np.hstack((R1,p1))
    c=np.mat([[0,0,0,1]])
    F1=np.vstack((temp,c))
    print(F1)
    
    u1=R1.transpose()*(v-p1)
    print(u1)
    

  • 相关阅读:
    SpringBoot构建RESTful API
    Zynq7000系列之芯片系统结构概述
    FPGA编程技巧系列之按键边沿检测
    异常处理规范
    接口定义规范
    工具类编写规范
    第三个月
    测试计算器心得
    2015年三月
    第一份工作
  • 原文地址:https://www.cnblogs.com/hzcya1995/p/13281742.html
Copyright © 2011-2022 走看看