四足机器狗：C++ 验证运动学，逆运动学。

头文件

#pragma once
#include <iostream>
#include <cmath>
using namespace std;

/*
建立机器人坐标 注：以右前腿为例，电机ID分别为0,1,2

坐标系建立 以右前腿，按改进DH参数方法（坐标系原点建立在连杆的起始端）

建立0（电机0到1轴交点）,1（初始坐标与0坐标重合）2（1电机轴线与足点竖直重力方向交点）3（膝关节）

内部角度统一为弧度

*/
#define PI           3.14159265358979323846
#define RD(radian)   ((radian) * (180.0 / M_PI)) //弧度转角度
#define DR(degree)   ((degree) * (M_PI / 180.0)) //角度转弧度

//测量参数：轴线之间的距离（公垂线距离）  单位（mm）
//小腿到轮轴线距离：150
#define m_d1  80  //坐标系1到坐标系2的连杆偏距
#define m_l1  200  //大腿长
#define m_l2  180  //小腿长

//硬限位，按坐标建立的正负向，面朝Z轴指向时，逆时针为 正，单位(弧度)

/*参考值，来源github- aliengo*/
//constexpr double a1_Hip_max = 0.802;    // unit:radian ( = 46   degree)
//constexpr double a1_Hip_min = -0.802;   // unit:radian ( = -46  degree)
//constexpr double a1_Thigh_max = 4.19;     // unit:radian ( = 240  degree)
//constexpr double a1_Thigh_min = -1.05;    // unit:radian ( = -60  degree)
//constexpr double a1_Calf_max = -0.916;   // unit:radian ( = -52.5  degree)
//constexpr double a1_Calf_min = -2.7;     // unit:radian ( = -154.5 degree)
/**/
#define theta1_hard_plus_lim  50/180*PI  //关节角硬限位
#define theta1_hard_lose_lim  -45/180*PI  //关节角硬限位
#define theta2_hard_plus_lim  45/180*PI //关节角硬限位
#define theta2_hard_lose_lim  -235/180*PI  //关节角硬限位
#define theta3_hard_plus_lim  155/180*PI  //关节角硬限位
#define theta3_hard_lose_lim  45/180*PI //关节角硬限位

//软限位，按坐标建立的正负向，面朝Z轴指向时，逆时针为 正
#define theta1_soft_plus_lim      42/180*PI  //关节角软限位
#define theta1_soft_lose_lim     -40/180*PI   //关节角软限位
#define theta2_soft_plus_lim      35/180*PI  //关节角软限位
#define theta2_soft_lose_lim    -220/180*PI   //关节角软限位
#define theta3_soft_plus_lim     150/180*PI   //关节角软限位
#define theta3_soft_lose_lim      45/180*PI   //关节角软限位

struct JointAngelData
{
	double theta1; //关节角1
	double theta2;
	double theta3;
};

class DogModel
{
public:
	DogModel();
	~DogModel();
	//double get_theta1();
	//double get_theta2();
	//double get_theta3();

	/*获取关节角度（弧度）结构体*/
	JointAngelData get_Joint_theta();

	/*足点到{0}的，运动学(弧度0，角度1)*/
	void kine_04(bool Rad0_Deg1, const double & ct1, const double & ct2, const double & ct3);

	/*足点笛卡尔映射到三个关节，逆运动学*/
	void ikine_04(const double& px, const double& py, const double& pz);
	/*速度雅克比*/
	void vel_jacobian();
	/*力雅克比*/
	void F_jacobian();

	/*建立机器人参数（弧度）*/
	double theta1; //关节角1
	double theta2;
	double theta3;
	
	double car0X; //笛卡尔三个坐标位置
	double car0Y;
	double car0Z;


private:
};

---

源文件

 

#include "dogModel.h"



DogModel::DogModel()
{
	this->theta1 = 0;
	this->theta2 = 0;
	this->theta3 = 0;
}

DogModel::~DogModel()
{
}

JointAngelData DogModel::get_Joint_theta()
{
	JointAngelData data;
	data.theta1 = this->theta1;
	data.theta2 = this->theta2;
	data.theta3 = this->theta3;
	return data;
}


/*足点到{0}的，运动学(角度输入)*/
void DogModel::kine_04(bool Rad0_Deg1,const double &ct1, const double& ct2, const double& ct3)
{
	double t1 = ct1;
	double t2 = ct2;
	double t3 = ct3;
    //输入关节角度，求出足点的相对于肩部{0}坐标的笛卡尔位置，姿态暂不考虑
	if (Rad0_Deg1)
	{
		/*先转角度为弧度*/
		t1 = ct1 / 180.0 * PI;//最好是180.0，防止整数相除，不如预期
		t2 = ct2 / 180.0 * PI;
		t3 = ct3 / 180.0 * PI;
		/**/
	}
	/*正运动学*/
	this->car0X = m_l1 * cos(t1)* cos(t2) + m_l2 * cos(t1) * cos(t2 + t3) + m_d1 * sin(t1);
	this->car0Y = m_l1 * sin(t1) * cos(t2) + m_l2 * sin(t1) * cos(t2 + t3) - m_d1 * cos(t1);
	this->car0Z = m_l1 * sin(t2) + m_l2 * sin(t2+t3);
	/**/
	cout << "{4}基于{0}的笛卡尔：car0X.car0Y,car0Z = " << endl;
	cout << this->car0X << " " << this->car0Y << " "<< this->car0Z << endl;
}

/*足点笛卡尔映射到三个关节，逆运动学*/
void DogModel::ikine_04(const double& px, const double& py, const double& pz)
{
	this->theta3 = acos((px * px + py * py + pz * pz - m_l1 * m_l1 - m_l2 * m_l2 - m_d1 * m_d1) / (2 * m_l1 * m_l2));
	this->theta2 = asin(pz / (sqrt(m_l1 * m_l1 + m_l2 * m_l2 + 2 * m_l1 * m_l2 * cos(theta3)))) - atan((m_l2 * sin(theta3)) / (m_l1 + m_l2 * cos(theta3)));
	this->theta1 = asin(m_d1 / (sqrt(px * px + py * py))) + atan(py / px);

	cout << "基于{4}的笛卡尔得出关节位置(弧度)：theta1.theta2,theta3 = " << endl;
	cout << theta1 << " " << theta2 << " " << theta3 << endl;
	cout << "基于{4}的笛卡尔得出关节位置(角度)：theta1.theta2,theta3 = " << endl;
	cout << theta1/PI*180 << " " << theta2 / PI * 180 << " " << theta3 / PI * 180 << endl;
}

---

Main函数

#include <iostream>
using namespace std;
#include "dogModel.h"

int main()
{
	DogModel dogModel;
	dogModel.kine_04(true,24, -120, 108);
	dogModel.ikine_04(128, -52, -214);
	JointAngelData curjoint = dogModel.get_Joint_theta();
	cout << curjoint.theta1 << endl;
	cout << curjoint.theta2 << endl;
	cout << curjoint.theta3 << endl;
	cout << dogModel.theta3 << endl;




	system("pause");
	return 0;
}