实验一机械臂正逆运动学

实验一机械臂正逆运动学
一、实验目的
1、巩固正逆运动学基础概念。
2、了解正逆运动学在机械臂控制中的实际用途。
二、实验内容
1、机械臂模型DH参数的计算。
2、机械臂正运动学的计算。
3、机械臂逆运动学的计算。
三、实验步骤
1、正运动学实验验证，编写程序计算以下三组关节角度对应的末端位姿，并通过位置控制使机器人运动到相应位置，将实际末端位置与你的计算值比较，求出绝对误差。
第一组：[0.927, -0.687, -0.396, 0, 1.083, 0.927]
第二组：[0.322, -0.855, -0.021, 0, 0.877, 0.322]
第三组：[-0.322, -0.636, -0.011, 0, 0.647, -0.322]
2、逆运动学实验验证，编写程序计算以下三组末端位姿对应的关节角度，并通过位置控制使机器人运动到你计算出的位置，将实际末端位置与所给的值比较，求绝对误差。
第一组：[0.2, 0.2, 0.2007,1.57, -1.57, 0]
第二组：[0.15 ,0.2, 0.2007, 0, 0, 0]
第三组：[0.3 ,0, 0.122, 1.57, 0, 0]
四、评定标准
1、实验结果的准确性
五、注意事项
1、本实验中姿态欧拉角与ROS中一致，全部采用弧度制，坐标全部以米为单位。
2、真实机器人实验中，如果机器人发生碰撞，一定要先扶住机械臂，再按下急停按钮，否则机械臂会倒下。
3、按下急停按钮后需要手动将机械臂复原到门位置，并重置控制内部机械臂位置。
4、真实机械臂实验过程中，按下急停按钮后复位的过程中最好按照原本的运动方向复位，不要让第一关节一直向着一个方向旋转以免造成线路缠绕。
5、机械臂启动时的位置被称为初始门位置（如下图），其末端位姿为[0.2289, 0, 0.454, 1.57, 0, 0]，机械臂各个关节的正方向如图所示。

代码，部分行从助教例程中直接复制，因此部分代码为无用代码，以及代码写了两次，懒得去其进行精简，所以很长。ubuntu带到win的注释乱码，也懒得修改了
正运动学

#include <string>
#include <ros/ros.h>
#include <iostream>
#include <time.h>
#include <Eigen/Dense>
#include <cmath>
#include "vector"
#include "std_msgs/Float32.h"
#include "std_msgs/Float64MultiArray.h"
#include "controller_manager_msgs/SwitchController.h"
#include "controller_manager_msgs/ListControllers.h"
#include "ikfast.h"
#include "probot_anno_manipulator_ikfast_moveit_plugin.cpp"using namespace ikfast_kinematics_plugin;void PrintMatrix(Eigen::Matrix<double,4,4> t){for(int i=0;i<4;i++){cout<<setw(15)<<"|   ";for(int j=0;j<4;j++){cout<<setw(15)<<t(i,j);}cout<<setw(15)<<"   |"<<endl;}cout<<endl;
}int main(int argc, char** argv){bool ret;//鑺傜偣鍒濆鍖?ros::init(argc,argv,"forward_kinematics");//鍒涘缓鑺傜偣鍙ユ焺瀵硅薄ros::NodeHandle node_handle;ros::AsyncSpinner spinner(1);ROS_INFO_STREAM("start");ros::Publisher pos_pub=node_handle.advertise<std_msgs::Float64MultiArray>("/probot_anno/arm_pos_controller/command",100);ikfast_kinematics_plugin::IKFastKinematicsPlugin ik;ret = ik.IKFastKinematicsPlugin::initialize("robot_description","manipulator","base_link","link_6",0.001);std_msgs::Float64MultiArray init_pos;init_pos.data.push_back(0);init_pos.data.push_back(0);init_pos.data.push_back(0);init_pos.data.push_back(0);init_pos.data.push_back(0);init_pos.data.push_back(0);sleep(1);double theta[6];//杈撳叆鍚勫叧鑺傚叧鑺傝for(int i = 0;i<6;i++){cout<<"鍏宠妭瑙?<<i+1<<": ";cin>>theta[i];}theta[6] = 0;theta[7] = 0;init_pos.data.at(0) = theta[0];init_pos.data.at(1) = theta[1];init_pos.data.at(2) = theta[2];init_pos.data.at(3) = theta[3];init_pos.data.at(4) = theta[4];init_pos.data.at(5) = theta[5];
//瀹氫箟DH鍙傛暟const double DH[8][3]={ 0,0,0.284,M_PI/2,0,0,0,0.225,0,M_PI/2,0,0.2289,-M_PI/2,0,0,M_PI/2,0,0,0,0,0.055,-M_PI/2,0,0};Eigen::Matrix<double,4,4> T[8];
//璁＄畻鍙樻崲鐭╅樀for(int i=0;i<8;i++){T[i] << cos(theta[i]),-sin(theta[i]),0,DH[i][1],sin(theta[i])*cos(DH[i][0]),cos(theta[i])*cos(DH[i][0]),-sin(DH[i][0]),-sin(DH[i][0])*DH[i][2],sin(theta[i])*sin(DH[i][0]),cos(theta[i])*sin(DH[i][0]),cos(DH[i][0]),cos(DH[i][0])*DH[i][2],0,0,0,1;}T[1] << cos(theta[1]+M_PI/2),-sin(theta[1]+M_PI/2),0,DH[1][1],sin(theta[1]+M_PI/2)*cos(DH[1][0]),cos(theta[1]+M_PI/2)*cos(DH[1][0]),-sin(DH[1][0]),-sin(DH[1][0])*DH[1][2],sin(theta[1]+M_PI/2)*sin(DH[1][0]),cos(theta[1]+M_PI/2)*sin(DH[1][0]),cos(DH[1][0]),cos(DH[1][0])*DH[1][2],0,0,0,1;   T[4]<< cos(theta[4]-M_PI/2),-sin(theta[4]-M_PI/2),0,DH[4][1],sin(theta[4]-M_PI/2)*cos(DH[4][0]),cos(theta[4]-M_PI/2)*cos(DH[4][0]),-sin(DH[4][0]),-sin(DH[4][0])*DH[4][2],sin(theta[4]-M_PI/2)*sin(DH[4][0]),cos(theta[4]-M_PI/2)*sin(DH[4][0]),cos(DH[4][0]),cos(DH[4][0])*DH[4][2],0,0,0,1;
//璁＄畻鏈浣嶅Э鍙樻崲鐭╅樀Eigen::Matrix<double,4,4> result;result << 1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1;for(int i = 0;i<8;i++){result = result * T[i];//PrintMatrix(result);}
//寰楀埌鏁版嵁double pos_x = result(0,3);double pos_y = result(1,3);double pos_z = result(2,3);double rot_x = atan2(result(2,1),result(2,2));double rot_y = atan2(-result(2,0),sqrt(result(1,0)*result(1,0)+result(0,0)*result(0,0)));double rot_z = atan2(result(1,0),result(0,0));
//杈撳嚭浣嶅Эcout<<"pos_x:"<<pos_x<<endl;cout<<"pos_y:"<<pos_y<<endl;cout<<"pos_z:"<<pos_z<<endl;cout<<"rot_x:"<<rot_x<<endl;cout<<"rot_y:"<<rot_y<<endl;cout<<"rot_z:"<<rot_z<<endl;         pos_pub.publish(init_pos);ROS_INFO_STREAM("published");}

逆运动学

#include <string>
#include <ros/ros.h>
#include <iostream>
#include <time.h>
#include <Eigen/Dense>
#include <math.h>
#include "vector"
#include "std_msgs/Float32.h"
#include "std_msgs/Float64MultiArray.h"
#include "controller_manager_msgs/SwitchController.h"
#include "controller_manager_msgs/ListControllers.h"
#include "ikfast.h"
#include "probot_anno_manipulator_ikfast_moveit_plugin.cpp"using namespace ikfast_kinematics_plugin;//瀹氫箟涓€浜涘嚱鏁颁究浜庢暡鍏紡
//骞虫柟
double squa(double t){return t*t;
}
//cos姹傚拰
double cosp(double theta1, double theta2){return (cos(theta1)*cos(theta2)-sin(theta1)*sin(theta2));
}
//sin姹傚拰
double sinp(double theta1, double theta2){return (sin(theta1)*cos(theta2) + sin(theta2)*cos(theta1));
}double judge(double theta){if(theta > -0.01 && theta < 0.01){return 0;}else return theta;
}int main(int argc, char**argv){bool ret;ros::init(argc,argv,"inverse_kinematics");ros::NodeHandle node_handle;ROS_INFO_STREAM("start");ros::Publisher pos_pub=node_handle.advertise<std_msgs::Float64MultiArray>("/probot_anno/arm_pos_controller/command",100);ikfast_kinematics_plugin::IKFastKinematicsPlugin ik;ret = ik.IKFastKinematicsPlugin::initialize("robot_description","manipulator","base_link","link_6",0.001);double theta[6];double CarCoor[6];//杈撳叆绗涘崱灏斿潗鏍?cout << "pos_x:";cin >> CarCoor[0];cout << "pos_y:";cin >> CarCoor[1];cout << "pos_z:";cin >> CarCoor[2];cout << "rot_x:";cin >> CarCoor[3];cout << "rot_y:";cin >> CarCoor[4];cout << "rot_z:";cin >> CarCoor[5];
//瀹氫箟涓€浜涘彉閲忎究浜庢暡鍏紡double px = CarCoor[0];double py = CarCoor[1];double pz = CarCoor[2];double gamma = CarCoor[3];double beta = CarCoor[4];double alpha = CarCoor[5];
//璁＄畻鍏舵鍙樻崲鐭╅樀Eigen::Matrix<double,3,3> R;R << cos(alpha)*cos(beta),cos(alpha)*sin(beta)*sin(gamma)-sin(alpha)*cos(gamma),cos(alpha)*sin(beta)*cos(gamma)+sin(alpha)*sin(gamma),sin(alpha)*cos(beta),sin(alpha)*sin(beta)*sin(gamma)+cos(alpha)*cos(gamma),sin(alpha)*sin(beta)*cos(gamma)-cos(alpha)*sin(gamma),-sin(beta),cos(beta)*sin(gamma),cos(beta)*cos(gamma);Eigen::Matrix<double,4,4>T0;T0 << R(0,0), R(0,1), R(0,2), CarCoor[0],R(1,0), R(1,1),R(1,2), CarCoor[1],R(2,0), R(2,1), R(2,2), CarCoor[2],0,0,0,1;
//灏嗗潗鏍囩郴浠庣鍏釜鍧愭爣绯昏浆鎹㈠洖瀹氫箟鐨勭鍏釜鍧愭爣绯?Eigen::Matrix<double,4,4>Tw1,Tw2;Tw1 << 1,0,0,0,0,1,0,0,0,0,1,-0.055,0,0,0,1;Tw2 << 1,0,0,0,0,0,-1,0,0,1,0,0,0,0,0,1;
//閫氳繃绗叚涓潗鏍囩郴涓嬬殑鍧愭爣閲嶆柊璁＄畻浣嶅Э绛?T0 = T0 * Tw2 * Tw1;for(int i = 0;i<3;i++){CarCoor[i] = T0(i,3);}CarCoor[3] = atan2(T0(2,1),T0(2,2));CarCoor[4] = atan2(-T0(2,0),sqrt(squa(T0(1,0))+squa(T0(0,0))));CarCoor[5] = atan2(T0(1,0),T0(0,0));px = CarCoor[0];py = CarCoor[1];pz = CarCoor[2];gamma = CarCoor[3];beta = CarCoor[4];alpha = CarCoor[5];R << cos(alpha)*cos(beta),cos(alpha)*sin(beta)*sin(gamma)-sin(alpha)*cos(gamma),cos(alpha)*sin(beta)*cos(gamma)+sin(alpha)*sin(gamma),sin(alpha)*cos(beta),sin(alpha)*sin(beta)*sin(gamma)+cos(alpha)*cos(gamma),sin(alpha)*sin(beta)*cos(gamma)-cos(alpha)*sin(gamma),-sin(beta),cos(beta)*sin(gamma),cos(beta)*cos(gamma);
//DH鍙傛暟琛?
//\alpha_{i-1},a_{i-1},\theta_i,d_iconst double DH[8][4]={0,0,0,0.284,M_PI/2,0,M_PI/2,0,0,0.225,0,0,M_PI/2,0,0,0.2289,-M_PI/2,0,-M_PI/2,0,M_PI/2,0,0,0,0,0,0,0.055,-M_PI/2,0,0,0};
//鎶奃H琛ㄩ噷涓€浜涗細鐢?double d1 = DH[0][3];double d4 = DH[3][3];double a3 = DH[2][1];
//璁＄畻\theta_1theta[0] = judge(atan2(py,px));
//杩樻槸濂芥暡鍏紡double s1 = sin(theta[0]);double c1 = cos(theta[0]);
//璁＄畻\theta_2double K = (squa(d4) - squa(px/c1) - squa(pz - d1) - squa(a3))/(2*a3);theta[1] = atan2(pz-d1, px/c1) - atan2( -K , sqrt(squa(px/c1) + squa(pz - d1) - squa(K)));double s2 = sin(theta[1]);double c2 = cos(theta[1]);
//璁＄畻\theta_3double s23 = (pz - c2 * a3 - d1)/d4;double c23 = ((px/c1)+s2 * a3) / d4;theta[2] = judge(atan2(s23, c23) - theta[1]);double s3 = sin(theta[2]);double c3 = cos(theta[2]);c23 = cosp(theta[1],theta[2]);s23 = sinp(theta[1],theta[2]);
//璁＄畻寰楀埌^4_6T,鏉ヨ绠楀悗涓夎锛孯1涓篰1_0R,绫绘帹Eigen::Matrix<double,3,3> R1,R2,R3,result;R1 << c1, s1, 0,-s1, c1, 0,0, 0, 1;R2 << -s2, 0, c2,-c2, 0, -s2,0, -1, 0;R3 << c3, s3, 0,-s3, c3, 0,0, 0, 1;result = R3 * R2 * R1 * R;
//璁＄畻\theta_5theta[4] = judge(atan2(-result(1,2), sqrt(squa(result(1,0))+squa(result(1,1)))));double s5 = sin(theta[4]);double c5 = cos(theta[4]);
//璁＄畻\theta_4theta[3] = judge( atan2(-result(2,2)/c5, -result(0,2)/c5));double s4 = sin(theta[3]);double c4 = cos(theta[3]);
//璁＄畻\theta_6theta[5] = judge(atan2(result(1,1)/c5,-result(1,0)/c5));
//鎶婅绠楀緱鍒扮殑鍏宠妭瑙掓墦鍑烘潵搴峰悍for(int i =0;i<6;i++){cout<<"theta"<<i+1<<": "<<theta[i]<<endl;}std_msgs::Float64MultiArray init_pos;init_pos.data.push_back(0);init_pos.data.push_back(0);init_pos.data.push_back(0);init_pos.data.push_back(0);init_pos.data.push_back(0);init_pos.data.push_back(0);sleep(1);init_pos.data.at(0) = theta[0];init_pos.data.at(1) = theta[1];init_pos.data.at(2) = theta[2];init_pos.data.at(3) = theta[3];init_pos.data.at(4) = theta[4];init_pos.data.at(5) = theta[5];pos_pub.publish(init_pos);ROS_INFO_STREAM("published");
}

实验一机械臂正逆运动学相关推荐

修正逆解文章——六轴UR机械臂正逆运动学求解_MATLAB代码（标准DH参数表）
如下参考链接1的作者大大实现了UR5机械臂的正运动学和逆运动学的Matlab代码.但逆解部分在不同版本的Matlab中运行有错误. 本篇文章是MatlabR2016a下完成的,并说明一下原代码错误的原 ...
UR5构型机械臂正逆运动学
前言整理之前的一个项目,当时看着一个博客硬生生计算了差不多一个星期.尝试用MatLab符号推导工具箱化简一部分工作.我使用的大象机器人一款开源入门级协作机器人产品myCobot,开发文档十分完善,但 ...
UR机械臂正逆运动学求解
最近有个任务:求解UR机械臂正逆运动学,在网上参考了一下大家的求解办法,众说纷纭,其中有些朋友求解过程非常常规,但是最后求解的8组解,只有4组可用.在这里我介绍一个可以求解8组解析解的方法,供大家参考 ...
机械臂正逆运动学-----数值解
机械臂正逆运动学-----数值解建立DH坐标系求正运动学单关节齐次传递矩阵正运动学:返回齐次矩阵正运动学:返回欧拉角向量求雅可比矩阵求机械臂逆运动学合成通用运动学类机械臂的运动学包括 ...
ROS机械臂正逆运动学
这里做一个六轴机械臂用于正逆运动学实验. 这里其实一共只有3轴,只有3轴位置没有姿态.所以urdf文件里我在末端做了3个虚拟关节,以便将kdl的frame能够填满,使得齐次坐标变换是规则的. 1.ur ...
五自由度机械臂正逆运动学算法（Ｃ语言+Matlab）
五自由度机械臂建模学习代码都记录在个人github上,欢迎关注~ Matlab机器人工具箱版本9.10 机械臂还是原来的机械臂,之前用ROS做底层驱动,不需要写正逆运动学和相关算法就能得到一些简单的 ...
用matlab实现机械臂正逆运动学控制
设计要求: 1.建立一个三自由度的机器人 2.建立坐标系,给出 D-H 参数表: 3.推导正运动学,并写出机器人的齐次变换矩阵: 4.推导逆运动学,并让机器人完成按要求绘制给定图形. 5.MATLAB ...
matlab欧拉迭代,matlab机械臂正逆运动学求解问题，使用牛顿-欧拉迭代算法
代码复制的有问题,详细见本楼,谢谢. clc;clear; DR=pi/180; %time j = 1; for i = 0 : 0.1 : 2 %input theta= 45 * DR *(1+ ...
6轴机械臂正逆解运算实现
6轴机械臂正逆解运算实现利用Gluon-6L3机械臂模型的参数,对机械臂进行运动学分析. 这里采用标准DH坐标系,并将d6设置为0,方便后续计算. 首先,SDH的变换矩阵为: ii−1T=Ai=^{ ...

实验一机械臂正逆运动学

实验一机械臂正逆运动学相关推荐

最新文章

热门文章

实验一 机械臂正逆运动学

实验一 机械臂正逆运动学相关推荐

最新文章

热门文章

实验一机械臂正逆运动学

实验一机械臂正逆运动学相关推荐