应《网络安全法》要求，自2017年10月1日起，未进行实名认证将不得使用互联网跟帖服务。为保障您的帐号能够正常使用，请尽快对帐号进行手机号验证，感谢您的理解与支持！

24小时热门版块排行榜    

小木虫论坛-学术科研互动平台 » 专业学科区 » 信息科学 » 人工智能 » 【原创】六自由度机器人Jacobian(雅克比)矩阵计算类
141/2返回列表12下一页
查看: 3689  |  回复: 13
只看楼主 @他人 存档 新回复提醒 (忽略) 收藏    在APP中查看

想飞的猪

木虫 (正式写手)

[交流] 【原创】六自由度机器人Jacobian(雅克比)矩阵计算类 已有1人参与

作者：想飞的猪

说明：MLGetIdentityMat为获得单位矩阵函数

MLMatMulti为矩阵相乘函数

和OpenCV求逆矩阵函数cvInvert没有给出请大家自己写一下！很简单的！

typedef struct RobotJacobian6
{
//变量!
//各关节传递矩阵!
union
{
  struct
  {
   double AMat[6][4][4];
  };
  double A0to1[4][4];
  double A1to2[4][4];
  double A2to3[4][4];
  double A3to4[4][4];
  double A4to5[4][4];
  double A5to6[4][4];
};


union
{
  struct
  {
   double TMat[6][4][4];
  };
  struct
  {
   double T0to6[4][4];
   double T1to6[4][4];
   double T2to6[4][4];
   double T3to6[4][4];
   double T4to6[4][4];
   double T5to6[4][4];
  };
};

//末端位姿!
double EndPose[4][4];

//D-H参数表!
double DHParam[6][4];//顺序为：Angle d_L a_L a_A!

//雅克比矩阵!
double EndJacobian[6][6];
//逆雅克比矩阵!
double EndInvJacobian[6][6];

//基坐标的笛卡尔微分运动到末端坐标的传递矩阵!
double JBasetoEnd[6][6];
double T_1to6[4][4];//该矩阵的姿态与基坐标一致，位置与末端坐标一致!
                     //以便可以按照基坐标进行平动和绕基坐标轴方向转动!

double mInput[6]; //输入!
double mOutput[6];//输出!
int    mMode;


void GetAMat()
{
  for (int i=0;i<6;i++)
  {
   MLGetDHTransMat(AMat,DHParam[0],DHParam[1],DHParam[2],DHParam[3]);
  }
}

void GetTMat()
{
  MLGetIdentityMat(T5to6);

  MLMatMulti(AMat[5],T5to6);
  MLMatMulti(AMat[4],T5to6,T4to6);
  MLMatMulti(AMat[3],T4to6,T3to6);
  MLMatMulti(AMat[2],T3to6,T2to6);
  MLMatMulti(AMat[1],T2to6,T1to6);
  MLMatMulti(AMat[0],T1to6,T0to6);
}

void UpdateAngle(double Angles[6])//Angles为弧度!
{
  for (int i=0;i<6;i++)
  {
   DHParam[0]=Angles;
  }
  GetAMat();
  GetEndPose();
  GetTMat();
  GetEndJacobian();
  GetEndInvJacobian();

  GetJBasetoEnd();
}

void Inti(double DHparameter[6][4])
{
  for (int i=0;i<6;i++)
  {
   for (int j=0;j<4;j++)
   {
    DHParam[j]=DHparameter[j];
   }
  }
  GetAMat();
  GetEndPose();
  GetTMat();
  GetEndJacobian();
  GetEndInvJacobian();

  GetJBasetoEnd();
  
  for (i=0;i<6;i++)
  {
   mInput=0;
   mOutput=0;
  }

  mMode=BASE;
}

void GetEndPose()
{
  MLGetIdentityMat(EndPose);
  for (int i=0;i<6;i++)
  {
   MLMatMulti(AMat[5-i],EndPose);
  }
}

void GetEndJacobian()
{
  for (int i=0;i<6;i++)
  {
   EndJacobian[0]=-1*TMat[Xx][Nn]*TMat[Yy][Pp]+TMat[Yy][Nn]*TMat[Xx][Pp];
   EndJacobian[1]=-1*TMat[Xx][Oo]*TMat[Yy][Pp]+TMat[Yy][Oo]*TMat[Xx][Pp];
   EndJacobian[2]=-1*TMat[Xx][Aa]*TMat[Yy][Pp]+TMat[Yy][Aa]*TMat[Xx][Pp];

   EndJacobian[3]=TMat[Zz][Nn];
   EndJacobian[4]=TMat[Zz][Oo];
   EndJacobian[5]=TMat[Zz][Aa];
  }
}

void GetEndInvJacobian()
{
  double Data1[36];
  CvMat Mat1 = cvMat( 6,6,CV_64FC1,Data1);

  double Data2[36];
  CvMat Mat2 = cvMat( 6,6,CV_64FC1,Data2);

  for (int i=0;i<6;i++)
  {
   for (int j=0;j<6;j++)
   {
    cvmSet(&Mat1,i,j,EndJacobian[j]);
   }
  }

  cvInvert(&Mat1,&Mat2,CV_SVD);

     for (i=0;i<6;i++)
  {
   for (int j=0;j<6;j++)
   {
    EndInvJacobian[j]=cvmGet(&Mat2,i,j);
   }
  }
}

void EndOutput(double Input[6], double Output[6])//Output为角速度!
{
  MLMatMulti_3(EndInvJacobian,Input,Output);
}

void GetJBasetoEnd()
{
  double TransMat[4][4];
  MLGetIdentityMat(TransMat);
  TransMat[0][3]=-1*EndPose[0][3];
  TransMat[1][3]=-1*EndPose[1][3];
  TransMat[2][3]=-1*EndPose[2][3];

  MLMatMulti(TransMat,EndPose,T_1to6);

  JBasetoEnd[0][0]=T_1to6[Xx][Nn]; JBasetoEnd[0][1]=T_1to6[Yy][Nn]; JBasetoEnd[0][2]=T_1to6[Zz][Nn];
  JBasetoEnd[1][0]=T_1to6[Xx][Oo]; JBasetoEnd[1][1]=T_1to6[Yy][Oo]; JBasetoEnd[1][2]=T_1to6[Zz][Oo];
  JBasetoEnd[2][0]=T_1to6[Xx][Aa]; JBasetoEnd[2][1]=T_1to6[Yy][Aa]; JBasetoEnd[2][2]=T_1to6[Zz][Aa];

  for (int i=3;i<6;i++)
  {
   for (int j=0;j<3;j++)
   {
    JBasetoEnd[j]=0;
   }
  }

  JBasetoEnd[3][3]=T_1to6[Xx][Nn]; JBasetoEnd[3][4]=T_1to6[Yy][Nn]; JBasetoEnd[3][5]=T_1to6[Zz][Nn];
  JBasetoEnd[4][3]=T_1to6[Xx][Oo]; JBasetoEnd[4][4]=T_1to6[Yy][Oo]; JBasetoEnd[4][5]=T_1to6[Zz][Oo];
  JBasetoEnd[5][3]=T_1to6[Xx][Aa]; JBasetoEnd[5][4]=T_1to6[Yy][Aa]; JBasetoEnd[5][5]=T_1to6[Zz][Aa];

  
  JBasetoEnd[0][3]=T_1to6[Yy][Pp]*T_1to6[Zz][Nn]-T_1to6[Zz][Pp]*T_1to6[Yy][Nn];//(P×N)x
  JBasetoEnd[0][4]=T_1to6[Zz][Pp]*T_1to6[Xx][Nn]-T_1to6[Xx][Pp]*T_1to6[Zz][Nn];//(P×N)y
  JBasetoEnd[0][5]=T_1to6[Xx][Pp]*T_1to6[Yy][Nn]-T_1to6[Yy][Pp]*T_1to6[Xx][Nn];//(P×N)z
  
  JBasetoEnd[1][3]=T_1to6[Yy][Pp]*T_1to6[Zz][Oo]-T_1to6[Zz][Pp]*T_1to6[Yy][Oo];//(P×O)x
  JBasetoEnd[1][4]=T_1to6[Zz][Pp]*T_1to6[Xx][Oo]-T_1to6[Xx][Pp]*T_1to6[Zz][Oo];//(P×O)y
  JBasetoEnd[1][5]=T_1to6[Xx][Pp]*T_1to6[Yy][Oo]-T_1to6[Yy][Pp]*T_1to6[Xx][Oo];//(P×O)z

  JBasetoEnd[2][3]=T_1to6[Yy][Pp]*T_1to6[Zz][Aa]-T_1to6[Zz][Pp]*T_1to6[Yy][Aa];//(P×A)x
  JBasetoEnd[2][4]=T_1to6[Zz][Pp]*T_1to6[Xx][Aa]-T_1to6[Xx][Pp]*T_1to6[Zz][Aa];//(P×A)y
  JBasetoEnd[2][5]=T_1to6[Xx][Pp]*T_1to6[Yy][Aa]-T_1to6[Yy][Pp]*T_1to6[Xx][Aa];//(P×A)z
}

void BaseOutput(double BaseInput[6], double Output[6])//Output为角速度!
{
  double EndInput[6];
  MLMatMulti_3(JBasetoEnd,BaseInput,EndInput);

  EndOutput(EndInput,Output);
}

void SetInput(double Input[6])
{
  for (int i=0;i<6;i++)
  {
   mInput=Input;
  }
}

void SetMode(int mode)
{
  mMode=mode;
}

void GetOutput(int mode=BASE)
{
  int i=0;
  int j=0;
  switch(mode)
  {
  case eND:
   EndOutput(mInput,mOutput);
   break;
   
  case BASE:
   BaseOutput(mInput,mOutput);
   break;
  }
}

RobotJacobian6()
{
  
}

RobotJacobian6(double DHparameter[6][4])
{
  Inti(DHparameter);
}

}MLJacobian;
回复此楼

» 收录本帖的淘帖专辑推荐

机器人技术专辑 蓝袋 机器人学

» 猜你喜欢

» 本主题相关价值贴推荐，对您同样有帮助:
1楼 2010-09-03 11:16:52
已阅   回复此楼   关注TA 给TA发消息 送TA红花 TA的回帖

YBQ大学

禁虫 (正式写手)
★ ★
lovibond(金币+2): 不适合作为资源帖，但是要鼓励原创分享 2011-06-27 21:00:00
本帖内容被屏蔽
2楼2010-09-03 13:26:46
已阅   回复此楼   关注TA 给TA发消息 送TA红花 TA的回帖

想飞的猪

木虫 (正式写手)
引用回帖:
Originally posted by YBQ大学 at 2010-09-03 13:26:46:
运行不会报错吧

你是在哪个平台上写的

这个只是计算没有涉及太多的接口，什么平台都可以吧！而且我不知道你的平台指什么，系统还是开发软件，我的是在XP用VC++6.0写的！

感觉你的编程能力不是很...！
好好学学吧！
赞一下 回复此楼
3楼2010-09-04 12:48:37
已阅   回复此楼   关注TA 给TA发消息 送TA红花 TA的回帖

牛耕地

银虫 (初入文坛)

0.8

顶！好东西！
回复此楼
4楼2011-02-17 15:50:00
已阅   回复此楼   关注TA 给TA发消息 送TA红花 TA的回帖

想飞的猪

木虫 (正式写手)
引用回帖:
Originally posted by 牛耕地 at 2011-02-17 15:50:00:
顶！好东西！

谢谢！以后多交流！我用OpenGL做了个东西！
赞一下 回复此楼
5楼2011-02-17 20:42:08
已阅   回复此楼   关注TA 给TA发消息 送TA红花 TA的回帖

牛耕地

银虫 (初入文坛)
好，我就是做虚拟人动画的，对这个也比较了解。
不过我没有实现过IK的东西，能把你的东西分享一下吗？
引用回帖:
Originally posted by 想飞的猪 at 2011-02-17 20:42:08:



谢谢！以后多交流！我用OpenGL做了个东西！

赞一下 回复此楼
6楼2011-02-18 09:29:02
已阅   回复此楼   关注TA 给TA发消息 送TA红花 TA的回帖

想飞的猪

木虫 (正式写手)
引用回帖:
Originally posted by 牛耕地 at 2011-02-18 09:29:02:
好，我就是做虚拟人动画的，对这个也比较了解。
不过我没有实现过IK的东西，能把你的东西分享一下吗？


IK是什么啊？
回复此楼
7楼2011-02-18 13:27:15
已阅   回复此楼   关注TA 给TA发消息 送TA红花 TA的回帖

牛耕地

银虫 (初入文坛)
inverse kinematics

你计算雅可比不是用来求解逆运动学吗？
引用回帖:
Originally posted by 想飞的猪 at 2011-02-18 13:27:15:


IK是什么啊？

赞一下 回复此楼
8楼2011-02-18 15:07:10
已阅   回复此楼   关注TA 给TA发消息 送TA红花 TA的回帖

想飞的猪

木虫 (正式写手)
引用回帖:
Originally posted by 牛耕地 at 2011-02-18 15:07:10:
inverse kinematics

你计算雅可比不是用来求解逆运动学吗？


哦！呵呵！
回复此楼
9楼2011-02-18 17:47:42
已阅   回复此楼   关注TA 给TA发消息 送TA红花 TA的回帖

ytinghost3

铁杆木虫 (著名写手)
帖子真精彩!
已经收录到淘贴专辑《机器人技术专辑》
赞一下 回复此楼
谎言与誓言的区别在于：一个是听的人当真了，一个是说的人当真了。
10楼2011-09-01 22:14:38
已阅   回复此楼   关注TA 给TA发消息 送TA红花 TA的回帖
相关版块跳转 我要订阅楼主 想飞的猪 的主题更新
141/2返回列表12下一页
普通表情 龙 兔 虎 猫 高级回复 (可上传附件)
最具人气热帖推荐 [查看全部] 作者 回/看 最后发表
[基金申请] 国自然评审意见 +7 wangmingqi 2026-08-28 8/400 2026-08-28 18:42 by zhuzg0628
[基金申请] 国自然面上复盘~欢迎讨论 (金币+15) +13 晴天加油 2026-08-26 14/700 2026-08-28 18:36 by huagongfeihu
[教师之家] 导师吐槽：我怎么摊上了这么个极品研究生！ +8 苏东坡二世 2026-08-23 8/400 2026-08-28 14:41 by lkforward
[基金申请] 基金不中，共勉 +11 eulota 2026-08-26 11/550 2026-08-28 14:22 by 火星超人xi
[基金申请] 哪位高人中了，把查询到的截图贴出来让我看看，让我长长见识 +5 yuleib84 2026-08-26 6/300 2026-08-28 00:02 by yudaoqian88
[基金申请] 申请删除本帖 +6 lyz123lyz 2026-08-27 7/350 2026-08-27 17:31 by 宁静致远sy
[基金申请] 怎么看青基中了没有啊 +5 叶九微 2026-08-26 5/250 2026-08-27 10:35 by l_zh2008
[文学芳草园] 梦想 +3 myrtle 2026-08-26 3/150 2026-08-27 10:01 by angelyueyi
[基金申请] 2026年的国家社科基金项目通讯评审的新规则与新动向、新挑战 +7 process2012 2026-08-23 10/500 2026-08-26 19:23 by hmhminy
[基金申请] 范进中举一文的中心思想 +9 炎黄贵胄 2026-08-22 10/500 2026-08-26 15:40 by semaglutide
[基金申请] 出来了 +9 trojank 2026-08-26 9/450 2026-08-26 14:25 by 宝贝虫子
[基金申请] 国合里面能看到了 +7 一怀馨秋 2026-08-26 7/350 2026-08-26 11:23 by zhaosm1982
[基金申请] 系统进不去 +4 yanglien 2026-08-26 5/250 2026-08-26 11:10 by wenfengw83
[基金申请] 今天务委会开完了，明天出结果吗 +19 angus9576 2026-08-25 23/1150 2026-08-26 10:03 by zp519
[职场人生] 学生撞见辅导员送外卖，第二天全班都沉默了 +3 鲸鱼融金_浙江_� 2026-08-22 3/150 2026-08-26 09:01 by zzuzxg
[基金申请] 牛来！米来！面来！ +8 beefly 2026-08-26 8/400 2026-08-26 08:37 by xuzhipiao
[基金申请] 在坚冰还盖着北海的时候，我看到了怒放的梅花。 (金币+10) +6 ziyangfang 2026-08-25 9/450 2026-08-25 20:26 by huagongfeihu
[基金申请] 明天应该可查了！？ +6 chengyan1220 2026-08-23 6/300 2026-08-25 19:45 by zfd97
[基金申请] 如果此刻你正在为国基感到焦虑，不妨来听听这首《基金之外》 +8 scalable 2026-08-24 8/400 2026-08-25 12:52 by jnhyjjm
[基金申请] 没有任何消息-是不是就凉了 +9 图啦图啦 2026-08-24 10/500 2026-08-25 11:59 by 南海小哥
信息提示
关闭
请填处理意见
关闭