UR机械臂正逆解

1. 建模

// 定义UR5的kinematic chain
Chain ur5_chain;

// 添加UR5的各个连杆和关节
ur5_chain.addSegment(Segment(Joint(Joint::RotZ), Frame::DH(0.0, M_PI/2, 0.089159, 0.0)));
ur5_chain.addSegment(Segment(Joint(Joint::RotZ), Frame::DH(-0.425, 0.0, 0.0, 0.0)));
ur5_chain.addSegment(Segment(Joint(Joint::RotZ), Frame::DH(-0.39225, 0.0, 0.0, 0.0)));
ur5_chain.addSegment(Segment(Joint(Joint::RotZ), Frame::DH(0.0, M_PI/2, 0.10915, 0.0)));
ur5_chain.addSegment(Segment(Joint(Joint::RotZ), Frame::DH(0.0, -M_PI/2, 0.09465, 0.0)));
ur5_chain.addSegment(Segment(Joint(Joint::RotZ), Frame::DH(0.0, 0.0, 0.0823, 0.0)));

2. 正解

// 创建正向运动学求解器
KDL::ChainFkSolverPos_recursive fk_solver(ur5_chain);

// 定义关节角度
KDL::JntArray joint_angles(6);
joint_angles(0) = KDL::PI / 180.0 * jointAngles[0];
joint_angles(1) = KDL::PI / 180.0 * jointAngles[1];
joint_angles(2) = KDL::PI / 180.0 * jointAngles[2];
joint_angles(3) = KDL::PI / 180.0 * jointAngles[3];
joint_angles(4) = KDL::PI / 180.0 * jointAngles[4];
joint_angles(5) = KDL::PI / 180.0 * jointAngles[5];

// 计算末端执行器的位置
KDL::Frame end_effector_pose;
fk_solver.JntToCart(joint_angles, end_effector_pose);

// 打印末端执行器的位置和姿态
Eigen::Matrix3d m;
m << end_effector_pose.M.data[0], end_effector_pose.M.data[1], end_effector_pose.M.data[2],
	end_effector_pose.M.data[3], end_effector_pose.M.data[4], end_effector_pose.M.data[5],
	end_effector_pose.M.data[6], end_effector_pose.M.data[7], end_effector_pose.M.data[8];

auto v = ConvertRotationMatrixToRotationVector(m);
cout << "Pose : "<< end_effector_pose.p[0]<<", "<< end_effector_pose.p[1]<<", "<< end_effector_pose.p[2]<<", "<< v[0]<<", "<< v[1]<<", "<< v[2]<<endl;

3. 逆解

Eigen::Vector3d rotVec;
rotVec << pose[3], pose[4], pose[5];

auto m = ConvertRotationVectorToRotationMatrix(rotVec);

KDL::Vector   v(pose[0], pose[1], pose[2]);
KDL::Rotation r(
	m(0, 0), m(0, 1), m(0, 2),
	m(1, 0), m(1, 1), m(1, 2),
	m(2, 0), m(2, 1), m(2, 2));

KDL::Frame targetFrame(r, v);

KDL::JntArray q(pImpl->mChain->getNrOfJoints());
KDL::JntArray q_init(pImpl->mChain->getNrOfJoints());
KDL::JntArray q_sol(pImpl->mChain->getNrOfJoints());

KDL::ChainIkSolverPos_LMA solver(*(pImpl->mChain));
   solver.CartToJnt(q_init, targetFrame, q_sol);

cout << q_sol(0) / KDL::PI * 180.0<<", "<< q_sol(1) / KDL::PI * 180.0<<", "<< q_sol(2) / KDL::PI * 180.0<<", "<< q_sol(3) / KDL::PI * 180.0<<", "<< q_sol(4) / KDL::PI * 180.0<<", "<< q_sol(5) / KDL::PI * 180.0<<endl;

运行结果：
以关节参数（10,20,30,40,50,60）运行，计算正逆解