WXYANXW 2023-02-24 11:11 采纳率: 63.6%
浏览 82
已结题

把这段matlab代码改成c语言的函数。

function [angle_theta1,angle_theta2,angle_theta3] = inver(px,py,pz) %11,0,0为零点

syms theta1 theta2 theta3 l a1 a2 a3
pi = 3.1415926;
a1 = 4.9;
a2 = 6.5;
a3 = 15.5;
l = 12.8;
    
theta1 = atan2(py,px);

theta2 = -2*atan((((8*(a2^2*a3*l - a2^2*a3*pz))/((a1^2 - 2*a1*px - a2^2 + 2*a2*a3 - a3^2 + l^2 - 2*l*pz + px^2 + pz^2)*(a1^2 - 2*a1*a2 - 2*a1*px + a2^2 + 2*a2*px - a3^2 + l^2 - 2*l*pz + px^2 + pz^2)) - (4*a2*a3*((- a1^2 + 2*a1*px + a2^2 + 2*a2*a3 + a3^2 - l^2 + 2*l*pz - px^2 - pz^2)*(a1^2 - 2*a1*px - a2^2 + 2*a2*a3 - a3^2 + l^2 - 2*l*pz + px^2 + pz^2))^(1/2))/((a1^2 - 2*a1*px - a2^2 + 2*a2*a3 - a3^2 + l^2 - 2*l*pz + px^2 + pz^2)*(a1^2 - 2*a1*a2 - 2*a1*px + a2^2 + 2*a2*px - a3^2 + l^2 - 2*l*pz + px^2 + pz^2)))*(a1^2 - 2*a1*px - a2^2 + 2*a2*a3 - a3^2 + l^2 - 2*l*pz + px^2 + pz^2))/(4*a2*a3));

theta3 = -2*atan(((- a1^2 + 2*a1*px + a2^2 + 2*a2*a3 + a3^2 - l^2 + 2*l*pz - px^2 - pz^2)*(a1^2 - 2*a1*px - a2^2 + 2*a2*a3 - a3^2 + l^2 - 2*l*pz + px^2 + pz^2))^(1/2)/(a1^2 - 2*a1*px - a2^2 + 2*a2*a3 - a3^2 + l^2 - 2*l*pz + px^2 + pz^2));






angle_theta1 = theta1/pi*180;
angle_theta2 = theta2/pi*180;
angle_theta3 = theta3/pi*180;

end

img

最后的代码是matlab上的运行成果,转换成c语言后算出来的角度angle_theta也需要一致,需要验证答案正确性。

把这段matlab代码改成c语言的函数。

这段matlab代码是计算逆运动学三个角度的,由于其中的^2平方形式无法用在c语言上,所以需要转换成pow的方式,比较难。其实只要将所有的^2平方格式的转化成pow格式即可。函数的解释是:输入三个参数px py pz,为尖端的三个坐标,然后利用这三个坐标计算theta123,然后弧度制转角度制,输出。

记得先验证答案正确再回答,感谢大家!

  • 写回答

7条回答 默认 最新

  • A1358722560 2023-02-24 13:12
    关注

    根据要求,编写了以下示例代码

    #include<stdlib.h>
    #include<stdio.h>
    #include <math.h>
    float pi = 3.1415926;
    float  a1 = 4.9;
    float a2 = 6.5;
    float a3 = 15.5;
    float l = 12.8;
    float theta1=0;
    float theta2=0;
    float theta3=0;
    float angle_theta1=0;
    float angle_theta2=0;
    float angle_theta3=0;
    float x[3]={0,0,0};
    void Inver(float px,float py,float pz,float *angle)
    {
    
        
        theta1 = atan2(py,px);
     
        theta2 = -2*atan((((8*(a2*a2*a3*l - a2*a2*a3*pz))/((a1*a1 - 2*a1*px - a2*a2 + 2*a2*a3 - a3*a3 + l*l - 2*l*pz + px*px + pz*pz)*(a1*a1 - 2*a1*a2 - 2*a1*px + a2*a2 + 2*a2*px - a3*a3 + l*l - 2*l*pz + px*px + pz*pz)) - (4*a2*a3*sqrt((- a1*a1 + 2*a1*px + a2*a2 + 2*a2*a3 + a3*a3 - l*l + 2*l*pz - px*px - pz*pz)*(a1*a1 - 2*a1*px - a2*a2 + 2*a2*a3 - a3*a3 + l*l - 2*l*pz + px*px + pz*pz)))/((a1*a1 - 2*a1*px - a2*a2 + 2*a2*a3 - a3*a3 + l*l - 2*l*pz + px*px + pz*pz)*(a1*a1 - 2*a1*a2 - 2*a1*px + a2*a2 + 2*a2*px - a3*a3 + l*l - 2*l*pz + px*px + pz*pz)))*(a1*a1 - 2*a1*px - a2*a2 + 2*a2*a3 - a3*a3 + l*l - 2*l*pz + px*px + pz*pz))/(4*a2*a3));
         
        theta3 = -2*atan(sqrt((- a1*a1 + 2*a1*px + a2*a2 + 2*a2*a3 + a3*a3 - l*l + 2*l*pz - px*px - pz*pz)*(a1*a1 - 2*a1*px - a2*a2 + 2*a2*a3 - a3*a3 + l*l - 2*l*pz + px*px + pz*pz))/(a1*a1 - 2*a1*px - a2*a2 + 2*a2*a3 - a3*a3 + l*l - 2*l*pz + px*px + pz*pz));
         
         angle[0] = theta1/pi*180;
        angle[1] = theta2/pi*180;
        angle[2] = theta3/pi*180;
     
    }
    
    int main(void)
    {
        Inver(11,0,0,x);
        printf("%f\n",x[0]);
        printf("%f\n",x[1]);
        printf("%f\n",x[2]);
        
    }
    
     
    
    

    运行结果为:

    img


    与题目中的相同

    img

    结果满足要求
    如果问题得到解决请点 采纳~~

    本回答被题主选为最佳回答 , 对您是否有帮助呢?
    评论 编辑记录
查看更多回答(6条)

报告相同问题?

问题事件

  • 系统已结题 3月5日
  • 已采纳回答 2月25日
  • 创建了问题 2月24日

悬赏问题

  • ¥15 鼠标右键,撤销删除 复制 移动,要怎样删除
  • ¥15 使用MATLAB进行余弦相似度计算加速
  • ¥15 服务器安装php5.6版本
  • ¥15 我想用51单片机和数码管做一个从0开始的计数表 我写了一串代码 但是放到单片机里面数码管只闪烁一下然后熄灭
  • ¥20 系统工程中,状态空间模型中状态方程的应用。请猛男来完整讲一下下面所有问题
  • ¥15 我想在WPF的Model Code中获取ViewModel Code中的一个参数
  • ¥15 arcgis处理土地利用道路 建筑 林地分类
  • ¥20 使用visual studio 工具用C++语音,调用openslsx库读取excel文件的sheet问题
  • ¥100 寻会做云闪付tn转h5支付链接的技术
  • ¥15 DockerSwarm跨节点无法访问问题