辨识

古城微笑少年丶 2022-12-23 03:30 34阅读 0赞

# main函数 #

#include "pareIdentify.h"
    
    int main(int *argc,char **argv)
    {
    	double  *resul1;
    	double  *resul2;
    	paraIdenti_("D://rawdata.csv",&resul1,&resul2);
    	for (int i = 0;i< 24;i++)
    	{
    		printf("resul1:%d\t%.10lf\n", i + 1, resul1[i]);
    	}
    	printf("\n");
    	for (int i = 0; i < 34; i++)
    	{
    		printf("resul2:%d\t%.10lf\n", i + 1, resul2[i]);
    	}
    	free(resul1);
    	free(resul2);
    	getchar();
    	return 0;
    }

# 头文件 #

#pragma once
    #define _CRT_SECURE_NO_WARNINGS
    #include "stdio.h"
    #include "stdlib.h"
    #include "math.h"
    #include "string.h"
    
    //矩阵相乘；
    void mutipleMatrix(double **res1, double **res2, double ***desMatix1, int rows_1, int columns_1, int rows_2, int columns_2, int type);
    //矩阵转置；
    void matrixTranspose(double **res, double ***des, int rows_, int columns_);
    //初等行变换方法求逆矩阵
    void Matrix_Inv(double **naturalmat/*out*/, int num, double **InvMat/*out*/);
    //求矩阵最小值；
    double cal_min_array(double *res, int len_);
    //构造对角矩阵;
    void diagMatrix(double *resMatrix, double ***desMatrix1, int len);
    //合并构造对角矩阵；
    void blkdiagMatrix(double **oneMatrix, double **twoMatric, int len_1, int len_2, double ***desMatrix1);
    //参数识别para1：excel地址，para2：接收结果1的空一级指针地址；para3：接收结果2的空指针地址
    void paraIdenti_(char *fileName,double **result1/*in*/, double **result2/*in*/);

# 实现 #

#include "pareIdentify.h"
    
    void mutipleMatrix(double ** res1, double ** res2, double *** desMatix1, int rows_1, int columns_1, int rows_2, int columns_2, int type)
    { 
    		if (res1 == NULL || res2 == NULL)
    		{ 
    			printf("res1 or res2 error\n");
    			return;
    		}
    		if (columns_1 != rows_2)
    		{ 
    			printf("You can't multiply these two matrices\n");
    			return;
    		}
    		double **desMatix = (double **)calloc(rows_1, sizeof(double *));
    		if (desMatix == NULL)
    		{ 
    			printf("calloc desMatric error\n");
    			return;
    		}
    		for (int i = 0; i < rows_1; i++)
    		{ 
    			desMatix[i] = (double *)calloc(columns_2, sizeof(double));
    		}
    		if (type == 0)
    		{ 
    			for (int i = 0; i < rows_1; i++)
    			{ 
    				for (int j = 0; j < columns_2; j++)
    				{ 
    					double sum = 0;
    					for (int m = 0; m < columns_1; m++)
    					{ 
    						sum += res1[i][m] * res2[m][j]; 
    					}
    					desMatix[i][j] = sum;
    
    				}
    			}
    		}
    		else if (type == 1)
    		{ 
    			for (int i = 0; i < rows_1; i++)
    			{ 
    				for (int j = 0; j < columns_2; j++)
    				{ 
    					double sum = 0;
    					for (int m = 0; m < columns_1; m++)
    					{ 
    						/*printf("\n*************%lf\n", (*((double *)res1 + i*columns_1 + m))); printf("\n*************%lf\n", (*((double *)res2 + m*columns_2 + j)));*/
    						sum += res1[i][m] * (*((double *)res2 + m*columns_2 + j));; /*(*((double *)res1 + i*columns_1 + m)) * (*((double *)res2 + m*columns_2 + j));*/
    					}
    					desMatix[i][j] = sum;
    
    				}
    			}
    		}
    		else if (type == 2)
    		{ 
    			for (int i = 0; i < rows_1; i++)
    			{ 
    				for (int j = 0; j < columns_2; j++)
    				{ 
    					double sum = 0;
    					for (int m = 0; m < columns_1; m++)
    					{ 
    						sum += (*((double *)res1 + i*columns_1 + m)) * res2[m][j];; /*(*((double *)res1 + i*columns_1 + m)) * (*((double *)res2 + m*columns_2 + j));*/
    					}
    					desMatix[i][j] = sum;
    
    				}
    			}
    		}
    
    		*desMatix1 = desMatix;
    		desMatix = NULL;
    		return;
    }
    
    void matrixTranspose(double ** res, double *** des, int rows_, int columns_)
    { 
    	double **desMatric = (double **)calloc(columns_, sizeof(double *));
    	if (des == NULL)
    	{ 
    		printf("calloc in matrixTranspose error\n");
    		return;
    	}
    	for (int i = 0; i < columns_; i++)
    	{ 
    		desMatric[i] = (double *)calloc(rows_, sizeof(double));
    	}
    	for (int temprows = 0; temprows < columns_; temprows++)
    	{ 
    		for (int tempcolumns = 0; tempcolumns < rows_; tempcolumns++)
    		{ 
    			desMatric[temprows][tempcolumns] = *((double *)res + tempcolumns*columns_ + temprows);
    		}
    	}
    	*des = desMatric;
    	desMatric = NULL;
    	return;
    }
    
    void Matrix_Inv(double ** naturalmat, int num, double ** InvMat)
    { 
    	int i, j, k;
    	double **MatEnhanced;//增广矩阵(A|E)
    	MatEnhanced = (double**)malloc(num * sizeof(double*));
    	for (i = 0; i < num; i++)
    		MatEnhanced[i] = (double*)malloc(2 * num * sizeof(double));
    	double *temp;
    	temp = (double*)malloc(2 * num * sizeof(double));
    	double xishu = 1;//初等变换时系数，设初值为1
    	for (i = 0; i < num; i++)                           //增广矩阵赋值，前半部分
    	{ 
    		for (j = 0; j < num; j++)
    			MatEnhanced[i][j] = naturalmat[i][j];
    	}
    	for (i = 0; i < num; i++)                           //增广矩阵赋值，后半部分
    	{ 
    		for (j = num; j < 2 * num; j++)
    			MatEnhanced[i][j] = 0;//先将后半部分全部赋值为0
    		MatEnhanced[i][i + num] = 1;//再将其对角线部分赋值为1
    	}
    	//接下来进行初等行变换
    	for (i = 0; i < num; i++)
    	{ 
    		if (MatEnhanced[i][i] == 0)//如果前半部分的对角线上的元素为0，此时进行行变换
    		{ 
    			if (i == num - 1)//如果是最后一行，那么说明该矩阵不可逆
    				return;
    			//对第i行以后的各行进行判断，找到第i个元素不为零的行，并与第i行进行交换
    			for (j = i; j < num; j++)
    			{ 
    				if (MatEnhanced[j][i] != 0)
    				{ 
    					k = j;//记住该行的行号
    					break;//退出循环
    				}
    			}
    			//接下来对第i行和第k行进行交换
    			temp = MatEnhanced[k];//第k行
    			MatEnhanced[k] = MatEnhanced[i];
    			MatEnhanced[i] = temp;
    		}
    		//初等变换
    		for (j = 0; j < num; j++)//对其他行的所有列进行计算
    		{ 
    			if (j != i)//本行不参与计算
    			{ 
    				if (MatEnhanced[j][i] != 0)//只有当其不为零时进行计算，否则不计算
    				{ 
    					xishu = MatEnhanced[j][i] / MatEnhanced[i][i];
    					for (k = i; k < 2 * num; k++)//对后面的所有列进行计算
    					{ 
    						MatEnhanced[j][k] -= xishu*MatEnhanced[i][k];
    					}
    				}
    			}
    		}
    		//将本行所有列都除以对角线上的值，将前半部分化成单位矩阵
    		xishu = MatEnhanced[i][i];
    		for (j = i; j < 2 * num; j++)
    			if (xishu != 0)
    				MatEnhanced[i][j] /= xishu;
    	}
    	//计算完成后，后半部分即为原矩阵的逆矩阵，将其赋值给InvMat.
    	for (i = 0; i < num; i++)
    	{ 
    		for (j = 0; j < num; j++)
    			InvMat[i][j] = MatEnhanced[i][j + num];
    	}
    	//内存释放
    	free(MatEnhanced);
    	free(temp);
    	return;//返回
    }
    
    double cal_min_array(double * res, int len_)
    { 
    	double min = res[0];
    	for (int i = 1; i < len_; i++)
    	{ 
    		if (min > res[i])
    		{ 
    			min = res[i];
    		}
    	}
    	return min;
    }
    
    void diagMatrix(double * resMatrix, double *** desMatrix1, int len)
    { 
    	int rows_columns = len;//方阵维数；
    	double **desMatrix = (double **)calloc(rows_columns, sizeof(double *));
    	if (desMatrix == NULL)
    	{ 
    		printf("malloc dial_biaocha_ffinv1 error;");
    		return;
    	}
    	for (int i = 0; i < rows_columns; i++)
    	{ 
    		desMatrix[i] = (double *)calloc(rows_columns, sizeof(double));
    	}
    	for (int i = 0; i < rows_columns; i++)
    	{ 
    		for (int j = 0; j < rows_columns; j++)
    		{ 
    			if (i == j)
    			{ 
    				desMatrix[i][j] = resMatrix[i];
    			}
    		}
    	}
    	*desMatrix1 = desMatrix;
    }
    
    void blkdiagMatrix(double ** oneMatrix, double ** twoMatric, int len_1, int len_2, double *** desMatrix1)
    { 
    	int rows_columns3 = len_1 + len_2;//计算方阵维数；
    	double **desMatrix = (double **)calloc(rows_columns3, sizeof(double *));
    	if (desMatrix == NULL)
    	{ 
    		printf("malloc blkdiagMatrix error;");
    		return;
    	}
    	for (int i = 0; i < rows_columns3; i++)
    	{ 
    		desMatrix[i] = (double*)calloc(rows_columns3, sizeof(double));
    	}
    	for (int i = 0; i < rows_columns3; i++)
    	{ 
    		for (int j = 0; j < rows_columns3; j++)
    		{ 
    			if (i == j)
    			{ 
    				if (i < len_1)
    				{ 
    					desMatrix[i][j] = oneMatrix[i][j];
    				}
    				else
    				{ 
    					desMatrix[i][j] = twoMatric[i - len_1][j - len_1];
    				}
    
    			}
    		}
    	}
    	*desMatrix1 = desMatrix;
    }
    
    void paraIdenti_(char *fileName, double **result1, double **result2)
    { 
    	FILE *fp = NULL;
    	fp = fopen(fileName, "r");
    	if (fp == NULL)
    	{ 
    		printf("open fileName failure OR rows or columns is Invalid");
    		return;
    	}
    	int rows = 8000, columns = 18;
    	float **dataAll_ = NULL;
    	dataAll_ = (float **)malloc(sizeof(float *) * rows);
    	if (dataAll_ == NULL)
    	{ 
    		printf("malloc dataAll_ error;");
    		return;
    	}
    	for (int i = 0; i < rows; i++)
    	{ 
    		dataAll_[i] = (float*)malloc(sizeof(float) * columns);
    	}
    
    	fseek(fp, 0, SEEK_SET); //刚开始将指针指向头； 
    	for (int i = 0; i < rows; i++) //获取excel中 8000*18 数据；存入到data中
    	{ 
    		for (int j = 0; j < columns; j++)
    		{ 
    			fscanf(fp, "%f", &dataAll_[i][j]);
    			fseek(fp, 1L, SEEK_CUR);
    		}
    	}
    	fclose(fp);
    	double POS123_1_J1[8000];
    	double TOR123_1_J1[8000];
    	double TOR123_1_J2[8000];
    	double TOR123_1_J3[8000];
    	double POS123_2_J1[8000];
    	double TOR123_2_J1[8000];
    	double TOR123_2_J2[8000];
    	double TOR123_2_J3[8000];
    	double POS456_1_J1[8000];
    	double TOR456_1_J1[8000];
    	double TOR456_1_J4[8000];
    	double TOR456_1_J5[8000];
    	double TOR456_1_J6[8000];
    	double POS456_2_J1[8000];
    	double TOR456_2_J1[8000];
    	double TOR456_2_J4[8000];
    	double TOR456_2_J5[8000];
    	double TOR456_2_J6[8000];
    	memset(POS123_1_J1, 0, 8000 * sizeof(double));
    	memset(TOR123_1_J1, 0, 8000 * sizeof(double));
    	memset(TOR123_1_J2, 0, 8000 * sizeof(double));
    	memset(TOR123_1_J3, 0, 8000 * sizeof(double));
    	memset(POS123_2_J1, 0, 8000 * sizeof(double));
    	memset(TOR123_2_J1, 0, 8000 * sizeof(double));
    	memset(TOR123_2_J2, 0, 8000 * sizeof(double));
    	memset(TOR123_2_J3, 0, 8000 * sizeof(double));
    	memset(POS456_1_J1, 0, 8000 * sizeof(double));
    	memset(TOR456_1_J1, 0, 8000 * sizeof(double));
    	memset(TOR456_1_J4, 0, 8000 * sizeof(double));
    	memset(TOR456_1_J5, 0, 8000 * sizeof(double));
    	memset(TOR456_1_J6, 0, 8000 * sizeof(double));
    	memset(POS456_2_J1, 0, 8000 * sizeof(double));
    	memset(TOR456_2_J1, 0, 8000 * sizeof(double));
    	memset(TOR456_2_J4, 0, 8000 * sizeof(double));
    	memset(TOR456_2_J5, 0, 8000 * sizeof(double));
    	memset(TOR456_2_J6, 0, 8000 * sizeof(double));
    
    	for (int i = 0; i < 8000; i++)
    	{ 
    		POS123_1_J1[i] = dataAll_[i][0];
    		TOR123_1_J1[i] = dataAll_[i][1];
    		TOR123_1_J2[i] = dataAll_[i][2];
    		TOR123_1_J3[i] = dataAll_[i][3];
    		POS123_2_J1[i] = dataAll_[i][4];
    		TOR123_2_J1[i] = dataAll_[i][5];
    		TOR123_2_J2[i] = dataAll_[i][6];
    		TOR123_2_J3[i] = dataAll_[i][7];
    		POS456_1_J1[i] = dataAll_[i][8];
    		TOR456_1_J1[i] = dataAll_[i][9];
    		TOR456_1_J4[i] = dataAll_[i][10];
    		TOR456_1_J5[i] = dataAll_[i][11];
    		TOR456_1_J6[i] = dataAll_[i][12];
    		POS456_2_J1[i] = dataAll_[i][13];
    		TOR456_2_J1[i] = dataAll_[i][14];
    		TOR456_2_J4[i] = dataAll_[i][15];
    		TOR456_2_J5[i] = dataAll_[i][16];
    		TOR456_2_J6[i] = dataAll_[i][17];
    	}
    	//free dataAll_
    	if (dataAll_ != NULL)
    	{ 
    		for (int i = 0; i < 8000; i++)
    		{ 
    			free(dataAll_[i]);
    		}
    		free(dataAll_);
    	}
    	typedef double double_Array[1000];
    	double_Array POS123_1_J1_aver;
    	double_Array POS123_2_J1_aver;
    	double_Array POS456_1_J1_aver;
    	double_Array POS456_2_J1_aver;
    	double_Array TOR123_1_J1_aver;
    	double_Array TOR123_1_J2_aver;
    	double_Array TOR123_1_J3_aver;
    	double_Array TOR123_2_J1_aver;
    	double_Array TOR123_2_J2_aver;
    	double_Array TOR123_2_J3_aver;
    	double_Array TOR456_1_J1_aver;
    	double_Array TOR456_1_J4_aver;
    	double_Array TOR456_1_J5_aver;
    	double_Array TOR456_1_J6_aver;
    	double_Array TOR456_2_J1_aver;
    	double_Array TOR456_2_J4_aver;
    	double_Array TOR456_2_J5_aver;
    	double_Array TOR456_2_J6_aver;
    	memset(POS123_1_J1_aver, 0, 1000 * sizeof(double));
    	memset(POS123_2_J1_aver, 0, 1000 * sizeof(double));
    	memset(POS456_1_J1_aver, 0, 1000 * sizeof(double));
    	memset(POS456_2_J1_aver, 0, 1000 * sizeof(double));
    	memset(TOR123_1_J1_aver, 0, 1000 * sizeof(double));
    	memset(TOR123_1_J2_aver, 0, 1000 * sizeof(double));
    	memset(TOR123_1_J3_aver, 0, 1000 * sizeof(double));
    	memset(TOR123_2_J1_aver, 0, 1000 * sizeof(double));
    	memset(TOR123_2_J2_aver, 0, 1000 * sizeof(double));
    	memset(TOR123_2_J3_aver, 0, 1000 * sizeof(double));
    	memset(TOR456_1_J1_aver, 0, 1000 * sizeof(double));
    	memset(TOR456_1_J4_aver, 0, 1000 * sizeof(double));
    	memset(TOR456_1_J5_aver, 0, 1000 * sizeof(double));
    	memset(TOR456_1_J6_aver, 0, 1000 * sizeof(double));
    	memset(TOR456_2_J1_aver, 0, 1000 * sizeof(double));
    	memset(TOR456_2_J4_aver, 0, 1000 * sizeof(double));
    	memset(TOR456_2_J5_aver, 0, 1000 * sizeof(double));
    	memset(TOR456_2_J6_aver, 0, 1000 * sizeof(double));
    
    	double_Array POS123_1_J1_std;
    	double_Array POS123_2_J1_std;
    	double_Array POS456_1_J1_std;
    	double_Array POS456_2_J1_std;
    	double_Array TOR123_1_J1_std;
    	double_Array TOR123_1_J2_std;
    	double_Array TOR123_1_J3_std;
    	double_Array TOR123_2_J1_std;
    	double_Array TOR123_2_J2_std;
    	double_Array TOR123_2_J3_std;
    	double_Array TOR456_1_J1_std;
    	double_Array TOR456_1_J4_std;
    	double_Array TOR456_1_J5_std;
    	double_Array TOR456_1_J6_std;
    	double_Array TOR456_2_J1_std;
    	double_Array TOR456_2_J4_std;
    	double_Array TOR456_2_J5_std;
    	double_Array TOR456_2_J6_std;
    	memset(POS123_1_J1_std, 0, 1000 * sizeof(double));
    	memset(POS123_2_J1_std, 0, 1000 * sizeof(double));
    	memset(POS456_1_J1_std, 0, 1000 * sizeof(double));
    	memset(POS456_2_J1_std, 0, 1000 * sizeof(double));
    	memset(TOR123_1_J1_std, 0, 1000 * sizeof(double));
    	memset(TOR123_1_J2_std, 0, 1000 * sizeof(double));
    	memset(TOR123_1_J3_std, 0, 1000 * sizeof(double));
    	memset(TOR123_2_J1_std, 0, 1000 * sizeof(double));
    	memset(TOR123_2_J2_std, 0, 1000 * sizeof(double));
    	memset(TOR123_2_J3_std, 0, 1000 * sizeof(double));
    	memset(TOR456_1_J1_std, 0, 1000 * sizeof(double));
    	memset(TOR456_1_J4_std, 0, 1000 * sizeof(double));
    	memset(TOR456_1_J5_std, 0, 1000 * sizeof(double));
    	memset(TOR456_1_J6_std, 0, 1000 * sizeof(double));
    	memset(TOR456_2_J1_std, 0, 1000 * sizeof(double));
    	memset(TOR456_2_J4_std, 0, 1000 * sizeof(double));
    	memset(TOR456_2_J5_std, 0, 1000 * sizeof(double));
    	memset(TOR456_2_J6_std, 0, 1000 * sizeof(double));
    	计算aver;
    	for (int j = 0; j < 1000; j++)
    	{ 
    		POS123_1_J1_aver[j] = (POS123_1_J1[j] + POS123_1_J1[j + 1000] + POS123_1_J1[j + 2000] + POS123_1_J1[j + 3000] + POS123_1_J1[j + 4000] + POS123_1_J1[j + 5000] + POS123_1_J1[j + 6000] + POS123_1_J1[j + 7000] / 8);
    		POS123_2_J1_aver[j] = (POS123_2_J1[j] + POS123_2_J1[j + 1000] + POS123_2_J1[j + 2000] + POS123_2_J1[j + 3000] + POS123_2_J1[j + 4000] + POS123_2_J1[j + 5000] + POS123_2_J1[j + 6000] + POS123_2_J1[j + 7000] / 8);
    		POS456_1_J1_aver[j] = (POS456_1_J1[j] + POS456_1_J1[j + 1000] + POS456_1_J1[j + 2000] + POS456_1_J1[j + 3000] + POS456_1_J1[j + 4000] + POS456_1_J1[j + 5000] + POS456_1_J1[j + 6000] + POS456_1_J1[j + 7000] / 8);
    		POS456_2_J1_aver[j] = (POS456_2_J1[j] + POS456_2_J1[j + 1000] + POS456_2_J1[j + 2000] + POS456_2_J1[j + 3000] + POS456_2_J1[j + 4000] + POS456_2_J1[j + 5000] + POS456_2_J1[j + 6000] + POS456_2_J1[j + 7000] / 8);
    		TOR123_1_J1_aver[j] = (TOR123_1_J1[j] + TOR123_1_J1[j + 1000] + TOR123_1_J1[j + 2000] + TOR123_1_J1[j + 3000] + TOR123_1_J1[j + 4000] + TOR123_1_J1[j + 5000] + TOR123_1_J1[j + 6000] + TOR123_1_J1[j + 7000]) / 8;
    		TOR123_2_J1_aver[j] = (TOR123_2_J1[j] + TOR123_2_J1[j + 1000] + TOR123_2_J1[j + 2000] + TOR123_2_J1[j + 3000] + TOR123_2_J1[j + 4000] + TOR123_2_J1[j + 5000] + TOR123_2_J1[j + 6000] + TOR123_2_J1[j + 7000]) / 8;
    		TOR123_1_J2_aver[j] = (TOR123_1_J2[j] + TOR123_1_J2[j + 1000] + TOR123_1_J2[j + 2000] + TOR123_1_J2[j + 3000] + TOR123_1_J2[j + 4000] + TOR123_1_J2[j + 5000] + TOR123_1_J2[j + 6000] + TOR123_1_J2[j + 7000]) / 8;
    		TOR123_2_J2_aver[j] = (TOR123_2_J2[j] + TOR123_2_J2[j + 1000] + TOR123_2_J2[j + 2000] + TOR123_2_J2[j + 3000] + TOR123_2_J2[j + 4000] + TOR123_2_J2[j + 5000] + TOR123_2_J2[j + 6000] + TOR123_2_J2[j + 7000]) / 8;
    		TOR123_1_J3_aver[j] = (TOR123_1_J3[j] + TOR123_1_J3[j + 1000] + TOR123_1_J3[j + 2000] + TOR123_1_J3[j + 3000] + TOR123_1_J3[j + 4000] + TOR123_1_J3[j + 5000] + TOR123_1_J3[j + 6000] + TOR123_1_J3[j + 7000]) / 8;
    		TOR123_2_J3_aver[j] = (TOR123_2_J3[j] + TOR123_2_J3[j + 1000] + TOR123_2_J3[j + 2000] + TOR123_2_J3[j + 3000] + TOR123_2_J3[j + 4000] + TOR123_2_J3[j + 5000] + TOR123_2_J3[j + 6000] + TOR123_2_J3[j + 7000]) / 8;
    		TOR456_1_J1_aver[j] = (TOR456_1_J1[j] + TOR456_1_J1[j + 1000] + TOR456_1_J1[j + 2000] + TOR456_1_J1[j + 3000] + TOR456_1_J1[j + 4000] + TOR456_1_J1[j + 5000] + TOR456_1_J1[j + 6000] + TOR456_1_J1[j + 7000]) / 8;
    		TOR456_2_J1_aver[j] = (TOR456_2_J1[j] + TOR456_2_J1[j + 1000] + TOR456_2_J1[j + 2000] + TOR456_2_J1[j + 3000] + TOR456_2_J1[j + 4000] + TOR456_2_J1[j + 5000] + TOR456_2_J1[j + 6000] + TOR456_2_J1[j + 7000]) / 8;
    		TOR456_1_J4_aver[j] = (TOR456_1_J4[j] + TOR456_1_J4[j + 1000] + TOR456_1_J4[j + 2000] + TOR456_1_J4[j + 3000] + TOR456_1_J4[j + 4000] + TOR456_1_J4[j + 5000] + TOR456_1_J4[j + 6000] + TOR456_1_J4[j + 7000]) / 8;
    		TOR456_2_J4_aver[j] = (TOR456_2_J4[j] + TOR456_2_J4[j + 1000] + TOR456_2_J4[j + 2000] + TOR456_2_J4[j + 3000] + TOR456_2_J4[j + 4000] + TOR456_2_J4[j + 5000] + TOR456_2_J4[j + 6000] + TOR456_2_J4[j + 7000]) / 8;
    		TOR456_1_J5_aver[j] = (TOR456_1_J5[j] + TOR456_1_J5[j + 1000] + TOR456_1_J5[j + 2000] + TOR456_1_J5[j + 3000] + TOR456_1_J5[j + 4000] + TOR456_1_J5[j + 5000] + TOR456_1_J5[j + 6000] + TOR456_1_J5[j + 7000]) / 8;
    		TOR456_2_J5_aver[j] = (TOR456_2_J5[j] + TOR456_2_J5[j + 1000] + TOR456_2_J5[j + 2000] + TOR456_2_J5[j + 3000] + TOR456_2_J5[j + 4000] + TOR456_2_J5[j + 5000] + TOR456_2_J5[j + 6000] + TOR456_2_J5[j + 7000]) / 8;
    		TOR456_1_J6_aver[j] = (TOR456_1_J6[j] + TOR456_1_J6[j + 1000] + TOR456_1_J6[j + 2000] + TOR456_1_J6[j + 3000] + TOR456_1_J6[j + 4000] + TOR456_1_J6[j + 5000] + TOR456_1_J6[j + 6000] + TOR456_1_J6[j + 7000]) / 8;
    		TOR456_2_J6_aver[j] = (TOR456_2_J6[j] + TOR456_2_J6[j + 1000] + TOR456_2_J6[j + 2000] + TOR456_2_J6[j + 3000] + TOR456_2_J6[j + 4000] + TOR456_2_J6[j + 5000] + TOR456_2_J6[j + 6000] + TOR456_2_J6[j + 7000]) / 8;
    	}
    	for (int k = 0; k < 1000; k++)
    	{ 
    		TOR123_1_J1_std[k] = sqrt((pow((TOR123_1_J1[k] - TOR123_1_J1_aver[k]), 2) + pow((TOR123_1_J1[k + 1000] - TOR123_1_J1_aver[k]), 2) + pow((TOR123_1_J1[k + 2000] - TOR123_1_J1_aver[k]), 2) + pow((TOR123_1_J1[k + 3000] - TOR123_1_J1_aver[k]), 2) + pow((TOR123_1_J1[k + 4000] - TOR123_1_J1_aver[k]), 2) + pow((TOR123_1_J1[k + 5000] - TOR123_1_J1_aver[k]), 2) + pow((TOR123_1_J1[k + 6000] - TOR123_1_J1_aver[k]), 2) + pow((TOR123_1_J1[k + 7000] - TOR123_1_J1_aver[k]), 2)) / 7);
    		TOR123_1_J2_std[k] = sqrt((pow((TOR123_1_J2[k] - TOR123_1_J2_aver[k]), 2) + pow((TOR123_1_J2[k + 1000] - TOR123_1_J2_aver[k]), 2) + pow((TOR123_1_J2[k + 2000] - TOR123_1_J2_aver[k]), 2) + pow((TOR123_1_J2[k + 3000] - TOR123_1_J2_aver[k]), 2) + pow((TOR123_1_J2[k + 4000] - TOR123_1_J2_aver[k]), 2) + pow((TOR123_1_J2[k + 5000] - TOR123_1_J2_aver[k]), 2) + pow((TOR123_1_J2[k + 6000] - TOR123_1_J2_aver[k]), 2) + pow((TOR123_1_J2[k + 7000] - TOR123_1_J2_aver[k]), 2)) / 7);
    		TOR123_1_J3_std[k] = sqrt((pow((TOR123_1_J3[k] - TOR123_1_J3_aver[k]), 2) + pow((TOR123_1_J3[k + 1000] - TOR123_1_J3_aver[k]), 2) + pow((TOR123_1_J3[k + 2000] - TOR123_1_J3_aver[k]), 2) + pow((TOR123_1_J3[k + 3000] - TOR123_1_J3_aver[k]), 2) + pow((TOR123_1_J3[k + 4000] - TOR123_1_J3_aver[k]), 2) + pow((TOR123_1_J3[k + 5000] - TOR123_1_J3_aver[k]), 2) + pow((TOR123_1_J3[k + 6000] - TOR123_1_J3_aver[k]), 2) + pow((TOR123_1_J3[k + 7000] - TOR123_1_J3_aver[k]), 2)) / 7);
    		TOR123_2_J1_std[k] = sqrt((pow((TOR123_2_J1[k] - TOR123_2_J1_aver[k]), 2) + pow((TOR123_2_J1[k + 1000] - TOR123_2_J1_aver[k]), 2) + pow((TOR123_2_J1[k + 2000] - TOR123_2_J1_aver[k]), 2) + pow((TOR123_2_J1[k + 3000] - TOR123_2_J1_aver[k]), 2) + pow((TOR123_2_J1[k + 4000] - TOR123_2_J1_aver[k]), 2) + pow((TOR123_2_J1[k + 5000] - TOR123_2_J1_aver[k]), 2) + pow((TOR123_2_J1[k + 6000] - TOR123_2_J1_aver[k]), 2) + pow((TOR123_2_J1[k + 7000] - TOR123_2_J1_aver[k]), 2)) / 7);
    		TOR123_2_J2_std[k] = sqrt((pow((TOR123_2_J2[k] - TOR123_2_J2_aver[k]), 2) + pow((TOR123_2_J2[k + 1000] - TOR123_2_J2_aver[k]), 2) + pow((TOR123_2_J2[k + 2000] - TOR123_2_J2_aver[k]), 2) + pow((TOR123_2_J2[k + 3000] - TOR123_2_J2_aver[k]), 2) + pow((TOR123_2_J2[k + 4000] - TOR123_2_J2_aver[k]), 2) + pow((TOR123_2_J2[k + 5000] - TOR123_2_J2_aver[k]), 2) + pow((TOR123_2_J2[k + 6000] - TOR123_2_J2_aver[k]), 2) + pow((TOR123_2_J2[k + 7000] - TOR123_2_J2_aver[k]), 2)) / 7);
    		TOR123_2_J3_std[k] = sqrt((pow((TOR123_2_J3[k] - TOR123_2_J3_aver[k]), 2) + pow((TOR123_2_J3[k + 1000] - TOR123_2_J3_aver[k]), 2) + pow((TOR123_2_J3[k + 2000] - TOR123_2_J3_aver[k]), 2) + pow((TOR123_2_J3[k + 3000] - TOR123_2_J3_aver[k]), 2) + pow((TOR123_2_J3[k + 4000] - TOR123_2_J3_aver[k]), 2) + pow((TOR123_2_J3[k + 5000] - TOR123_2_J3_aver[k]), 2) + pow((TOR123_2_J3[k + 6000] - TOR123_2_J3_aver[k]), 2) + pow((TOR123_2_J3[k + 7000] - TOR123_2_J3_aver[k]), 2)) / 7);
    		TOR456_1_J1_std[k] = sqrt((pow((TOR456_1_J1[k] - TOR456_1_J1_aver[k]), 2) + pow((TOR456_1_J1[k + 1000] - TOR456_1_J1_aver[k]), 2) + pow((TOR456_1_J1[k + 2000] - TOR456_1_J1_aver[k]), 2) + pow((TOR456_1_J1[k + 3000] - TOR456_1_J1_aver[k]), 2) + pow((TOR456_1_J1[k + 4000] - TOR456_1_J1_aver[k]), 2) + pow((TOR456_1_J1[k + 5000] - TOR456_1_J1_aver[k]), 2) + pow((TOR456_1_J1[k + 6000] - TOR456_1_J1_aver[k]), 2) + pow((TOR456_1_J1[k + 7000] - TOR456_1_J1_aver[k]), 2)) / 7);
    		TOR456_1_J4_std[k] = sqrt((pow((TOR456_1_J4[k] - TOR456_1_J4_aver[k]), 2) + pow((TOR456_1_J4[k + 1000] - TOR456_1_J4_aver[k]), 2) + pow((TOR456_1_J4[k + 2000] - TOR456_1_J4_aver[k]), 2) + pow((TOR456_1_J4[k + 3000] - TOR456_1_J4_aver[k]), 2) + pow((TOR456_1_J4[k + 4000] - TOR456_1_J4_aver[k]), 2) + pow((TOR456_1_J4[k + 5000] - TOR456_1_J4_aver[k]), 2) + pow((TOR456_1_J4[k + 6000] - TOR456_1_J4_aver[k]), 2) + pow((TOR456_1_J4[k + 7000] - TOR456_1_J4_aver[k]), 2)) / 7);
    		TOR456_1_J5_std[k] = sqrt((pow((TOR456_1_J5[k] - TOR456_1_J5_aver[k]), 2) + pow((TOR456_1_J5[k + 1000] - TOR456_1_J5_aver[k]), 2) + pow((TOR456_1_J5[k + 2000] - TOR456_1_J5_aver[k]), 2) + pow((TOR456_1_J5[k + 3000] - TOR456_1_J5_aver[k]), 2) + pow((TOR456_1_J5[k + 4000] - TOR456_1_J5_aver[k]), 2) + pow((TOR456_1_J5[k + 5000] - TOR456_1_J5_aver[k]), 2) + pow((TOR456_1_J5[k + 6000] - TOR456_1_J5_aver[k]), 2) + pow((TOR456_1_J5[k + 7000] - TOR456_1_J5_aver[k]), 2)) / 7);
    		TOR456_1_J6_std[k] = sqrt((pow((TOR456_1_J6[k] - TOR456_1_J6_aver[k]), 2) + pow((TOR456_1_J6[k + 1000] - TOR456_1_J6_aver[k]), 2) + pow((TOR456_1_J6[k + 2000] - TOR456_1_J6_aver[k]), 2) + pow((TOR456_1_J6[k + 3000] - TOR456_1_J6_aver[k]), 2) + pow((TOR456_1_J6[k + 4000] - TOR456_1_J6_aver[k]), 2) + pow((TOR456_1_J6[k + 5000] - TOR456_1_J6_aver[k]), 2) + pow((TOR456_1_J6[k + 6000] - TOR456_1_J6_aver[k]), 2) + pow((TOR456_1_J6[k + 7000] - TOR456_1_J6_aver[k]), 2)) / 7);
    		TOR456_2_J1_std[k] = sqrt((pow((TOR456_2_J1[k] - TOR456_2_J1_aver[k]), 2) + pow((TOR456_2_J1[k + 1000] - TOR456_2_J1_aver[k]), 2) + pow((TOR456_2_J1[k + 2000] - TOR456_2_J1_aver[k]), 2) + pow((TOR456_2_J1[k + 3000] - TOR456_2_J1_aver[k]), 2) + pow((TOR456_2_J1[k + 4000] - TOR456_2_J1_aver[k]), 2) + pow((TOR456_2_J1[k + 5000] - TOR456_2_J1_aver[k]), 2) + pow((TOR456_2_J1[k + 6000] - TOR456_2_J1_aver[k]), 2) + pow((TOR456_2_J1[k + 7000] - TOR456_2_J1_aver[k]), 2)) / 7);
    		TOR456_2_J4_std[k] = sqrt((pow((TOR456_2_J4[k] - TOR456_2_J4_aver[k]), 2) + pow((TOR456_2_J4[k + 1000] - TOR456_2_J4_aver[k]), 2) + pow((TOR456_2_J4[k + 2000] - TOR456_2_J4_aver[k]), 2) + pow((TOR456_2_J4[k + 3000] - TOR456_2_J4_aver[k]), 2) + pow((TOR456_2_J4[k + 4000] - TOR456_2_J4_aver[k]), 2) + pow((TOR456_2_J4[k + 5000] - TOR456_2_J4_aver[k]), 2) + pow((TOR456_2_J4[k + 6000] - TOR456_2_J4_aver[k]), 2) + pow((TOR456_2_J4[k + 7000] - TOR456_2_J4_aver[k]), 2)) / 7);
    		TOR456_2_J5_std[k] = sqrt((pow((TOR456_2_J5[k] - TOR456_2_J5_aver[k]), 2) + pow((TOR456_2_J5[k + 1000] - TOR456_2_J5_aver[k]), 2) + pow((TOR456_2_J5[k + 2000] - TOR456_2_J5_aver[k]), 2) + pow((TOR456_2_J5[k + 3000] - TOR456_2_J5_aver[k]), 2) + pow((TOR456_2_J5[k + 4000] - TOR456_2_J5_aver[k]), 2) + pow((TOR456_2_J5[k + 5000] - TOR456_2_J5_aver[k]), 2) + pow((TOR456_2_J5[k + 6000] - TOR456_2_J5_aver[k]), 2) + pow((TOR456_2_J5[k + 7000] - TOR456_2_J5_aver[k]), 2)) / 7);
    		TOR456_2_J6_std[k] = sqrt((pow((TOR456_2_J6[k] - TOR456_2_J6_aver[k]), 2) + pow((TOR456_2_J6[k + 1000] - TOR456_2_J6_aver[k]), 2) + pow((TOR456_2_J6[k + 2000] - TOR456_2_J6_aver[k]), 2) + pow((TOR456_2_J6[k + 3000] - TOR456_2_J6_aver[k]), 2) + pow((TOR456_2_J6[k + 4000] - TOR456_2_J6_aver[k]), 2) + pow((TOR456_2_J6[k + 5000] - TOR456_2_J6_aver[k]), 2) + pow((TOR456_2_J6[k + 6000] - TOR456_2_J6_aver[k]), 2) + pow((TOR456_2_J6[k + 7000] - TOR456_2_J6_aver[k]), 2)) / 7);
    	}
    	//求反 TOR123_1_J3_aver， TOR123_2_J3_aver；
    	for (int i = 0; i<1000; i++)
    	{ 
    		TOR123_1_J3_aver[i] = -TOR123_1_J3_aver[i];
    		TOR123_2_J3_aver[i] = -TOR123_2_J3_aver[i];
    	}
    	double biaocha_ffinv1[3000], biaocha_ffinv2[3000];
    	memset(biaocha_ffinv1, 0, 3000);
    	memset(biaocha_ffinv2, 0, 3000);
    	for (int i = 0; i < 1000; i++)
    	{ 
    		biaocha_ffinv1[3 * i] = 1 / TOR123_1_J1_std[i];
    		biaocha_ffinv1[3 * i + 1] = 1 / TOR123_1_J2_std[i];
    		biaocha_ffinv1[3 * i + 2] = 1 / TOR123_1_J3_std[i];
    		biaocha_ffinv2[3 * i] = 1 / TOR123_2_J1_std[i];
    		biaocha_ffinv2[3 * i + 1] = 1 / TOR123_2_J2_std[i];
    		biaocha_ffinv2[3 * i + 2] = 1 / TOR123_2_J3_std[i];
    		if (TOR123_1_J1_std[i] == 0)
    		{ 
    			biaocha_ffinv1[3 * i] = 1;
    		}
    		if (TOR123_1_J2_std[i] == 0)
    		{ 
    			biaocha_ffinv1[3 * i + 1] = 1;
    		}
    		if (TOR123_1_J3_std[i] == 0)
    		{ 
    			biaocha_ffinv1[3 * i + 2] = 1;
    		}
    		if (TOR123_2_J1_std[i] == 0)
    		{ 
    			biaocha_ffinv2[3 * i] = 1;
    		}
    		if (TOR123_2_J2_std[i] == 0)
    		{ 
    			biaocha_ffinv2[3 * i + 1] = 1;
    		}
    		if (TOR123_2_J3_std[i] == 0)
    		{ 
    			biaocha_ffinv2[3 * i + 2] = 1;
    		}
    	}
    	//构造对角矩阵；diag(biaocha_ffinv1)
    	double **dial_biaocha_ffinv1 = NULL;
    	diagMatrix(biaocha_ffinv1, &dial_biaocha_ffinv1, sizeof biaocha_ffinv1 / sizeof(double));
    	// !对角矩阵1 diag(biaocha_ffinv1)
    
    
    	//构造对角矩阵2；diag(biaocha_ffinv2)
    	double **dial_biaocha_ffinv2 = NULL;
    	diagMatrix(biaocha_ffinv2, &dial_biaocha_ffinv2, sizeof biaocha_ffinv2 / sizeof(double));
    	// !构造对角矩阵2；diag(biaocha_ffinv2)
    
    	//构造对角矩阵123; biaocha_ffinv123=blkdiag(diag(biaocha_ffinv1),diag(biaocha_ffinv2));
    	double **biaocha_ffinv123 = NULL;
    	blkdiagMatrix(dial_biaocha_ffinv1, dial_biaocha_ffinv2, sizeof biaocha_ffinv1 / sizeof(double), sizeof biaocha_ffinv2 / sizeof(double), &biaocha_ffinv123);
    
    	//释放内存；
    	if (dial_biaocha_ffinv1 != NULL && dial_biaocha_ffinv2 != NULL)
    	{ 
    		for (int i = 0; i < 3000; i++)
    		{ 
    			free(dial_biaocha_ffinv1[i]);
    			free(dial_biaocha_ffinv2[i]);
    		}
    		free(dial_biaocha_ffinv1);
    		free(dial_biaocha_ffinv2);
    	}
    	// ！！释放内存；
    
    	// !构造对角矩阵123; biaocha_ffinv123=blkdiag(diag(biaocha_ffinv1),diag(biaocha_ffinv2));
    	//biaocha_ffinv3=zeros(1,4000);biaocha_ffinv4 = zeros(1, 4000);
    	double biaocha_ffinv3[4000], biaocha_ffinv4[4000];
    	memset(biaocha_ffinv3, 0, 4000 * sizeof(double));
    	memset(biaocha_ffinv4, 0, 4000 * sizeof(double));
    	for (int i = 0; i < 1000; i++)
    	{ 
    		biaocha_ffinv3[4 * i] = 1 / TOR456_1_J1_std[i];
    		biaocha_ffinv3[4 * i + 1] = 1 / TOR456_1_J4_std[i];
    		biaocha_ffinv3[4 * i + 2] = 1 / TOR456_1_J5_std[i];
    		biaocha_ffinv3[4 * i + 3] = 1 / TOR456_1_J6_std[i];
    		biaocha_ffinv4[4 * i] = 1 / TOR456_2_J1_std[i];
    		biaocha_ffinv4[4 * i + 1] = 1 / TOR456_2_J4_std[i];
    		biaocha_ffinv4[4 * i + 2] = 1 / TOR456_2_J5_std[i];
    		biaocha_ffinv4[4 * i + 3] = 1 / TOR456_2_J6_std[i];
    
    		if (TOR456_1_J1_std[i] == 0)
    		{ 
    			biaocha_ffinv3[4 * i] = 1;
    		}
    		if (TOR456_1_J4_std[i] == 0)
    		{ 
    			biaocha_ffinv3[4 * i + 1] = 1;
    		}
    		if (TOR456_1_J5_std[i] == 0)
    		{ 
    			biaocha_ffinv3[4 * i + 2] = 1;
    		}
    		if (TOR456_1_J6_std[i] == 0)
    		{ 
    			biaocha_ffinv3[4 * i + 3] = 1;
    		}
    		if (TOR456_2_J1_std[i] == 0)
    		{ 
    			biaocha_ffinv4[4 * i] = 1;
    		}
    		if (TOR456_2_J4_std[i] == 0)
    		{ 
    			biaocha_ffinv4[4 * i + 1] = 1;
    		}
    		if (TOR456_2_J5_std[i] == 0)
    		{ 
    			biaocha_ffinv4[4 * i + 2] = 1;
    		}
    		if (TOR456_2_J6_std[i] == 0)
    		{ 
    			biaocha_ffinv4[4 * i + 3] = 1;
    		}
    	}
    	//构造对角矩阵3；diag(biaocha_ffinv3)
    	double **dial_biaocha_ffinv3 = NULL;
    	diagMatrix(biaocha_ffinv3, &dial_biaocha_ffinv3, sizeof biaocha_ffinv3 / sizeof(double));
    	// !对角矩阵1 diag(biaocha_ffinv1)
    
    	//构造对角矩阵4；diag(biaocha_ffinv4)
    	double **dial_biaocha_ffinv4 = NULL;
    	diagMatrix(biaocha_ffinv4, &dial_biaocha_ffinv4, sizeof biaocha_ffinv4 / sizeof(double));
    	// !构造对角矩阵2；diag(biaocha_ffinv2)
    
    	//构造对角矩阵456; biaocha_ffinv456=blkdiag(diag(biaocha_ffinv3),diag(biaocha_ffinv4));
    	double **biaocha_ffinv456 = NULL;
    	blkdiagMatrix(dial_biaocha_ffinv3, dial_biaocha_ffinv4, sizeof biaocha_ffinv3 / sizeof(double), sizeof biaocha_ffinv4 / sizeof(double), &biaocha_ffinv456);
    
    	if (dial_biaocha_ffinv3 != NULL && dial_biaocha_ffinv4 != NULL)
    	{ 
    		for (int i = 0; i < 3000; i++)
    		{ 
    			free(dial_biaocha_ffinv3[i]);
    			free(dial_biaocha_ffinv4[i]);
    		}
    		free(dial_biaocha_ffinv3);
    		free(dial_biaocha_ffinv4);
    	}
    
    	double p1[1000][2], p2[1000][2], p3[1000][2];
    	for (int i = 0; i<1000; i++)
    	{ 
    		p1[i][0] = i + 1;
    		p2[i][0] = i + 1;
    		p3[i][0] = i + 1;
    		p1[i][1] = TOR123_1_J1_aver[i];
    		p2[i][1] = TOR123_1_J2_aver[i];
    		p3[i][1] = TOR123_1_J3_aver[i];
    	}
    	double p11[16][1000][2], p21[16][1000][2], p31[16][1000][2];
    	for (int i = 0; i<1000; i++)
    	{ 
    		p11[0][i][0] = p1[i][0];
    		p21[0][i][0] = p2[i][0];
    		p31[0][i][0] = p3[i][0];
    
    		p11[0][i][1] = p1[i][1];
    		p21[0][i][1] = p2[i][1];
    		p31[0][i][1] = p3[i][1];
    	}
    	for (int i = 1; i < 16; i++)
    	{ 
    		p11[i][0][0] = p11[i - 1][0][0]; p11[i][1][0] = p11[i - 1][1][0]; p11[i][1][1] = (2 * (p11[i - 1][0][1] + p11[i - 1][4][1]) + 27 * p11[i - 1][1][1] - 8 * p11[i - 1][3][1] + 12 * p11[i - 1][2][1]) / 35; p11[i][0][1] = (69 * p11[i - 1][0][1] + 4 * (p11[i - 1][1][1] + p11[i - 1][3][1]) - 6 * p11[i - 1][2][1] - p11[i - 1][4][1]) / 70;
    		p21[i][0][0] = p21[i - 1][0][0]; p21[i][1][0] = p21[i - 1][1][0]; p21[i][1][1] = (2 * (p21[i - 1][0][1] + p21[i - 1][4][1]) + 27 * p21[i - 1][1][1] - 8 * p21[i - 1][3][1] + 12 * p21[i - 1][2][1]) / 35; p21[i][0][1] = (69 * p21[i - 1][0][1] + 4 * (p21[i - 1][1][1] + p21[i - 1][3][1]) - 6 * p21[i - 1][2][1] - p21[i - 1][4][1]) / 70;
    		p31[i][0][0] = p31[i - 1][0][0]; p31[i][1][0] = p31[i - 1][1][0]; p31[i][1][1] = (2 * (p31[i - 1][0][1] + p31[i - 1][4][1]) + 27 * p31[i - 1][1][1] - 8 * p31[i - 1][3][1] + 12 * p31[i - 1][2][1]) / 35; p31[i][0][1] = (69 * p31[i - 1][0][1] + 4 * (p31[i - 1][1][1] + p31[i - 1][3][1]) - 6 * p31[i - 1][2][1] - p31[i - 1][4][1]) / 70;
    
    		for (int j = 2; j < 998; j++)
    		{ 
    			p11[i][j][0] = p11[i - 1][j][0];
    			p11[i][j][1] = (-3 * (p11[i - 1][j - 2][1] + p11[i - 1][j + 2][1]) + 12 * (p11[i - 1][j - 1][1] + p11[i - 1][j + 1][1]) + 17 * p11[i - 1][j][1]) / 35;
    			p21[i][j][0] = p21[i - 1][j][0];
    			p21[i][j][1] = (-3 * (p21[i - 1][j - 2][1] + p21[i - 1][j + 2][1]) + 12 * (p21[i - 1][j - 1][1] + p21[i - 1][j + 1][1]) + 17 * p21[i - 1][j][1]) / 35;
    			p31[i][j][0] = p31[i - 1][j][0];
    			p31[i][j][1] = (-3 * (p31[i - 1][j - 2][1] + p31[i - 1][j + 2][1]) + 12 * (p31[i - 1][j - 1][1] + p31[i - 1][j + 1][1]) + 17 * p31[i - 1][j][1]) / 35;
    		}
    		p11[i][998][0] = p11[i - 1][998][0]; p11[i][999][0] = p11[i - 1][999][0]; p11[i][999][1] = (-p11[i - 1][995][1] + 4 * (p11[i - 1][996][1] + p11[i - 1][998][1]) - 6 * p11[i - 1][997][1] + 69 * p11[i - 1][999][1]) / 70; p11[i][998][1] = (2 * (p11[i - 1][995][1] + p11[i - 1][999][1]) - 8 * p11[i - 1][996][1] + 12 * p11[i - 1][997][1] + 27 * p11[i - 1][998][1]) / 35;
    		p21[i][998][0] = p21[i - 1][998][0]; p21[i][999][0] = p21[i - 1][999][0]; p21[i][999][1] = (-p21[i - 1][995][1] + 4 * (p21[i - 1][996][1] + p21[i - 1][998][1]) - 6 * p21[i - 1][997][1] + 69 * p21[i - 1][999][1]) / 70; p21[i][998][1] = (2 * (p21[i - 1][995][1] + p21[i - 1][999][1]) - 8 * p21[i - 1][996][1] + 12 * p21[i - 1][997][1] + 27 * p21[i - 1][998][1]) / 35;
    		p31[i][998][0] = p31[i - 1][998][0]; p31[i][999][0] = p31[i - 1][999][0]; p31[i][999][1] = (-p31[i - 1][995][1] + 4 * (p31[i - 1][996][1] + p31[i - 1][998][1]) - 6 * p31[i - 1][997][1] + 69 * p31[i - 1][999][1]) / 70; p31[i][998][1] = (2 * (p31[i - 1][995][1] + p31[i - 1][999][1]) - 8 * p31[i - 1][996][1] + 12 * p31[i - 1][997][1] + 27 * p31[i - 1][998][1]) / 35;
    	}
    	double p1f1[1000], p2f1[1000], p3f1[1000];
    	for (int i = 0; i < 1000; i++)
    	{ 
    		p1f1[i] = p11[6][i][1];
    		p2f1[i] = p21[6][i][1];
    		p3f1[i] = p31[6][i][1];
    	}
    
    	for (int i = 0; i < 1000; i++)
    	{ 
    		p1[i][0] = i + 1;
    		p2[i][0] = i + 1;
    		p3[i][0] = i + 1;
    		p1[i][1] = TOR123_2_J1_aver[i];
    		p2[i][1] = TOR123_2_J2_aver[i];
    		p3[i][1] = TOR123_2_J3_aver[i];
    	}
    	for (int i = 0; i < 1000; i++)
    	{ 
    		p11[0][i][0] = p1[i][0];
    		p21[0][i][0] = p2[i][0];
    		p31[0][i][0] = p3[i][0];
    
    		p11[0][i][1] = p1[i][1];
    		p21[0][i][1] = p2[i][1];
    		p31[0][i][1] = p3[i][1];
    	}
    	for (int i = 1; i < 16; i++)
    	{ 
    		p11[i][0][0] = p11[i - 1][0][0]; p11[i][1][0] = p11[i - 1][1][0]; p11[i][1][1] = (2 * (p11[i - 1][0][1] + p11[i - 1][4][1]) + 27 * p11[i - 1][1][1] - 8 * p11[i - 1][3][1] + 12 * p11[i - 1][2][1]) / 35; p11[i][0][1] = (69 * p11[i - 1][0][1] + 4 * (p11[i - 1][1][1] + p11[i - 1][3][1]) - 6 * p11[i - 1][2][1] - p11[i - 1][4][1]) / 70;
    		p21[i][0][0] = p21[i - 1][0][0]; p21[i][1][0] = p21[i - 1][1][0]; p21[i][1][1] = (2 * (p21[i - 1][0][1] + p21[i - 1][4][1]) + 27 * p21[i - 1][1][1] - 8 * p21[i - 1][3][1] + 12 * p21[i - 1][2][1]) / 35; p21[i][0][1] = (69 * p21[i - 1][0][1] + 4 * (p21[i - 1][1][1] + p21[i - 1][3][1]) - 6 * p21[i - 1][2][1] - p21[i - 1][4][1]) / 70;
    		p31[i][0][0] = p31[i - 1][0][0]; p31[i][1][0] = p31[i - 1][1][0]; p31[i][1][1] = (2 * (p31[i - 1][0][1] + p31[i - 1][4][1]) + 27 * p31[i - 1][1][1] - 8 * p31[i - 1][3][1] + 12 * p31[i - 1][2][1]) / 35; p31[i][0][1] = (69 * p31[i - 1][0][1] + 4 * (p31[i - 1][1][1] + p31[i - 1][3][1]) - 6 * p31[i - 1][2][1] - p31[i - 1][4][1]) / 70;
    
    		for (int j = 2; j < 998; j++)
    		{ 
    			p11[i][j][0] = p11[i - 1][j][0];
    			p11[i][j][1] = (-3 * (p11[i - 1][j - 2][1] + p11[i - 1][j + 2][1]) + 12 * (p11[i - 1][j - 1][1] + p11[i - 1][j + 1][1]) + 17 * p11[i - 1][j][1]) / 35;
    			p21[i][j][0] = p21[i - 1][j][0];
    			p21[i][j][1] = (-3 * (p21[i - 1][j - 2][1] + p21[i - 1][j + 2][1]) + 12 * (p21[i - 1][j - 1][1] + p21[i - 1][j + 1][1]) + 17 * p21[i - 1][j][1]) / 35;
    			p31[i][j][0] = p31[i - 1][j][0];
    			p31[i][j][1] = (-3 * (p31[i - 1][j - 2][1] + p31[i - 1][j + 2][1]) + 12 * (p31[i - 1][j - 1][1] + p31[i - 1][j + 1][1]) + 17 * p31[i - 1][j][1]) / 35;
    		}
    		p11[i][998][0] = p11[i - 1][998][0]; p11[i][999][0] = p11[i - 1][999][0]; p11[i][999][1] = (-p11[i - 1][995][1] + 4 * (p11[i - 1][996][1] + p11[i - 1][998][1]) - 6 * p11[i - 1][997][1] + 69 * p11[i - 1][999][1]) / 70; p11[i][998][1] = (2 * (p11[i - 1][995][1] + p11[i - 1][999][1]) - 8 * p11[i - 1][996][1] + 12 * p11[i - 1][997][1] + 27 * p11[i - 1][998][1]) / 35;
    		p21[i][998][0] = p21[i - 1][998][0]; p21[i][999][0] = p21[i - 1][999][0]; p21[i][999][1] = (-p21[i - 1][995][1] + 4 * (p21[i - 1][996][1] + p21[i - 1][998][1]) - 6 * p21[i - 1][997][1] + 69 * p21[i - 1][999][1]) / 70; p21[i][998][1] = (2 * (p21[i - 1][995][1] + p21[i - 1][999][1]) - 8 * p21[i - 1][996][1] + 12 * p21[i - 1][997][1] + 27 * p21[i - 1][998][1]) / 35;
    		p31[i][998][0] = p31[i - 1][998][0]; p31[i][999][0] = p31[i - 1][999][0]; p31[i][999][1] = (-p31[i - 1][995][1] + 4 * (p31[i - 1][996][1] + p31[i - 1][998][1]) - 6 * p31[i - 1][997][1] + 69 * p31[i - 1][999][1]) / 70; p31[i][998][1] = (2 * (p31[i - 1][995][1] + p31[i - 1][999][1]) - 8 * p31[i - 1][996][1] + 12 * p31[i - 1][997][1] + 27 * p31[i - 1][998][1]) / 35;
    	}
    	double p1f2[1000], p2f2[1000], p3f2[1000];
    	for (int i = 0; i < 1000; i++)
    	{ 
    		p1f2[i] = p11[6][i][1];
    		p2f2[i] = p21[6][i][1];
    		p3f2[i] = p31[6][i][1];
    	}
    	/*t_final1 = zeros(1, 3000); t_final2 = zeros(1, 3000);*/
    	double t_final1[3000], t_final2[3000], t_final123[6000];
    	memset(t_final1, 0, 3000 * sizeof(double)); memset(t_final2, 0, 3000 * sizeof(double));
    	for (int i = 0; i < 1000; i++)
    	{ 
    		t_final1[3 * i] = p1f1[i];
    		t_final1[3 * i + 1] = p2f1[i];
    		t_final1[3 * i + 2] = p3f1[i];
    
    		t_final2[3 * i] = p1f2[i];
    		t_final2[3 * i + 1] = p2f2[i];
    		t_final2[3 * i + 2] = p3f2[i];
    	}
    	//上下cat矩阵；t_final123=cat(1,t_final1,t_final2);
    	for (int i = 0; i < 6000; i++)
    	{ 
    		if (i < 3000)
    		{ 
    			t_final123[i] = t_final1[i];
    		}
    		else
    		{ 
    			t_final123[i] = t_final2[i - 3000];
    		}
    
    	}
    	//275lines 
    	double p4[1000][2];
    	for (int i = 0; i < 1000; i++)
    	{ 
    		p1[i][0] = i + 1;
    		p2[i][0] = i + 1;
    		p3[i][0] = i + 1;
    		p4[i][0] = i + 1;
    
    		p1[i][1] = TOR456_1_J1_aver[i];
    		p2[i][1] = TOR456_1_J4_aver[i];
    		p3[i][1] = TOR456_1_J5_aver[i];
    		p4[i][1] = TOR456_1_J6_aver[i];
    	}
    	double p41[16][1000][2];
    	for (int i = 0; i < 1000; i++)
    	{ 
    		p11[0][i][0] = p1[i][0];
    		p21[0][i][0] = p2[i][0];
    		p31[0][i][0] = p3[i][0];
    		p41[0][i][0] = p4[i][0];
    
    		p11[0][i][1] = p1[i][1];
    		p21[0][i][1] = p2[i][1];
    		p31[0][i][1] = p3[i][1];
    		p41[0][i][1] = p4[i][1];
    	}
    	for (int i = 1; i < 16; i++)
    	{ 
    		p11[i][0][0] = p11[i - 1][0][0]; p11[i][1][0] = p11[i - 1][1][0]; p11[i][1][1] = (2 * (p11[i - 1][0][1] + p11[i - 1][4][1]) + 27 * p11[i - 1][1][1] - 8 * p11[i - 1][3][1] + 12 * p11[i - 1][2][1]) / 35; p11[i][0][1] = (69 * p11[i - 1][0][1] + 4 * (p11[i - 1][1][1] + p11[i - 1][3][1]) - 6 * p11[i - 1][2][1] - p11[i - 1][4][1]) / 70;
    		p21[i][0][0] = p21[i - 1][0][0]; p21[i][1][0] = p21[i - 1][1][0]; p21[i][1][1] = (2 * (p21[i - 1][0][1] + p21[i - 1][4][1]) + 27 * p21[i - 1][1][1] - 8 * p21[i - 1][3][1] + 12 * p21[i - 1][2][1]) / 35; p21[i][0][1] = (69 * p21[i - 1][0][1] + 4 * (p21[i - 1][1][1] + p21[i - 1][3][1]) - 6 * p21[i - 1][2][1] - p21[i - 1][4][1]) / 70;
    		p31[i][0][0] = p31[i - 1][0][0]; p31[i][1][0] = p31[i - 1][1][0]; p31[i][1][1] = (2 * (p31[i - 1][0][1] + p31[i - 1][4][1]) + 27 * p31[i - 1][1][1] - 8 * p31[i - 1][3][1] + 12 * p31[i - 1][2][1]) / 35; p31[i][0][1] = (69 * p31[i - 1][0][1] + 4 * (p31[i - 1][1][1] + p31[i - 1][3][1]) - 6 * p31[i - 1][2][1] - p31[i - 1][4][1]) / 70;
    		p41[i][0][0] = p41[i - 1][0][0]; p41[i][1][0] = p41[i - 1][1][0]; p41[i][1][1] = (2 * (p41[i - 1][0][1] + p41[i - 1][4][1]) + 27 * p41[i - 1][1][1] - 8 * p41[i - 1][3][1] + 12 * p41[i - 1][2][1]) / 35; p41[i][0][1] = (69 * p41[i - 1][0][1] + 4 * (p41[i - 1][1][1] + p41[i - 1][3][1]) - 6 * p41[i - 1][2][1] - p41[i - 1][4][1]) / 70;
    
    		for (int j = 2; j < 998; j++)
    		{ 
    			p11[i][j][0] = p11[i - 1][j][0];
    			p11[i][j][1] = (-3 * (p11[i - 1][j - 2][1] + p11[i - 1][j + 2][1]) + 12 * (p11[i - 1][j - 1][1] + p11[i - 1][j + 1][1]) + 17 * p11[i - 1][j][1]) / 35;
    			p21[i][j][0] = p21[i - 1][j][0];
    			p21[i][j][1] = (-3 * (p21[i - 1][j - 2][1] + p21[i - 1][j + 2][1]) + 12 * (p21[i - 1][j - 1][1] + p21[i - 1][j + 1][1]) + 17 * p21[i - 1][j][1]) / 35;
    			p31[i][j][0] = p31[i - 1][j][0];
    			p31[i][j][1] = (-3 * (p31[i - 1][j - 2][1] + p31[i - 1][j + 2][1]) + 12 * (p31[i - 1][j - 1][1] + p31[i - 1][j + 1][1]) + 17 * p31[i - 1][j][1]) / 35;
    			p41[i][j][0] = p41[i - 1][j][0];
    			p41[i][j][1] = (-3 * (p41[i - 1][j - 2][1] + p41[i - 1][j + 2][1]) + 12 * (p41[i - 1][j - 1][1] + p41[i - 1][j + 1][1]) + 17 * p41[i - 1][j][1]) / 35;
    		}
    		p11[i][998][0] = p11[i - 1][998][0]; p11[i][999][0] = p11[i - 1][999][0]; p11[i][999][1] = (-p11[i - 1][995][1] + 4 * (p11[i - 1][996][1] + p11[i - 1][998][1]) - 6 * p11[i - 1][997][1] + 69 * p11[i - 1][999][1]) / 70; p11[i][998][1] = (2 * (p11[i - 1][995][1] + p11[i - 1][999][1]) - 8 * p11[i - 1][996][1] + 12 * p11[i - 1][997][1] + 27 * p11[i - 1][998][1]) / 35;
    		p21[i][998][0] = p21[i - 1][998][0]; p21[i][999][0] = p21[i - 1][999][0]; p21[i][999][1] = (-p21[i - 1][995][1] + 4 * (p21[i - 1][996][1] + p21[i - 1][998][1]) - 6 * p21[i - 1][997][1] + 69 * p21[i - 1][999][1]) / 70; p21[i][998][1] = (2 * (p21[i - 1][995][1] + p21[i - 1][999][1]) - 8 * p21[i - 1][996][1] + 12 * p21[i - 1][997][1] + 27 * p21[i - 1][998][1]) / 35;
    		p31[i][998][0] = p31[i - 1][998][0]; p31[i][999][0] = p31[i - 1][999][0]; p31[i][999][1] = (-p31[i - 1][995][1] + 4 * (p31[i - 1][996][1] + p31[i - 1][998][1]) - 6 * p31[i - 1][997][1] + 69 * p31[i - 1][999][1]) / 70; p31[i][998][1] = (2 * (p31[i - 1][995][1] + p31[i - 1][999][1]) - 8 * p31[i - 1][996][1] + 12 * p31[i - 1][997][1] + 27 * p31[i - 1][998][1]) / 35;
    		p41[i][998][0] = p41[i - 1][998][0]; p41[i][999][0] = p41[i - 1][999][0]; p41[i][999][1] = (-p41[i - 1][995][1] + 4 * (p41[i - 1][996][1] + p41[i - 1][998][1]) - 6 * p41[i - 1][997][1] + 69 * p41[i - 1][999][1]) / 70; p41[i][998][1] = (2 * (p41[i - 1][995][1] + p41[i - 1][999][1]) - 8 * p41[i - 1][996][1] + 12 * p41[i - 1][997][1] + 27 * p41[i - 1][998][1]) / 35;
    	}
    	//304 lines
    	double p1f[1000], p2f[1000], p3f[1000], p4f[1000];
    	for (int i = 0; i < 1000; i++)
    	{ 
    		p1f[i] = p11[6][i][1];
    		p2f[i] = p21[6][i][1];
    		p3f[i] = p31[6][i][1];
    		p4f[i] = p41[6][i][1];
    	}
    	double t_final3[4000];
    	for (int i = 0; i < 1000; i++)
    	{ 
    		t_final3[4 * i] = p1f[i];
    		t_final3[4 * i + 1] = p2f[i];
    		t_final3[4 * i + 2] = p3f[i];
    		t_final3[4 * i + 3] = p4f[i];
    	}
    	for (int i = 0; i < 1000; i++)
    	{ 
    		p1[i][0] = i + 1;
    		p2[i][0] = i + 1;
    		p3[i][0] = i + 1;
    		p4[i][0] = i + 1;
    
    		p1[i][1] = TOR456_2_J1_aver[i];
    		p2[i][1] = TOR456_2_J4_aver[i];
    		p3[i][1] = TOR456_2_J5_aver[i];
    		p4[i][1] = TOR456_2_J6_aver[i];
    	}
    	for (int i = 0; i < 1000; i++)
    	{ 
    		p11[0][i][0] = p1[i][0];
    		p21[0][i][0] = p2[i][0];
    		p31[0][i][0] = p3[i][0];
    		p41[0][i][0] = p4[i][0];
    
    		p11[0][i][1] = p1[i][1];
    		p21[0][i][1] = p2[i][1];
    		p31[0][i][1] = p3[i][1];
    		p41[0][i][1] = p4[i][1];
    	}
    	for (int i = 1; i < 16; i++)
    	{ 
    		p11[i][0][0] = p11[i - 1][0][0]; p11[i][1][0] = p11[i - 1][1][0]; p11[i][1][1] = (2 * (p11[i - 1][0][1] + p11[i - 1][4][1]) + 27 * p11[i - 1][1][1] - 8 * p11[i - 1][3][1] + 12 * p11[i - 1][2][1]) / 35; p11[i][0][1] = (69 * p11[i - 1][0][1] + 4 * (p11[i - 1][1][1] + p11[i - 1][3][1]) - 6 * p11[i - 1][2][1] - p11[i - 1][4][1]) / 70;
    		p21[i][0][0] = p21[i - 1][0][0]; p21[i][1][0] = p21[i - 1][1][0]; p21[i][1][1] = (2 * (p21[i - 1][0][1] + p21[i - 1][4][1]) + 27 * p21[i - 1][1][1] - 8 * p21[i - 1][3][1] + 12 * p21[i - 1][2][1]) / 35; p21[i][0][1] = (69 * p21[i - 1][0][1] + 4 * (p21[i - 1][1][1] + p21[i - 1][3][1]) - 6 * p21[i - 1][2][1] - p21[i - 1][4][1]) / 70;
    		p31[i][0][0] = p31[i - 1][0][0]; p31[i][1][0] = p31[i - 1][1][0]; p31[i][1][1] = (2 * (p31[i - 1][0][1] + p31[i - 1][4][1]) + 27 * p31[i - 1][1][1] - 8 * p31[i - 1][3][1] + 12 * p31[i - 1][2][1]) / 35; p31[i][0][1] = (69 * p31[i - 1][0][1] + 4 * (p31[i - 1][1][1] + p31[i - 1][3][1]) - 6 * p31[i - 1][2][1] - p31[i - 1][4][1]) / 70;
    		p41[i][0][0] = p41[i - 1][0][0]; p41[i][1][0] = p41[i - 1][1][0]; p41[i][1][1] = (2 * (p41[i - 1][0][1] + p41[i - 1][4][1]) + 27 * p41[i - 1][1][1] - 8 * p41[i - 1][3][1] + 12 * p41[i - 1][2][1]) / 35; p41[i][0][1] = (69 * p41[i - 1][0][1] + 4 * (p41[i - 1][1][1] + p41[i - 1][3][1]) - 6 * p41[i - 1][2][1] - p41[i - 1][4][1]) / 70;
    
    		for (int j = 2; j < 998; j++)
    		{ 
    			p11[i][j][0] = p11[i - 1][j][0];
    			p11[i][j][1] = (-3 * (p11[i - 1][j - 2][1] + p11[i - 1][j + 2][1]) + 12 * (p11[i - 1][j - 1][1] + p11[i - 1][j + 1][1]) + 17 * p11[i - 1][j][1]) / 35;
    			p21[i][j][0] = p21[i - 1][j][0];
    			p21[i][j][1] = (-3 * (p21[i - 1][j - 2][1] + p21[i - 1][j + 2][1]) + 12 * (p21[i - 1][j - 1][1] + p21[i - 1][j + 1][1]) + 17 * p21[i - 1][j][1]) / 35;
    			p31[i][j][0] = p31[i - 1][j][0];
    			p31[i][j][1] = (-3 * (p31[i - 1][j - 2][1] + p31[i - 1][j + 2][1]) + 12 * (p31[i - 1][j - 1][1] + p31[i - 1][j + 1][1]) + 17 * p31[i - 1][j][1]) / 35;
    			p41[i][j][0] = p41[i - 1][j][0];
    			p41[i][j][1] = (-3 * (p41[i - 1][j - 2][1] + p41[i - 1][j + 2][1]) + 12 * (p41[i - 1][j - 1][1] + p41[i - 1][j + 1][1]) + 17 * p41[i - 1][j][1]) / 35;
    		}
    		p11[i][998][0] = p11[i - 1][998][0]; p11[i][999][0] = p11[i - 1][999][0]; p11[i][999][1] = (-p11[i - 1][995][1] + 4 * (p11[i - 1][996][1] + p11[i - 1][998][1]) - 6 * p11[i - 1][997][1] + 69 * p11[i - 1][999][1]) / 70; p11[i][998][1] = (2 * (p11[i - 1][995][1] + p11[i - 1][999][1]) - 8 * p11[i - 1][996][1] + 12 * p11[i - 1][997][1] + 27 * p11[i - 1][998][1]) / 35;
    		p21[i][998][0] = p21[i - 1][998][0]; p21[i][999][0] = p21[i - 1][999][0]; p21[i][999][1] = (-p21[i - 1][995][1] + 4 * (p21[i - 1][996][1] + p21[i - 1][998][1]) - 6 * p21[i - 1][997][1] + 69 * p21[i - 1][999][1]) / 70; p21[i][998][1] = (2 * (p21[i - 1][995][1] + p21[i - 1][999][1]) - 8 * p21[i - 1][996][1] + 12 * p21[i - 1][997][1] + 27 * p21[i - 1][998][1]) / 35;
    		p31[i][998][0] = p31[i - 1][998][0]; p31[i][999][0] = p31[i - 1][999][0]; p31[i][999][1] = (-p31[i - 1][995][1] + 4 * (p31[i - 1][996][1] + p31[i - 1][998][1]) - 6 * p31[i - 1][997][1] + 69 * p31[i - 1][999][1]) / 70; p31[i][998][1] = (2 * (p31[i - 1][995][1] + p31[i - 1][999][1]) - 8 * p31[i - 1][996][1] + 12 * p31[i - 1][997][1] + 27 * p31[i - 1][998][1]) / 35;
    		p41[i][998][0] = p41[i - 1][998][0]; p41[i][999][0] = p41[i - 1][999][0]; p41[i][999][1] = (-p41[i - 1][995][1] + 4 * (p41[i - 1][996][1] + p41[i - 1][998][1]) - 6 * p41[i - 1][997][1] + 69 * p41[i - 1][999][1]) / 70; p41[i][998][1] = (2 * (p41[i - 1][995][1] + p41[i - 1][999][1]) - 8 * p41[i - 1][996][1] + 12 * p41[i - 1][997][1] + 27 * p41[i - 1][998][1]) / 35;
    	}
    	//345 lines;
    	for (int i = 0; i < 1000; i++)
    	{ 
    		p1f[i] = p11[6][i][1];
    		p2f[i] = p21[6][i][1];
    		p3f[i] = p31[6][i][1];
    		p4f[i] = p41[6][i][1];
    	}
    	double t_final4[4000];
    	for (int i = 0; i < 1000; i++)
    	{ 
    		t_final4[4 * i] = p1f[i];
    		t_final4[4 * i + 1] = p2f[i];
    		t_final4[4 * i + 2] = p3f[i];
    		t_final4[4 * i + 3] = p4f[i];
    	}
    	//358 lines;
    	double t_final1456[8000];
    	for (int i = 0; i < 8000; i++)
    	{ 
    		if (i < 4000)
    		{ 
    			t_final1456[i] = t_final3[i];
    		}
    		else
    		{ 
    			t_final1456[i] = t_final4[i - 4000];
    		}
    	}
    	//360 lines;
    	double s1[1000], s2[1000], s3[1000], s4[1000];
    	for (int i = 0; i < 1000; i++)
    	{ 
    		s1[i] = i + 1;
    		s2[i] = i + 1;
    		s3[i] = i + 1;
    		s4[i] = i + 1;
    	}
    
    	int temp_k = 0;
    	double w = 2 * 3.1416 * 0.1;
    	double x1[] = {  0.1381185, 1.05939 , -0.0369357, -0.0429297, -0.0461455, -0.134689, 0.259093, -0.564252, 0.115489, 0.0369552, 0.0145963, 2.006398, -0.390787, 0.176768, 0.116194, -0.0775871, -0.000236926, -0.698232, 0.233103, 0.151767, -0.161488, 0.0392484, 0.04163783, -1.06853, 0.0197313, 0.134134, -0.0371975, 0.155346, 0.0436672, -0.078893, -0.0221862, -0.00831553, 0.0240897 };
    	double x2[] = {  0.14634071686442156, 1.1685708035132396, -0.20644852705720565, -0.06615920266945821, -0.19464260712664444, 0.046663090794211534, -0.267478468238523, 0.12807216621172485, 0.03611344327128146, -0.050300895018593444, 0.007242137065465804,	0.6720770836801273,	0.35032140111533216, -0.01197881198483224, 0.0014661968381333946,  -0.029372501538711626,  -0.052685372832512296, 0.5600444318384661, 0.35892862966872474, 0.17957873417040301, -0.14471442283008165, -0.055065367896443916, -0.23232035032801996,	0.06894743867679838, -0.009477784608994509, -0.06566738569577933, 0.031492309579670044, 0.00217516457932212, 0.7312222034875538, -0.4214891450657436,  -0.2101534047768039,	0.06093722566058837, 0.0718021192213875 };
    	double x3[] = {  0.7184768436716859, -0.05320807107963481,	0.0595850576323585,	0.13270118649045065,	0.05061991159386071, -0.13357158598646474 ,-0.8631182086464059,	0.06278667646731166,	0.0578491042747673,	0.056434639371598413, -0.03242905513888583, -0.5180837525886208,	0.943686595296595, -0.03187115932996163, -0.041755837994927276, -0.05132077638008118, -0.10987867505029916,	0.727055366289381, -0.2159023598784902,	0.04262248485657944, -0.07191834304233485,	0.036226604363571786,	0.3172332691031914,	1.1094306289238187, -0.053004464068939834, -0.060357492193567425, -0.03534860603229476 ,-0.13605765340779924 ,-0.048508696649126916, -0.40925582499031254,	0.10671384526989881,	0.01365024143980388,	0.020167165826632447,	0.22468571839870975,	0.7450281671919221, -0.023169742272374158, -0.055613837590208624, -0.02833916008226385, -0.08400940903152405 ,-0.12538807814189162,	0.03543852528608178, -0.23352289618551594,	0.042736328121584566,	0.05605040252111371 };
    	double x4[] = {  -0.33674435658713, -0.10909975694555996,	0.08786055848629629, -0.03215150479806473,	0.11861288969426818, -0.08914777365637251,	0.5615830335016828, -0.1846109486038434, -0.12378028278609485,	0.08852149524766696, -0.004968940772280886,	0.52156078628021474,	0.047435056445054954, -0.02452350920580409,	0.10734321729674465, -0.01816108124379431, -0.049551946202829, -0.9864149298784746,	0.38743011141118183,	0.1416513313760551,	0.02555283837512258, -0.08978013756409965, -0.0339331800547687576,	1.3834242723441201,	0.11737727674007759, -0.28932600555389193, -0.04095567083789001, -0.11723917538545217,	0.011518045981927887,	0.013442761081521447,	0.001684209444359778,	0.029379591347860442, -0.022091427800900797, -0.030930566042995571, -1.35252127368367, -0.03452156430921878,	0.13800701003931395,	0.1687832997812872,	0.0666208011103368,	0.016343472109995462,	0.012814438170343063,	0.01948200894477103, -0.022389613600769763,	0.004680260418655778 };
    
    	for (float i =(float)0.01; i < 10.01; i =(float)(i + 0.01))
    	{ 
    		double_Array bijiao1, bijiao2, bijiao3, bijiao4;
    		double t[1000];
    		double temp = i;
    		double theta1[1000];
    		double theta2[1000];
    		double theta3[1000];
    		double theta4[1000];
    		memcpy(bijiao1, POS123_1_J1_aver, sizeof(POS123_1_J1_aver));
    		memcpy(bijiao2, POS123_2_J1_aver, sizeof(POS123_2_J1_aver));
    		memcpy(bijiao3, POS456_1_J1_aver, sizeof(POS456_1_J1_aver));
    		memcpy(bijiao4, POS456_2_J1_aver, sizeof(POS456_2_J1_aver));
    		s1[temp_k] = 0;
    		s2[temp_k] = 0;
    		s3[temp_k] = 0;
    		s4[temp_k] = 0;
    		for (int j = 0; j < 1000; j++)
    		{ 
    			t[j] = temp;
    			temp = temp + 0.01;
    		}
    		for (int k = 0; k < 1000; k++)
    		{ 
    			theta1[k] = x1[0] + x1[1] * sin(w*t[k]) + x1[6] * cos(w*t[k]) + x1[2] * sin(2 * w*t[k]) + x1[7] * cos(2 * w*t[k]) + x1[3] * sin(3 * w*t[k]) + x1[8] * cos(3 * w*t[k]) + x1[4] * sin(4 * w*t[k]) + x1[9] * cos(4 * w*t[k]) + x1[5] * sin(5 * w*t[k]) + x1[10] * cos(5 * w*t[k]);
    			theta2[k] = x2[0] + x2[1] * sin(w*t[k]) + x2[6] * cos(w*t[k]) + x2[2] * sin(2 * w*t[k]) + x2[7] * cos(2 * w*t[k]) + x2[3] * sin(3 * w*t[k]) + x2[8] * cos(3 * w*t[k]) + x2[4] * sin(4 * w*t[k]) + x2[9] * cos(4 * w*t[k]) + x2[5] * sin(5 * w*t[k]) + x2[10] * cos(5 * w*t[k]);
    			theta3[k] = x3[0] + x3[1] * sin(w*t[k]) + x3[6] * cos(w*t[k]) + x3[2] * sin(2 * w*t[k]) + x3[7] * cos(2 * w*t[k]) + x3[3] * sin(3 * w*t[k]) + x3[8] * cos(3 * w*t[k]) + x3[4] * sin(4 * w*t[k]) + x3[9] * cos(4 * w*t[k]) + x3[5] * sin(5 * w*t[k]) + x3[10] * cos(5 * w*t[k]);
    			theta4[k] = x4[0] + x4[1] * sin(w*t[k]) + x4[6] * cos(w*t[k]) + x4[2] * sin(2 * w*t[k]) + x4[7] * cos(2 * w*t[k]) + x4[3] * sin(3 * w*t[k]) + x4[8] * cos(3 * w*t[k]) + x4[4] * sin(4 * w*t[k]) + x4[9] * cos(4 * w*t[k]) + x4[5] * sin(5 * w*t[k]) + x4[10] * cos(5 * w*t[k]);
    		}
    		for (int temp_j = 0; temp_j < 1000; temp_j++)
    		{ 
    			s1[temp_k] = s1[temp_k] + pow((bijiao1[temp_j] - theta1[temp_j]), 2);
    			s2[temp_k] = s2[temp_k] + pow((bijiao2[temp_j] - theta2[temp_j]), 2);
    			s3[temp_k] = s3[temp_k] + pow((bijiao3[temp_j] - theta3[temp_j]), 2);
    			s4[temp_k] = s4[temp_k] + pow((bijiao4[temp_j] - theta4[temp_j]), 2);
    		}
    		temp_k = temp_k + 1;
    	}
    	double tx1, tx2, tx3, tx4;
    	for (int j = 0; j < 1000; j++)
    	{ 
    		if (s1[j] == cal_min_array(s1, sizeof(s1) / sizeof(double)))
    		{ 
    			tx1 = (j + 1) / 100.0;//加一是因为寻最小值下标不同；
    		}
    		if (s2[j] == cal_min_array(s2, sizeof(s2) / sizeof(double)))
    		{ 
    			tx2 = (j + 1) / 100.0;
    		}
    		if (s3[j] == cal_min_array(s3, sizeof(s3) / sizeof(double)))
    		{ 
    			tx3 = (j + 1) / 100.0;
    		}
    		if (s4[j] == cal_min_array(s4, sizeof(s4) / sizeof(double)))
    		{ 
    			tx4 = (j + 1) / 100.0;
    		}
    	}
    
    	double t1[1000];
    	double t2[1000];
    	double t3[1000];
    	double t4[1000];
    	double temp1 = tx1, temp2 = tx2, temp3 = tx3, temp4 = tx4;
    	for (int j = 0; j < 1000; j++)
    	{ 
    		t1[j] = temp1;
    		temp1 = temp1 + 0.01;
    		t2[j] = temp2;
    		temp2 = temp2 + 0.01;
    		t3[j] = temp3;
    		temp3 = temp3 + 0.01;
    		t4[j] = temp4;
    		temp4 = temp4 + 0.01;
    	}
    	// lines ::::: 412
    	w = 2 * 3.1415926535897932384626 * 0.1;
    	double theta11[1000];
    	double theta21[1000];
    	double theta31[1000];
    	double qd11[1000];
    	double qd21[1000];
    	double qd31[1000];
    	double qdd11[1000];
    	double qdd21[1000];
    	double qdd31[1000];
    	double theta12[1000];
    	double theta22[1000];
    	double theta32[1000];
    	double qd12[1000];
    	double qd22[1000];
    	double qd32[1000];
    	double qdd12[1000];
    	double qdd22[1000];
    	double qdd32[1000];
    
    	double theta13[1000];
    	double theta43[1000];
    	double theta53[1000];
    	double theta63[1000];
    	double qd13[1000];
    	double qd43[1000];
    	double qd53[1000];
    	double qd63[1000];
    	double qdd13[1000];
    	double qdd43[1000];
    	double qdd53[1000];
    	double qdd63[1000];
    	double theta14[1000];
    	double theta44[1000];
    	double theta54[1000];
    	double theta64[1000];
    	double qd14[1000];
    	double qd44[1000];
    	double qd54[1000];
    	double qd64[1000];
    	double qdd14[1000];
    	double qdd44[1000];
    	double qdd54[1000];
    	double qdd64[1000];
    	for (int k = 0; k < 1000; k++)
    	{ 
    		theta11[k] = x1[0] + x1[1] * sin(w*t1[k]) + x1[6] * cos(w*t1[k]) + x1[2] * sin(2 * w*t1[k]) + x1[7] * cos(2 * w*t1[k]) + x1[3] * sin(3 * w*t1[k]) + x1[8] * cos(3 * w*t1[k]) + x1[4] * sin(4 * w*t1[k]) + x1[9] * cos(4 * w*t1[k]) + x1[5] * sin(5 * w*t1[k]) + x1[10] * cos(5 * w*t1[k]);
    		theta21[k] = x1[11] + x1[12] * sin(w*t1[k]) + x1[17] * cos(w*t1[k]) + x1[13] * sin(2 * w*t1[k]) + x1[18] * cos(2 * w*t1[k]) + x1[14] * sin(3 * w*t1[k]) + x1[19] * cos(3 * w*t1[k]) + x1[15] * sin(4 * w*t1[k]) + x1[20] * cos(4 * w*t1[k]) + x1[16] * sin(5 * w*t1[k]) + x1[21] * cos(5 * w*t1[k]);
    		theta31[k] = (x1[22] + x1[23] * sin(w*t1[k]) + x1[28] * cos(w*t1[k]) + x1[24] * sin(2 * w*t1[k]) + x1[29] * cos(2 * w*t1[k]) + x1[25] * sin(3 * w*t1[k]) + x1[30] * cos(3 * w*t1[k]) + x1[26] * sin(4 * w*t1[k]) + x1[31] * cos(4 * w*t1[k]) + x1[27] * sin(5 * w*t1[k]) + x1[32] * cos(5 * w*t1[k]));
    		qd11[k] = w*x1[1] * cos(w*t1[k]) - w*x1[6] * sin(w*t1[k]) + 2 * w*x1[2] * cos(2 * w*t1[k]) - 2 * w*x1[7] * sin(2 * w*t1[k]) + 3 * w*x1[3] * cos(3 * w*t1[k]) - 3 * w*x1[8] * sin(3 * w*t1[k]) + 4 * w*x1[4] * cos(4 * w*t1[k]) - 4 * w*x1[9] * sin(4 * w*t1[k]) + 5 * w*x1[5] * cos(5 * w*t1[k]) - 5 * w*x1[10] * sin(5 * w*t1[k]);
    		qd21[k] = w*x1[12] * cos(w*t1[k]) - w*x1[17] * sin(w*t1[k]) + 2 * w*x1[13] * cos(2 * w*t1[k]) - 2 * w*x1[18] * sin(2 * w*t1[k]) + 3 * w*x1[14] * cos(3 * w*t1[k]) - 3 * w*x1[19] * sin(3 * w*t1[k]) + 4 * w*x1[15] * cos(4 * w*t1[k]) - 4 * w*x1[20] * sin(4 * w*t1[k]) + 5 * w*x1[16] * cos(5 * w*t1[k]) - 5 * w*x1[21] * sin(5 * w*t1[k]);
    		qd31[k] = w*x1[23] * cos(w*t1[k]) - w*x1[28] * sin(w*t1[k]) + 2 * w*x1[24] * cos(2 * w*t1[k]) - 2 * w*x1[29] * sin(2 * w*t1[k]) + 3 * w*x1[25] * cos(3 * w*t1[k]) - 3 * w*x1[30] * sin(3 * w*t1[k]) + 4 * w*x1[26] * cos(4 * w*t1[k]) - 4 * w*x1[31] * sin(4 * w*t1[k]) + 5 * w*x1[27] * cos(5 * w*t1[k]) - 5 * w*x1[32] * sin(5 * w*t1[k]);
    		qdd11[k] = -w*w*x1[1] * sin(w*t1[k]) - w*w*x1[6] * cos(w*t1[k]) - 4 * w*w*x1[2] * sin(2 * w*t1[k]) - 4 * w*w*x1[7] * cos(2 * w*t1[k]) - 9 * w*w*x1[3] * sin(3 * w*t1[k]) - 9 * w*w*x1[8] * cos(3 * w*t1[k]) - 16 * w*w*x1[4] * sin(4 * w*t1[k]) - 16 * w*w*x1[9] * cos(4 * w*t1[k]) - 25 * w*w*x1[5] * sin(5 * w*t1[k]) - 25 * w*w*x1[10] * cos(5 * w*t1[k]);
    		qdd21[k] = -w*w*x1[12] * sin(w*t1[k]) - w*w*x1[17] * cos(w*t1[k]) - 4 * w*w*x1[13] * sin(2 * w*t1[k]) - 4 * w*w*x1[18] * cos(2 * w*t1[k]) - 9 * w*w*x1[14] * sin(3 * w*t1[k]) - 9 * w*w*x1[19] * cos(3 * w*t1[k]) - 16 * w*w*x1[15] * sin(4 * w*t1[k]) - 16 * w*w*x1[20] * cos(4 * w*t1[k]) - 25 * w*w*x1[16] * sin(5 * w*t1[k]) - 25 * w*w*x1[21] * cos(5 * w*t1[k]);
    		qdd31[k] = -w*w*x1[23] * sin(w*t1[k]) - w*w*x1[28] * cos(w*t1[k]) - 4 * w*w*x1[24] * sin(2 * w*t1[k]) - 4 * w*w*x1[29] * cos(2 * w*t1[k]) - 9 * w*w*x1[25] * sin(3 * w*t1[k]) - 9 * w*w*x1[30] * cos(3 * w*t1[k]) - 16 * w*w*x1[26] * sin(4 * w*t1[k]) - 16 * w*w*x1[31] * cos(4 * w*t1[k]) - 25 * w*w*x1[27] * sin(5 * w*t1[k]) - 25 * w*w*x1[32] * cos(5 * w*t1[k]);
    	}
    	for (int k = 0; k < 1000; k++)
    	{ 
    		theta12[k] = x2[0] + x2[1] * sin(w*t2[k]) + x2[6] * cos(w*t2[k]) + x2[2] * sin(2 * w*t2[k]) + x2[7] * cos(2 * w*t2[k]) + x2[3] * sin(3 * w*t2[k]) + x2[8] * cos(3 * w*t2[k]) + x2[4] * sin(4 * w*t2[k]) + x2[9] * cos(4 * w*t2[k]) + x2[5] * sin(5 * w*t2[k]) + x2[10] * cos(5 * w*t2[k]);
    		theta22[k] = x2[11] + x2[12] * sin(w*t2[k]) + x2[17] * cos(w*t2[k]) + x2[13] * sin(2 * w*t2[k]) + x2[18] * cos(2 * w*t2[k]) + x2[14] * sin(3 * w*t2[k]) + x2[19] * cos(3 * w*t2[k]) + x2[15] * sin(4 * w*t2[k]) + x2[20] * cos(4 * w*t2[k]) + x2[16] * sin(5 * w*t2[k]) + x2[21] * cos(5 * w*t2[k]);
    		theta32[k] = (x2[22] + x2[23] * sin(w*t2[k]) + x2[28] * cos(w*t2[k]) + x2[24] * sin(2 * w*t2[k]) + x2[29] * cos(2 * w*t2[k]) + x2[25] * sin(3 * w*t2[k]) + x2[30] * cos(3 * w*t2[k]) + x2[26] * sin(4 * w*t2[k]) + x2[31] * cos(4 * w*t2[k]) + x2[27] * sin(5 * w*t2[k]) + x2[32] * cos(5 * w*t2[k]));
    		qd12[k] = w*x2[1] * cos(w*t2[k]) - w*x2[6] * sin(w*t2[k]) + 2 * w*x2[2] * cos(2 * w*t2[k]) - 2 * w*x2[7] * sin(2 * w*t2[k]) + 3 * w*x2[3] * cos(3 * w*t2[k]) - 3 * w*x2[8] * sin(3 * w*t2[k]) + 4 * w*x2[4] * cos(4 * w*t2[k]) - 4 * w*x2[9] * sin(4 * w*t2[k]) + 5 * w*x2[5] * cos(5 * w*t2[k]) - 5 * w*x2[10] * sin(5 * w*t2[k]);
    		qd22[k] = w*x2[12] * cos(w*t2[k]) - w*x2[17] * sin(w*t2[k]) + 2 * w*x2[13] * cos(2 * w*t2[k]) - 2 * w*x2[18] * sin(2 * w*t2[k]) + 3 * w*x2[14] * cos(3 * w*t2[k]) - 3 * w*x2[19] * sin(3 * w*t2[k]) + 4 * w*x2[15] * cos(4 * w*t2[k]) - 4 * w*x2[20] * sin(4 * w*t2[k]) + 5 * w*x2[16] * cos(5 * w*t2[k]) - 5 * w*x2[21] * sin(5 * w*t2[k]);
    		qd32[k] = w*x2[23] * cos(w*t2[k]) - w*x2[28] * sin(w*t2[k]) + 2 * w*x2[24] * cos(2 * w*t2[k]) - 2 * w*x2[29] * sin(2 * w*t2[k]) + 3 * w*x2[25] * cos(3 * w*t2[k]) - 3 * w*x2[30] * sin(3 * w*t2[k]) + 4 * w*x2[26] * cos(4 * w*t2[k]) - 4 * w*x2[31] * sin(4 * w*t2[k]) + 5 * w*x2[27] * cos(5 * w*t2[k]) - 5 * w*x2[32] * sin(5 * w*t2[k]);
    		qdd12[k] = -w*w*x2[1] * sin(w*t2[k]) - w*w*x2[6] * cos(w*t2[k]) - 4 * w*w*x2[2] * sin(2 * w*t2[k]) - 4 * w*w*x2[7] * cos(2 * w*t2[k]) - 9 * w*w*x2[3] * sin(3 * w*t2[k]) - 9 * w*w*x2[8] * cos(3 * w*t2[k]) - 16 * w*w*x2[4] * sin(4 * w*t2[k]) - 16 * w*w*x2[9] * cos(4 * w*t2[k]) - 25 * w*w*x2[5] * sin(5 * w*t2[k]) - 25 * w*w*x2[10] * cos(5 * w*t2[k]);
    		qdd22[k] = -w*w*x2[12] * sin(w*t2[k]) - w*w*x2[17] * cos(w*t2[k]) - 4 * w*w*x2[13] * sin(2 * w*t2[k]) - 4 * w*w*x2[18] * cos(2 * w*t2[k]) - 9 * w*w*x2[14] * sin(3 * w*t2[k]) - 9 * w*w*x2[19] * cos(3 * w*t2[k]) - 16 * w*w*x2[15] * sin(4 * w*t2[k]) - 16 * w*w*x2[20] * cos(4 * w*t2[k]) - 25 * w*w*x2[16] * sin(5 * w*t2[k]) - 25 * w*w*x2[21] * cos(5 * w*t2[k]);
    		qdd32[k] = -w*w*x2[23] * sin(w*t2[k]) - w*w*x2[28] * cos(w*t2[k]) - 4 * w*w*x2[24] * sin(2 * w*t2[k]) - 4 * w*w*x2[29] * cos(2 * w*t2[k]) - 9 * w*w*x2[25] * sin(3 * w*t2[k]) - 9 * w*w*x2[30] * cos(3 * w*t2[k]) - 16 * w*w*x2[26] * sin(4 * w*t2[k]) - 16 * w*w*x2[31] * cos(4 * w*t2[k]) - 25 * w*w*x2[27] * sin(5 * w*t2[k]) - 25 * w*w*x2[32] * cos(5 * w*t2[k]);
    	}
    	for (int k = 0; k < 1000; k++)
    	{ 
    		theta13[k] = x3[0] + x3[1] * sin(w*t3[k]) + x3[6] * cos(w*t3[k]) + x3[2] * sin(2 * w*t3[k]) + x3[7] * cos(2 * w*t3[k]) + x3[3] * sin(3 * w*t3[k]) + x3[8] * cos(3 * w*t3[k]) + x3[4] * sin(4 * w*t3[k]) + x3[9] * cos(4 * w*t3[k]) + x3[5] * sin(5 * w*t3[k]) + x3[10] * cos(5 * w*t3[k]);
    		theta43[k] = x3[11] + x3[12] * sin(w*t3[k]) + x3[17] * cos(w*t3[k]) + x3[13] * sin(2 * w*t3[k]) + x3[18] * cos(2 * w*t3[k]) + x3[14] * sin(3 * w*t3[k]) + x3[19] * cos(3 * w*t3[k]) + x3[15] * sin(4 * w*t3[k]) + x3[20] * cos(4 * w*t3[k]) + x3[16] * sin(5 * w*t3[k]) + x3[21] * cos(5 * w*t3[k]);
    		theta53[k] = (x3[22] + x3[23] * sin(w*t3[k]) + x3[28] * cos(w*t3[k]) + x3[24] * sin(2 * w*t3[k]) + x3[29] * cos(2 * w*t3[k]) + x3[25] * sin(3 * w*t3[k]) + x3[30] * cos(3 * w*t3[k]) + x3[26] * sin(4 * w*t3[k]) + x3[31] * cos(4 * w*t3[k]) + x3[27] * sin(5 * w*t3[k]) + x3[32] * cos(5 * w*t3[k]));
    		theta63[k] = (x3[33] + x3[34] * sin(w*t3[k]) + x3[39] * cos(w*t3[k]) + x3[35] * sin(2 * w*t3[k]) + x3[40] * cos(2 * w*t3[k]) + x3[36] * sin(3 * w*t3[k]) + x3[41] * cos(3 * w*t3[k]) + x3[37] * sin(4 * w*t3[k]) + x3[42] * cos(4 * w*t3[k]) + x3[38] * sin(5 * w*t3[k]) + x3[43] * cos(5 * w*t3[k]));
    
    		qd13[k] = w*x3[1] * cos(w*t3[k]) - w*x3[6] * sin(w*t3[k]) + 2 * w*x3[2] * cos(2 * w*t3[k]) - 2 * w*x3[7] * sin(2 * w*t3[k]) + 3 * w*x3[3] * cos(3 * w*t3[k]) - 3 * w*x3[8] * sin(3 * w*t3[k]) + 4 * w*x3[4] * cos(4 * w*t3[k]) - 4 * w*x3[9] * sin(4 * w*t3[k]) + 5 * w*x3[5] * cos(5 * w*t3[k]) - 5 * w*x3[10] * sin(5 * w*t3[k]);
    		qd43[k] = w*x3[12] * cos(w*t3[k]) - w*x3[17] * sin(w*t3[k]) + 2 * w*x3[13] * cos(2 * w*t3[k]) - 2 * w*x3[18] * sin(2 * w*t3[k]) + 3 * w*x3[14] * cos(3 * w*t3[k]) - 3 * w*x3[19] * sin(3 * w*t3[k]) + 4 * w*x3[15] * cos(4 * w*t3[k]) - 4 * w*x3[20] * sin(4 * w*t3[k]) + 5 * w*x3[16] * cos(5 * w*t3[k]) - 5 * w*x3[21] * sin(5 * w*t3[k]);
    		qd53[k] = w*x3[23] * cos(w*t3[k]) - w*x3[28] * sin(w*t3[k]) + 2 * w*x3[24] * cos(2 * w*t3[k]) - 2 * w*x3[29] * sin(2 * w*t3[k]) + 3 * w*x3[25] * cos(3 * w*t3[k]) - 3 * w*x3[30] * sin(3 * w*t3[k]) + 4 * w*x3[26] * cos(4 * w*t3[k]) - 4 * w*x3[31] * sin(4 * w*t3[k]) + 5 * w*x3[27] * cos(5 * w*t3[k]) - 5 * w*x3[32] * sin(5 * w*t3[k]);
    		qd63[k] = w*x3[34] * cos(w*t3[k]) - w*x3[39] * sin(w*t3[k]) + 2 * w*x3[35] * cos(2 * w*t3[k]) - 2 * w*x3[40] * sin(2 * w*t3[k]) + 3 * w*x3[36] * cos(3 * w*t3[k]) - 3 * w*x3[41] * sin(3 * w*t3[k]) + 4 * w*x3[37] * cos(4 * w*t3[k]) - 4 * w*x3[42] * sin(4 * w*t3[k]) + 5 * w*x3[38] * cos(5 * w*t3[k]) - 5 * w*x3[43] * sin(5 * w*t3[k]);
    
    		qdd13[k] = -w*w*x3[1] * sin(w*t3[k]) - w*w*x3[6] * cos(w*t3[k]) - 4 * w*w*x3[2] * sin(2 * w*t3[k]) - 4 * w*w*x3[7] * cos(2 * w*t3[k]) - 9 * w*w*x3[3] * sin(3 * w*t3[k]) - 9 * w*w*x3[8] * cos(3 * w*t3[k]) - 16 * w*w*x3[4] * sin(4 * w*t3[k]) - 16 * w*w*x3[9] * cos(4 * w*t3[k]) - 25 * w*w*x3[5] * sin(5 * w*t3[k]) - 25 * w*w*x3[10] * cos(5 * w*t3[k]);
    		qdd43[k] = -w*w*x3[12] * sin(w*t3[k]) - w*w*x3[17] * cos(w*t3[k]) - 4 * w*w*x3[13] * sin(2 * w*t3[k]) - 4 * w*w*x3[18] * cos(2 * w*t3[k]) - 9 * w*w*x3[14] * sin(3 * w*t3[k]) - 9 * w*w*x3[19] * cos(3 * w*t3[k]) - 16 * w*w*x3[15] * sin(4 * w*t3[k]) - 16 * w*w*x3[20] * cos(4 * w*t3[k]) - 25 * w*w*x3[16] * sin(5 * w*t3[k]) - 25 * w*w*x3[21] * cos(5 * w*t3[k]);
    		qdd53[k] = -w*w*x3[23] * sin(w*t3[k]) - w*w*x3[28] * cos(w*t3[k]) - 4 * w*w*x3[24] * sin(2 * w*t3[k]) - 4 * w*w*x3[29] * cos(2 * w*t3[k]) - 9 * w*w*x3[25] * sin(3 * w*t3[k]) - 9 * w*w*x3[30] * cos(3 * w*t3[k]) - 16 * w*w*x3[26] * sin(4 * w*t3[k]) - 16 * w*w*x3[31] * cos(4 * w*t3[k]) - 25 * w*w*x3[27] * sin(5 * w*t3[k]) - 25 * w*w*x3[32] * cos(5 * w*t3[k]);
    		qdd63[k] = -w*w*x3[34] * sin(w*t3[k]) - w*w*x3[39] * cos(w*t3[k]) - 4 * w*w*x3[35] * sin(2 * w*t3[k]) - 4 * w*w*x3[40] * cos(2 * w*t3[k]) - 9 * w*w*x3[36] * sin(3 * w*t3[k]) - 9 * w*w*x3[41] * cos(3 * w*t3[k]) - 16 * w*w*x3[37] * sin(4 * w*t3[k]) - 16 * w*w*x3[42] * cos(4 * w*t3[k]) - 25 * w*w*x3[38] * sin(5 * w*t3[k]) - 25 * w*w*x3[43] * cos(5 * w*t3[k]);
    	}
    	for (int k = 0; k < 1000; k++)
    	{ 
    		theta14[k] = x4[0] + x4[1] * sin(w*t4[k]) + x4[6] * cos(w*t4[k]) + x4[2] * sin(2 * w*t4[k]) + x4[7] * cos(2 * w*t4[k]) + x4[3] * sin(3 * w*t4[k]) + x4[8] * cos(3 * w*t4[k]) + x4[4] * sin(4 * w*t4[k]) + x4[9] * cos(4 * w*t4[k]) + x4[5] * sin(5 * w*t4[k]) + x4[10] * cos(5 * w*t4[k]);
    		theta44[k] = x4[11] + x4[12] * sin(w*t4[k]) + x4[17] * cos(w*t4[k]) + x4[13] * sin(2 * w*t4[k]) + x4[18] * cos(2 * w*t4[k]) + x4[14] * sin(3 * w*t4[k]) + x4[19] * cos(3 * w*t4[k]) + x4[15] * sin(4 * w*t4[k]) + x4[20] * cos(4 * w*t4[k]) + x4[16] * sin(5 * w*t4[k]) + x4[21] * cos(5 * w*t4[k]);
    		theta54[k] = (x4[22] + x4[23] * sin(w*t4[k]) + x4[28] * cos(w*t4[k]) + x4[24] * sin(2 * w*t4[k]) + x4[29] * cos(2 * w*t4[k]) + x4[25] * sin(3 * w*t4[k]) + x4[30] * cos(3 * w*t4[k]) + x4[26] * sin(4 * w*t4[k]) + x4[31] * cos(4 * w*t4[k]) + x4[27] * sin(5 * w*t4[k]) + x4[32] * cos(5 * w*t4[k]));
    		theta64[k] = (x4[33] + x4[34] * sin(w*t4[k]) + x4[39] * cos(w*t4[k]) + x4[35] * sin(2 * w*t4[k]) + x4[40] * cos(2 * w*t4[k]) + x4[36] * sin(3 * w*t4[k]) + x4[41] * cos(3 * w*t4[k]) + x4[37] * sin(4 * w*t4[k]) + x4[42] * cos(4 * w*t4[k]) + x4[38] * sin(5 * w*t4[k]) + x4[43] * cos(5 * w*t4[k]));
    
    		qd14[k] = w*x4[1] * cos(w*t4[k]) - w*x4[6] * sin(w*t4[k]) + 2 * w*x4[2] * cos(2 * w*t4[k]) - 2 * w*x4[7] * sin(2 * w*t4[k]) + 3 * w*x4[3] * cos(3 * w*t4[k]) - 3 * w*x4[8] * sin(3 * w*t4[k]) + 4 * w*x4[4] * cos(4 * w*t4[k]) - 4 * w*x4[9] * sin(4 * w*t4[k]) + 5 * w*x4[5] * cos(5 * w*t4[k]) - 5 * w*x4[10] * sin(5 * w*t4[k]);
    		qd44[k] = w*x4[12] * cos(w*t4[k]) - w*x4[17] * sin(w*t4[k]) + 2 * w*x4[13] * cos(2 * w*t4[k]) - 2 * w*x4[18] * sin(2 * w*t4[k]) + 3 * w*x4[14] * cos(3 * w*t4[k]) - 3 * w*x4[19] * sin(3 * w*t4[k]) + 4 * w*x4[15] * cos(4 * w*t4[k]) - 4 * w*x4[20] * sin(4 * w*t4[k]) + 5 * w*x4[16] * cos(5 * w*t4[k]) - 5 * w*x4[21] * sin(5 * w*t4[k]);
    		qd54[k] = w*x4[23] * cos(w*t4[k]) - w*x4[28] * sin(w*t4[k]) + 2 * w*x4[24] * cos(2 * w*t4[k]) - 2 * w*x4[29] * sin(2 * w*t4[k]) + 3 * w*x4[25] * cos(3 * w*t4[k]) - 3 * w*x4[30] * sin(3 * w*t4[k]) + 4 * w*x4[26] * cos(4 * w*t4[k]) - 4 * w*x4[31] * sin(4 * w*t4[k]) + 5 * w*x4[27] * cos(5 * w*t4[k]) - 5 * w*x4[32] * sin(5 * w*t4[k]);
    		qd64[k] = w*x4[34] * cos(w*t4[k]) - w*x4[39] * sin(w*t4[k]) + 2 * w*x4[35] * cos(2 * w*t4[k]) - 2 * w*x4[40] * sin(2 * w*t4[k]) + 3 * w*x4[36] * cos(3 * w*t4[k]) - 3 * w*x4[41] * sin(3 * w*t4[k]) + 4 * w*x4[37] * cos(4 * w*t4[k]) - 4 * w*x4[42] * sin(4 * w*t4[k]) + 5 * w*x4[38] * cos(5 * w*t4[k]) - 5 * w*x4[43] * sin(5 * w*t4[k]);
    
    		qdd14[k] = -w*w*x4[1] * sin(w*t4[k]) - w*w*x4[6] * cos(w*t4[k]) - 4 * w*w*x4[2] * sin(2 * w*t4[k]) - 4 * w*w*x4[7] * cos(2 * w*t4[k]) - 9 * w*w*x4[3] * sin(3 * w*t4[k]) - 9 * w*w*x4[8] * cos(3 * w*t4[k]) - 16 * w*w*x4[4] * sin(4 * w*t4[k]) - 16 * w*w*x4[9] * cos(4 * w*t4[k]) - 25 * w*w*x4[5] * sin(5 * w*t4[k]) - 25 * w*w*x4[10] * cos(5 * w*t4[k]);
    		qdd44[k] = -w*w*x4[12] * sin(w*t4[k]) - w*w*x4[17] * cos(w*t4[k]) - 4 * w*w*x4[13] * sin(2 * w*t4[k]) - 4 * w*w*x4[18] * cos(2 * w*t4[k]) - 9 * w*w*x4[14] * sin(3 * w*t4[k]) - 9 * w*w*x4[19] * cos(3 * w*t4[k]) - 16 * w*w*x4[15] * sin(4 * w*t4[k]) - 16 * w*w*x4[20] * cos(4 * w*t4[k]) - 25 * w*w*x4[16] * sin(5 * w*t4[k]) - 25 * w*w*x4[21] * cos(5 * w*t4[k]);
    		qdd54[k] = -w*w*x4[23] * sin(w*t4[k]) - w*w*x4[28] * cos(w*t4[k]) - 4 * w*w*x4[24] * sin(2 * w*t4[k]) - 4 * w*w*x4[29] * cos(2 * w*t4[k]) - 9 * w*w*x4[25] * sin(3 * w*t4[k]) - 9 * w*w*x4[30] * cos(3 * w*t4[k]) - 16 * w*w*x4[26] * sin(4 * w*t4[k]) - 16 * w*w*x4[31] * cos(4 * w*t4[k]) - 25 * w*w*x4[27] * sin(5 * w*t4[k]) - 25 * w*w*x4[32] * cos(5 * w*t4[k]);
    		qdd64[k] = -w*w*x4[34] * sin(w*t4[k]) - w*w*x4[39] * cos(w*t4[k]) - 4 * w*w*x4[35] * sin(2 * w*t4[k]) - 4 * w*w*x4[40] * cos(2 * w*t4[k]) - 9 * w*w*x4[36] * sin(3 * w*t4[k]) - 9 * w*w*x4[41] * cos(3 * w*t4[k]) - 16 * w*w*x4[37] * sin(4 * w*t4[k]) - 16 * w*w*x4[42] * cos(4 * w*t4[k]) - 25 * w*w*x4[38] * sin(5 * w*t4[k]) - 25 * w*w*x4[43] * cos(5 * w*t4[k]);
    	}
    	int sub_ = 1000;
    	int sub_1 = 1000;
    	//一组；
    	double theta123_1[2000];
    	double theta123_2[2000];
    	double theta123_3[2000];
    	double qd123_1[2000];
    	double qd123_2[2000];
    	double qd123_3[2000];
    	double qdd123_1[2000];
    	double qdd123_2[2000];
    	double qdd123_3[2000];
    	// ！！一组；
    	//二组；
    	double theta1456_1[2000];
    	double theta1456_4[2000];
    	double theta1456_5[2000];
    	double theta1456_6[2000];
    	double qd1456_1[2000];
    	double qd1456_4[2000];
    	double qd1456_5[2000];
    	double qd1456_6[2000];
    	double qdd1456_1[2000];
    	double qdd1456_4[2000];
    	double qdd1456_5[2000];
    	double qdd1456_6[2000];
    	// ！！！二组；
    	for (int i = 0; i < sub_ + sub_1; i++)
    	{ 
    		if (i < sub_)
    		{ 
    			theta123_1[i] = theta11[i];
    			theta123_2[i] = theta21[i];
    			theta123_3[i] = theta31[i];
    			qd123_1[i] = qd11[i];
    			qd123_2[i] = qd21[i];
    			qd123_3[i] = qd31[i];
    			qdd123_1[i] = qdd11[i];
    			qdd123_2[i] = qdd21[i];
    			qdd123_3[i] = qdd31[i];
    
    			theta1456_1[i] = theta13[i];
    			theta1456_4[i] = theta43[i];
    			theta1456_5[i] = theta53[i];
    			theta1456_6[i] = theta63[i];
    			qd1456_1[i] = qd13[i];
    			qd1456_4[i] = qd43[i];
    			qd1456_5[i] = qd53[i];
    			qd1456_6[i] = qd63[i];
    			qdd1456_1[i] = qdd13[i];
    			qdd1456_4[i] = qdd43[i];
    			qdd1456_5[i] = qdd53[i];
    			qdd1456_6[i] = qdd63[i];
    		}
    		else if (sub_ < i < sub_ + sub_1)
    		{ 
    			theta123_1[i] = theta12[i - sub_];
    			theta123_2[i] = theta22[i - sub_];
    			theta123_3[i] = theta32[i - sub_];
    			qd123_1[i] = qd12[i - sub_];
    			qd123_2[i] = qd22[i - sub_];
    			qd123_3[i] = qd32[i - sub_];
    			qdd123_1[i] = qdd12[i - sub_];
    			qdd123_2[i] = qdd22[i - sub_];
    			qdd123_3[i] = qdd32[i - sub_];
    
    			theta1456_1[i] = theta14[i - sub_];
    			theta1456_4[i] = theta44[i - sub_];
    			theta1456_5[i] = theta54[i - sub_];
    			theta1456_6[i] = theta64[i - sub_];
    			qd1456_1[i] = qd14[i - sub_];
    			qd1456_4[i] = qd44[i - sub_];
    			qd1456_5[i] = qd54[i - sub_];
    			qd1456_6[i] = qd64[i - sub_];
    			qdd1456_1[i] = qdd14[i - sub_];
    			qdd1456_4[i] = qdd44[i - sub_];
    			qdd1456_5[i] = qdd54[i - sub_];
    			qdd1456_6[i] = qdd64[i - sub_];
    		}
    	}
    	double H_final123[6000][24];
    	double H_final1456[8000][34];
    	int temp_sum = 0;
    	int temp_sum1 = 0;
    	double r2 = 0.136, r3 = 0.113, r4 = 0.1005;
    	for (int i = 0; i < 2000; i++)
    	{ 
    		double H123[3][24] = {  {  qdd123_1[i] / 2 - (cos(2 * theta123_2[i] + 2 * theta123_3[i])*qdd123_1[i]) / 2 + sin(2 * theta123_2[i] + 2 * theta123_3[i])*qd123_1[i] * qd123_2[i] + sin(2 * theta123_2[i] + 2 * theta123_3[i])*qd123_1[i] * qd123_3[i],sin(2 * theta123_2[i] + 2 * theta123_3[i])*qdd123_1[i] + 2 * cos(2 * theta123_2[i] + 2 * theta123_3[i])*qd123_1[i] * qd123_2[i] + 2 * cos(2 * theta123_2[i] + 2 * theta123_3[i])*qd123_1[i] * qd123_3[i],		cos(theta123_2[i] + theta123_3[i])* pow(qd123_2[i],2) + cos(theta123_2[i] + theta123_3[i])* pow(qd123_3[i],2) + sin(theta123_2[i] + theta123_3[i])*qdd123_2[i] + sin(theta123_2[i] + theta123_3[i])*qdd123_3[i] + 2 * cos(theta123_2[i] + theta123_3[i])*qd123_2[i] * qd123_3[i], cos(theta123_2[i] + theta123_3[i])*qdd123_2[i] - sin(theta123_2[i] + theta123_3[i])* pow(qd123_3[i],2) - sin(theta123_2[i] + theta123_3[i])*	pow(qd123_2[i],2) + cos(theta123_2[i] + theta123_3[i])*qdd123_3[i] - 2 * sin(theta123_2[i] + theta123_3[i])*qd123_2[i] * qd123_3[i],	0,	(77 * qdd123_1[i] * cos(2 * theta123_2[i] + theta123_3[i])) / 200 + (77 * cos(theta123_3[i])*qdd123_1[i]) / 200 - (77 * qd123_1[i] * qd123_2[i] * sin(2 * theta123_2[i] + theta123_3[i])) / 100 - (77 * qd123_1[i] * qd123_3[i] * sin(2 * theta123_2[i] + theta123_3[i])) / 200 - (77 * sin(theta123_3[i])*qd123_1[i] * qd123_3[i]) / 200,		-(77 * qdd123_1[i] * sin(2 * theta123_2[i] + theta123_3[i])) / 200 - (77 * sin(theta123_3[i])*qdd123_1[i]) / 200 - (77 * cos(theta123_3[i])*qd123_1[i] * qd123_3[i]) / 200 - (77 * qd123_1[i] * qd123_2[i] * cos(2 * theta123_2[i] + theta123_3[i])) / 100 - (77 * qd123_1[i] * qd123_3[i] * cos(2 * theta123_2[i] + theta123_3[i])) / 200 ,		qdd123_1[i] / 2 - (cos(2 * theta123_2[i])*qdd123_1[i]) / 2 + sin(2 * theta123_2[i])*qd123_1[i] * qd123_2[i],	sin(2 * theta123_2[i])*qdd123_1[i] + 2 * cos(2 * theta123_2[i])*qd123_1[i] * qd123_2[i],		cos(theta123_2[i])*pow(qd123_2[i],2) + sin(theta123_2[i])*qdd123_2[i] ,		cos(theta123_2[i])*qdd123_2[i] - sin(theta123_2[i])*pow(qd123_2[i],2),			0,	 0,		0, qdd123_1[i] ,	qd123_1[i] / fabs(qd123_1[i]) + 1 ,		qd123_1[i] / fabs(qd123_1[i]) - 1 ,	qd123_1[i] ,	0,	0, 0,	0,0,	0 },
    		{  -(sin(2 * theta123_2[i] + 2 * theta123_3[i])* pow(qd123_1[i],2)) / 2,		-cos(2 * theta123_2[i] + 2 * theta123_3[i])* pow(qd123_1[i],2),		sin(theta123_2[i] + theta123_3[i])*qdd123_1[i],		cos(theta123_2[i] + theta123_3[i])*qdd123_1[i],		qdd123_2[i] + qdd123_3[i],		(981 * cos(theta123_2[i] + theta123_3[i])) / 100 + (77 * cos(theta123_3[i])*qdd123_2[i]) / 100 + (77 * cos(theta123_3[i])*qdd123_3[i]) / 200 + (77 * pow(qd123_1[i],2) * sin(2 * theta123_2[i] + theta123_3[i])) / 200 - (77 * sin(theta123_3[i])*pow(qd123_3[i],2)) / 200 - (77 * sin(theta123_3[i])*qd123_2[i] * qd123_3[i]) / 100	,		(77 * pow(qd123_1[i],2)* cos(2 * theta123_2[i] + theta123_3[i])) / 200 - (981 * sin(theta123_2[i] + theta123_3[i])) / 100 - (77 * sin(theta123_3[i])*qdd123_2[i]) / 100 - (77 * sin(theta123_3[i])*qdd123_3[i]) / 200 - (77 * cos(theta123_3[i])* pow(qd123_3[i],2)) / 200 - (77 * cos(theta123_3[i])*qd123_2[i] * qd123_3[i]) / 100 ,	-(sin(2 * theta123_2[i])*pow(qd123_1[i],2)) / 2		,		-cos(2 * theta123_2[i])*pow(qd123_1[i],2),		sin(theta123_2[i])*qdd123_1[i],		cos(theta123_2[i])*qdd123_1[i],			qdd123_2[i],		(981 * cos(theta123_2[i])) / 100	,		-(981 * sin(theta123_2[i])) / 100,			0,	0,	0,		0,		qd123_2[i] / fabs(qd123_2[i]) + 1,		qd123_2[i] / fabs(qd123_2[i]) - 1,		qd123_2[i],		0,	0,	0 },
    		{  -(sin(2 * theta123_2[i] + 2 * theta123_3[i])* pow(qd123_1[i],2)) / 2,		-cos(2 * theta123_2[i] + 2 * theta123_3[i])* pow(qd123_1[i],2),		sin(theta123_2[i] + theta123_3[i])*qdd123_1[i],		cos(theta123_2[i] + theta123_3[i])*qdd123_1[i],		qdd123_2[i] + qdd123_3[i],		(981 * cos(theta123_2[i] + theta123_3[i])) / 100 + (77 * cos(theta123_3[i])*qdd123_2[i]) / 200 + (77 * pow(qd123_1[i],2) * sin(2 * theta123_2[i] + theta123_3[i])) / 400 + (77 * sin(theta123_3[i])* pow(qd123_1[i],2)) / 400 + (77 * sin(theta123_3[i])* pow(qd123_2[i],2)) / 200 ,			 (77 * pow(qd123_1[i],2) * cos(2 * theta123_2[i] + theta123_3[i])) / 400 - (981 * sin(theta123_2[i] + theta123_3[i])) / 100 - (77 * sin(theta123_3[i])*qdd123_2[i]) / 200 + (77 * cos(theta123_3[i])* pow(qd123_1[i],2)) / 400 + (77 * cos(theta123_3[i])*pow(qd123_2[i],2)) / 200,		0,	 0,			0,		0,		0,	0,	0,	0,	0	,	0,		0,0,	0,	0,	qd123_3[i] / fabs(qd123_3[i]) + 1,	qd123_3[i] / fabs(qd123_3[i]) - 1,	qd123_3[i] }
    		};
    		for (int k = 0; k < 3; k++)
    		{ 
    			for (int j = 0; j < 24; j++)
    			{ 
    				H_final123[temp_sum][j] = H123[k][j];
    			}
    			temp_sum++;
    		}
    		double H1456[4][34] = {  {  cos(theta1456_4[i])*qdd1456_5[i] + pow(cos(theta1456_4[i]),2)*qdd1456_1[i] - cos(theta1456_4[i])*pow(cos(theta1456_6[i]),2)*qdd1456_5[i] - sin(theta1456_4[i])*qd1456_4[i] * qd1456_5[i] - pow(cos(theta1456_4[i]),2)*pow(cos(theta1456_6[i]),2)*qdd1456_1[i] + pow(cos(theta1456_5[i]),2)*pow(cos(theta1456_6[i]),2)*qdd1456_1[i] - sin(2 * theta1456_4[i])*qd1456_1[i] * qd1456_4[i] + 2 * pow(cos(theta1456_6[i]),2)*sin(theta1456_4[i])*qd1456_4[i] * qd1456_5[i] - pow(cos(theta1456_4[i]),2)*pow(cos(theta1456_5[i]),2)*pow(cos(theta1456_6[i]),2)*qdd1456_1[i] + cos(theta1456_4[i])*sin(theta1456_5[i])*qd1456_4[i] * qd1456_6[i] - cos(theta1456_5[i])*sin(theta1456_4[i])*qd1456_5[i] * qd1456_6[i] - cos(theta1456_4[i])*cos(theta1456_5[i])*pow(cos(theta1456_6[i]),2)*sin(theta1456_5[i])*pow(qd1456_4[i],2) + 2 * cos(theta1456_4[i])*pow(cos(theta1456_6[i]),2)*sin(theta1456_4[i])*qd1456_1[i] * qd1456_4[i] - 2 * cos(theta1456_5[i])*pow(cos(theta1456_6[i]),2)*sin(theta1456_5[i])*qd1456_1[i] * qd1456_5[i] + 2 * pow(cos(theta1456_4[i]),2)*cos(theta1456_6[i])*sin(theta1456_6[i])*qd1456_1[i] * qd1456_6[i] - 2 * pow(cos(theta1456_5[i]),2)*cos(theta1456_6[i])*sin(theta1456_6[i])*qd1456_1[i] * qd1456_6[i] - 2 * cos(theta1456_4[i])*pow(cos(theta1456_6[i]),2)*sin(theta1456_5[i])*qd1456_4[i] * qd1456_6[i] + 2 * cos(theta1456_5[i])*pow(cos(theta1456_6[i]),2)*sin(theta1456_4[i])*qd1456_5[i] * qd1456_6[i] - cos(theta1456_4[i])*cos(theta1456_6[i])*sin(theta1456_5[i])*sin(theta1456_6[i])*qdd1456_4[i] + cos(theta1456_5[i])*cos(theta1456_6[i])*sin(theta1456_4[i])*sin(theta1456_6[i])*qdd1456_5[i] - 2 * pow(cos(theta1456_5[i]),2)*pow(cos(theta1456_6[i]),2)*sin(theta1456_4[i])*qd1456_4[i] * qd1456_5[i] - cos(theta1456_5[i])*pow(cos(theta1456_6[i]),2)*sin(theta1456_4[i])*sin(theta1456_5[i])*qdd1456_4[i] + cos(theta1456_6[i])*sin(theta1456_4[i])*sin(theta1456_5[i])*sin(theta1456_6[i])*pow(qd1456_4[i],2) - cos(theta1456_6[i])*sin(theta1456_4[i])*sin(theta1456_5[i])*sin(theta1456_6[i])*pow(qd1456_5[i],2) - 2 * cos(theta1456_4[i])*cos(theta1456_5[i])*sin(theta1456_4[i])*qd1456_1[i] * qd1456_6[i] - 2 * cos(theta1456_5[i])*cos(theta1456_6[i])*sin(theta1456_6[i])*qd1456_1[i] * qd1456_4[i] + 2 * cos(theta1456_4[i])*cos(theta1456_6[i])*sin(theta1456_6[i])*qd1456_5[i] * qd1456_6[i] + 4 * cos(theta1456_4[i])*cos(theta1456_5[i])*pow(cos(theta1456_6[i]),2)*sin(theta1456_4[i])*qd1456_1[i] * qd1456_6[i] + 4 * pow(cos(theta1456_4[i]),2)*cos(theta1456_5[i])*cos(theta1456_6[i])*sin(theta1456_6[i])*qd1456_1[i] * qd1456_4[i] + 2 * cos(theta1456_4[i])*cos(theta1456_5[i])*cos(theta1456_6[i])*sin(theta1456_4[i])*sin(theta1456_6[i])*qdd1456_1[i] + 2 * cos(theta1456_4[i])*pow(cos(theta1456_5[i]),2)*pow(cos(theta1456_6[i]),2)*sin(theta1456_4[i])*qd1456_1[i] * qd1456_4[i] + 2 * pow(cos(theta1456_4[i]),2)*cos(theta1456_5[i])*pow(cos(theta1456_6[i]),2)*sin(theta1456_5[i])*qd1456_1[i] * qd1456_5[i] + 2 * pow(cos(theta1456_4[i]),2)*pow(cos(theta1456_5[i]),2)*cos(theta1456_6[i])*sin(theta1456_6[i])*qd1456_1[i] * qd1456_6[i] - 2 * cos(theta1456_4[i])*cos(theta1456_6[i])*sin(theta1456_4[i])*sin(theta1456_5[i])*sin(theta1456_6[i])*qd1456_1[i] * qd1456_5[i] + 2 * cos(theta1456_5[i])*cos(theta1456_6[i])*sin(theta1456_4[i])*sin(theta1456_5[i])*sin(theta1456_6[i])*qd1456_4[i] * qd1456_6[i],															qdd1456_4[i] / 4 + (sin(2 * theta1456_4[i])*pow(qd1456_1[i],2) / 8 - (cos(2 * theta1456_5[i])*qdd1456_4[i]) / 4 + (cos(2 * theta1456_6[i])*qdd1456_4[i]) / 4 - (sin(theta1456_4[i])*sin(2 * theta1456_5[i])*qdd1456_1[i]) / 4 - (sin(theta1456_5[i])*sin(2 * theta1456_6[i])*qdd1456_5[i]) / 2 + (sin(theta1456_4[i])*qd1456_1[i] * qd1456_5[i]) / 2 - (cos(2 * theta1456_5[i])*cos(2 * theta1456_6[i])*qdd1456_4[i]) / 4 - (cos(theta1456_5[i])*sin(2 * theta1456_6[i])*pow(qd1456_5[i],2)) / 2 + (sin(2 * theta1456_5[i])*qd1456_4[i] * qd1456_5[i]) / 2 - (sin(2 * theta1456_6[i])*qd1456_4[i] * qd1456_6[i]) / 2 - (cos(2 * theta1456_5[i])*sin(2 * theta1456_4[i])*pow(qd1456_1[i],2)) / 8 - (3 * cos(2 * theta1456_6[i])*sin(2 * theta1456_4[i])*pow(qd1456_1[i],2)) / 8 - (sin(theta1456_4[i])*cos(2 * theta1456_5[i])*qd1456_1[i] * qd1456_5[i]) / 2 - (sin(theta1456_4[i])*cos(2 * theta1456_6[i])*qd1456_1[i] * qd1456_5[i]) / 2 - sin(theta1456_5[i])*cos(2 * theta1456_6[i])*qd1456_5[i] * qd1456_6[i] - (cos(theta1456_5[i])*cos(2 * theta1456_4[i])*sin(2 * theta1456_6[i])*pow(qd1456_1[i],2)) / 2 + (cos(2 * theta1456_6[i])*sin(2 * theta1456_5[i])*qd1456_4[i] * qd1456_5[i]) / 2 + (cos(2 * theta1456_5[i])*sin(2 * theta1456_6[i])*qd1456_4[i] * qd1456_6[i]) / 2 - (cos(theta1456_4[i])*sin(theta1456_5[i])*sin(2 * theta1456_6[i])*qdd1456_1[i]) / 2 - (cos(2 * theta1456_5[i])*cos(2 * theta1456_6[i])*sin(2 * theta1456_4[i])*pow(qd1456_1[i],2)) / 8 - (sin(theta1456_4[i])*cos(2 * theta1456_6[i])*sin(2 * theta1456_5[i])*qdd1456_1[i]) / 4 - cos(theta1456_4[i])*cos(theta1456_5[i])*sin(2 * theta1456_6[i])*qd1456_1[i] * qd1456_5[i] - cos(theta1456_4[i])*sin(theta1456_5[i])*cos(2 * theta1456_6[i])*qd1456_1[i] * qd1456_6[i] - (sin(theta1456_4[i])*cos(2 * theta1456_5[i])*cos(2 * theta1456_6[i])*qd1456_1[i] * qd1456_5[i]) / 2 + (sin(theta1456_4[i])*sin(2 * theta1456_5[i])*sin(2 * theta1456_6[i])*qd1456_1[i] * qd1456_6[i]) / 2),					qdd1456_5[i] / 2 + (sin(2 * theta1456_5[i])*pow(qd1456_1[i],2)) / 4 - (sin(2 * theta1456_5[i])*pow(qd1456_4[i], 2)) / 4 + (cos(theta1456_4[i])*qdd1456_1[i]) / 2 - (cos(2 * theta1456_6[i])*qdd1456_5[i]) / 2 - (cos(theta1456_4[i])*cos(2 * theta1456_6[i])*qdd1456_1[i]) / 2 - (sin(theta1456_5[i])*sin(2 * theta1456_6[i])*qdd1456_4[i]) / 2 - sin(theta1456_4[i])*qd1456_1[i] * qd1456_4[i] + sin(2 * theta1456_6[i])*qd1456_5[i] * qd1456_6[i] + cos(theta1456_4[i])*sin(2 * theta1456_6[i])*qd1456_1[i] * qd1456_6[i] - sin(theta1456_5[i])*cos(2 * theta1456_6[i])*qd1456_4[i] * qd1456_6[i] + pow(cos(theta1456_5[i]),2) * sin(theta1456_4[i])*qd1456_1[i] * qd1456_4[i] + (cos(theta1456_5[i])*sin(theta1456_4[i])*sin(2 * theta1456_6[i])*qdd1456_1[i]) / 2 + (cos(theta1456_5[i])*sin(theta1456_5[i])*cos(2 * theta1456_6[i])*pow(qd1456_1[i],2) / 2 - (cos(theta1456_5[i])*sin(theta1456_5[i])*cos(2 * theta1456_6[i])*pow(qd1456_4[i],2)) / 2 - pow(cos(theta1456_4[i]),2) * cos(theta1456_5[i])*sin(theta1456_5[i])*pow(qd1456_1[i],2) / 2 + (cos(theta1456_4[i])*sin(theta1456_4[i])*sin(theta1456_5[i])*sin(2 * theta1456_6[i])*pow(qd1456_1[i],2) / 2 + cos(theta1456_4[i])*cos(theta1456_5[i])*sin(2 * theta1456_6[i])*qd1456_1[i] * qd1456_4[i] + cos(theta1456_5[i])*sin(theta1456_4[i])*cos(2 * theta1456_6[i])*qd1456_1[i] * qd1456_6[i] - pow(cos(theta1456_4[i]),2) * cos(theta1456_5[i])*sin(theta1456_5[i])*cos(2 * theta1456_6[i])*pow(qd1456_1[i],2)) / 2 + pow(cos(theta1456_5[i]),2) * sin(theta1456_4[i])*cos(2 * theta1456_6[i])*qd1456_1[i] * qd1456_4[i]),						(sin(2 * theta1456_6[i])*pow(qd1456_4[i],2)) / 2 - (sin(2 * theta1456_6[i])*pow(qd1456_5[i],2)) / 2 - sin(theta1456_5[i])*qd1456_4[i] * qd1456_5[i] + 2 * pow(cos(theta1456_6[i]),2) * sin(theta1456_5[i])*qd1456_4[i] * qd1456_5[i] + cos(theta1456_4[i])*cos(theta1456_5[i])*sin(theta1456_4[i])*pow(qd1456_1[i],2) - cos(theta1456_4[i])*sin(theta1456_5[i])*qd1456_1[i] * qd1456_4[i] + cos(theta1456_5[i])*sin(theta1456_4[i])*qd1456_1[i] * qd1456_5[i] - pow(cos(theta1456_4[i]),2) * cos(theta1456_6[i])*sin(theta1456_6[i])*pow(qd1456_1[i],2) + pow(cos(theta1456_5[i]),2) * cos(theta1456_6[i])*sin(theta1456_6[i])*pow(qd1456_1[i],2) - pow(cos(theta1456_5[i]),2) * cos(theta1456_6[i])*sin(theta1456_6[i])*pow(qd1456_4[i],2) - 2 * cos(theta1456_4[i])*cos(theta1456_5[i])*pow(cos(theta1456_6[i]),2) * sin(theta1456_4[i])*pow(qd1456_1[i],2) + 2 * cos(theta1456_4[i])*pow(cos(theta1456_6[i]),2) * sin(theta1456_5[i])*qd1456_1[i] * qd1456_4[i] - 2 * cos(theta1456_5[i])*pow(cos(theta1456_6[i]),2) * sin(theta1456_4[i])*qd1456_1[i] * qd1456_5[i] - pow(cos(theta1456_4[i]),2) * pow(cos(theta1456_5[i]),2) * cos(theta1456_6[i])*sin(theta1456_6[i])*pow(qd1456_1[i],2) - 2 * cos(theta1456_4[i])*cos(theta1456_6[i])*sin(theta1456_6[i])*qd1456_1[i] * qd1456_5[i] - 2 * cos(theta1456_5[i])*cos(theta1456_6[i])*sin(theta1456_4[i])*sin(theta1456_5[i])*sin(theta1456_6[i])*qd1456_1[i] * qd1456_4[i],										sin(theta1456_4[i])*sin(theta1456_5[i])*pow(qd1456_5[i],2) - sin(theta1456_4[i])*sin(theta1456_5[i])*pow(qd1456_4[i],2) + 2 * cos(theta1456_5[i])*qd1456_1[i] * qd1456_4[i] - 2 * cos(theta1456_4[i])*qd1456_5[i] * qd1456_6[i] - 2 * pow(cos(theta1456_4[i]),2) * qd1456_1[i] * qd1456_6[i] + 2 * pow(cos(theta1456_5[i]),2) * qd1456_1[i] * qd1456_6[i] + cos(theta1456_4[i])*sin(theta1456_5[i])*qdd1456_4[i] - cos(theta1456_5[i])*sin(theta1456_4[i])*qdd1456_5[i] - 4 * pow(cos(theta1456_4[i]),2) * cos(theta1456_5[i])*qd1456_1[i] * qd1456_4[i] - 4 * cos(theta1456_5[i])*pow(cos(theta1456_6[i]),2) * qd1456_1[i] * qd1456_4[i] + 4 * cos(theta1456_4[i])*pow(cos(theta1456_6[i]),2) * qd1456_5[i] * qd1456_6[i] - 2 * cos(theta1456_4[i])*cos(theta1456_5[i])*sin(theta1456_4[i])*qdd1456_1[i] + 2 * cos(theta1456_4[i])*cos(theta1456_6[i])*sin(theta1456_6[i])*qdd1456_5[i] - 2 * pow(cos(theta1456_4[i]),2) * pow(cos(theta1456_5[i]),2) * qd1456_1[i] * qd1456_6[i] + 4 * pow(cos(theta1456_4[i]),2) * pow(cos(theta1456_6[i]),2) * qd1456_1[i] * qd1456_6[i] - 4 * pow(cos(theta1456_5[i]),2) * pow(cos(theta1456_6[i]),2) * qd1456_1[i] * qd1456_6[i] + 2 * pow(cos(theta1456_4[i]),2) * cos(theta1456_6[i])*sin(theta1456_6[i])*qdd1456_1[i] - 2 * pow(cos(theta1456_5[i]),2) * cos(theta1456_6[i])*sin(theta1456_6[i])*qdd1456_1[i] - 2 * cos(theta1456_4[i])*pow(cos(theta1456_6[i]),2) * sin(theta1456_5[i])*qdd1456_4[i] + 2 * cos(theta1456_5[i])*pow(cos(theta1456_6[i]),2) * sin(theta1456_4[i])*qdd1456_5[i] + 2 * pow(cos(theta1456_6[i]),2) * sin(theta1456_4[i])*sin(theta1456_5[i])*pow(qd1456_4[i],2) - 2 * pow(cos(theta1456_6[i]),2) * sin(theta1456_4[i])*sin(theta1456_5[i])*pow(qd1456_5[i] ,2) + 4 * pow(cos(theta1456_4[i]),2) * pow(cos(theta1456_5[i]),2) * pow(cos(theta1456_6[i]),2) * qd1456_1[i] * qd1456_6[i] + 2 * cos(theta1456_4[i])*sin(theta1456_4[i])*sin(theta1456_5[i])*qd1456_1[i] * qd1456_5[i] - 2 * cos(theta1456_5[i])*sin(theta1456_4[i])*sin(theta1456_5[i])*qd1456_4[i] * qd1456_6[i] - 4 * cos(theta1456_6[i])*sin(theta1456_4[i])*sin(theta1456_6[i])*qd1456_4[i] * qd1456_5[i] + 2 * pow(cos(theta1456_4[i]),2) * pow(cos(theta1456_5[i]),2) * cos(theta1456_6[i])*sin(theta1456_6[i])*qdd1456_1[i] + 8 * pow(cos(theta1456_4[i]),2) * cos(theta1456_5[i])*pow(cos(theta1456_6[i]),2) * qd1456_1[i] * qd1456_4[i] + 4 * cos(theta1456_4[i])*cos(theta1456_5[i])*pow(cos(theta1456_6[i]),2) * sin(theta1456_4[i])*qdd1456_1[i] + 2 * cos(theta1456_4[i])*cos(theta1456_5[i])*cos(theta1456_6[i])*sin(theta1456_5[i])*sin(theta1456_6[i])*pow(qd1456_4[i],2) - 4 * cos(theta1456_4[i])*cos(theta1456_6[i])*sin(theta1456_4[i])*sin(theta1456_6[i])*qd1456_1[i] * qd1456_4[i] + 4 * cos(theta1456_5[i])*cos(theta1456_6[i])*sin(theta1456_5[i])*sin(theta1456_6[i])*qd1456_1[i] * qd1456_5[i] + 4 * cos(theta1456_4[i])*cos(theta1456_6[i])*sin(theta1456_5[i])*sin(theta1456_6[i])*qd1456_4[i] * qd1456_6[i] - 4 * cos(theta1456_5[i])*cos(theta1456_6[i])*sin(theta1456_4[i])*sin(theta1456_6[i])*qd1456_5[i] * qd1456_6[i] - 4 * cos(theta1456_4[i])*pow(cos(theta1456_6[i]),2) * sin(theta1456_4[i])*sin(theta1456_5[i])*qd1456_1[i] * qd1456_5[i] + 4 * cos(theta1456_5[i])*pow(cos(theta1456_6[i]),2) * sin(theta1456_4[i])*sin(theta1456_5[i])*qd1456_4[i] * qd1456_6[i] + 4 * pow(cos(theta1456_5[i]),2) * cos(theta1456_6[i])*sin(theta1456_4[i])*sin(theta1456_6[i])*qd1456_4[i] * qd1456_5[i] + 2 * cos(theta1456_5[i])*cos(theta1456_6[i])*sin(theta1456_4[i])*sin(theta1456_5[i])*sin(theta1456_6[i])*qdd1456_4[i] - 8 * cos(theta1456_4[i])*cos(theta1456_5[i])*cos(theta1456_6[i])*sin(theta1456_4[i])*sin(theta1456_6[i])*qd1456_1[i] * qd1456_6[i] - 4 * cos(theta1456_4[i])*pow(cos(theta1456_5[i]),2) * cos(theta1456_6[i])*sin(theta1456_4[i])*sin(theta1456_6[i])*qd1456_1[i] * qd1456_4[i] - 4 * pow(cos(theta1456_4[i]),2) * cos(theta1456_5[i])*cos(theta1456_6[i])*sin(theta1456_5[i])*sin(theta1456_6[i])*qd1456_1[i] * qd1456_5[i],						(3 * sin(2 * theta1456_4[i])*sin(2 * theta1456_6[i])*(pow(qd1456_1[i],2)) / 4 - (sin(2 * theta1456_6[i])*qdd1456_4[i]) / 2 - sin(theta1456_5[i])*cos(2 * theta1456_6[i])*qdd1456_5[i] - cos(theta1456_5[i])*cos(2 * theta1456_6[i])*pow(qd1456_5[i],2) + (cos(2 * theta1456_5[i])*sin(2 * theta1456_6[i])*qdd1456_4[i]) / 2 - cos(2 * theta1456_6[i])*qd1456_4[i] * qd1456_6[i] + sin(theta1456_4[i])*sin(2 * theta1456_6[i])*qd1456_1[i] * qd1456_5[i] + 2 * sin(theta1456_5[i])*sin(2 * theta1456_6[i])*qd1456_5[i] * qd1456_6[i] - cos(theta1456_5[i])*cos(2 * theta1456_4[i])*cos(2 * theta1456_6[i])*pow(qd1456_1[i],2) + cos(2 * theta1456_5[i])*cos(2 * theta1456_6[i])*qd1456_4[i] * qd1456_6[i] - cos(theta1456_4[i])*sin(theta1456_5[i])*cos(2 * theta1456_6[i])*qdd1456_1[i] - sin(2 * theta1456_5[i])*sin(2 * theta1456_6[i])*qd1456_4[i] * qd1456_5[i] + (cos(2 * theta1456_5[i])*sin(2 * theta1456_4[i])*sin(2 * theta1456_6[i])*pow(qd1456_1[i],2)) / 4 + (sin(theta1456_4[i])*sin(2 * theta1456_5[i])*sin(2 * theta1456_6[i])*qdd1456_1[i]) / 2 - 2 * cos(theta1456_4[i])*cos(theta1456_5[i])*cos(2 * theta1456_6[i])*qd1456_1[i] * qd1456_5[i] + 2 * cos(theta1456_4[i])*sin(theta1456_5[i])*sin(2 * theta1456_6[i])*qd1456_1[i] * qd1456_6[i] + sin(theta1456_4[i])*cos(2 * theta1456_5[i])*sin(2 * theta1456_6[i])*qd1456_1[i] * qd1456_5[i] + sin(theta1456_4[i])*cos(2 * theta1456_6[i])*sin(2 * theta1456_5[i])*qd1456_1[i] * qd1456_6[i]),			sin(2 * theta1456_6[i])*qdd1456_5[i] + cos(theta1456_4[i])*sin(2 * theta1456_6[i])*qdd1456_1[i] - sin(theta1456_5[i])*cos(2 * theta1456_6[i])*qdd1456_4[i] + 2 * cos(2 * theta1456_6[i])*qd1456_5[i] * qd1456_6[i] + 2 * cos(theta1456_4[i])*cos(2 * theta1456_6[i])*qd1456_1[i] * qd1456_6[i] + 2 * sin(theta1456_5[i])*sin(2 * theta1456_6[i])*qd1456_4[i] * qd1456_6[i] + cos(theta1456_5[i])*sin(theta1456_4[i])*cos(2 * theta1456_6[i])*qdd1456_1[i] - cos(theta1456_5[i])*sin(theta1456_5[i])*sin(2 * theta1456_6[i])*pow(qd1456_1[i],2) + cos(theta1456_5[i])*sin(theta1456_5[i])*sin(2 * theta1456_6[i])*pow(qd1456_4[i],2) + cos(theta1456_4[i])*sin(theta1456_4[i])*sin(theta1456_5[i])*cos(2 * theta1456_6[i])*pow(qd1456_1[i],2) + 2 * cos(theta1456_4[i])*cos(theta1456_5[i])*cos(2 * theta1456_6[i])*qd1456_1[i] * qd1456_4[i] - 2 * cos(theta1456_5[i])*sin(theta1456_4[i])*sin(2 * theta1456_6[i])*qd1456_1[i] * qd1456_6[i] + pow(cos(theta1456_4[i]),2)*cos(theta1456_5[i])*sin(theta1456_5[i])*sin(2 * theta1456_6[i])*pow(qd1456_1[i],2) - 2 * pow(cos(theta1456_5[i]),2)*sin(theta1456_4[i])*sin(2 * theta1456_6[i])*qd1456_1[i] * qd1456_4[i],					pow(qd1456_5[i],2) - pow(qd1456_4[i],2) + pow(cos(theta1456_4[i]),2)*pow(qd1456_1[i],2) - pow(cos(theta1456_5[i]),2)*pow(qd1456_1[i],2) + pow(cos(theta1456_5[i]),2)*pow(qd1456_4[i],2) + 2 * pow(cos(theta1456_6[i]),2)*pow(qd1456_4[i],2) - 2 * pow(cos(theta1456_6[i]),2)*pow(qd1456_5[i],2) + 2 * cos(theta1456_4[i])*qd1456_1[i] * qd1456_5[i] + pow(cos(theta1456_4[i]),2)*pow(cos(theta1456_5[i]),2)*pow(qd1456_1[i],2) - 2 * pow(cos(theta1456_4[i]),2)*pow(cos(theta1456_6[i]),2)*pow(qd1456_1[i],2) + 2 * pow(cos(theta1456_5[i]),2)*pow(cos(theta1456_6[i]),2)*pow(qd1456_1[i],2) - 2 * pow(cos(theta1456_5[i]),2)*pow(cos(theta1456_6[i]),2)*pow(qd1456_4[i],2) - 4 * cos(theta1456_4[i])*pow(cos(theta1456_6[i]),2)*qd1456_1[i] * qd1456_5[i] - 2 * pow(cos(theta1456_4[i]),2)*pow(cos(theta1456_5[i]),2)*pow(cos(theta1456_6[i]),2)*pow(qd1456_1[i],2) + 2 * cos(theta1456_5[i])*sin(theta1456_4[i])*sin(theta1456_5[i])*qd1456_1[i] * qd1456_4[i] - 4 * cos(theta1456_6[i])*sin(theta1456_5[i])*sin(theta1456_6[i])*qd1456_4[i] * qd1456_5[i] + 4 * cos(theta1456_4[i])*cos(theta1456_5[i])*cos(theta1456_6[i])*sin(theta1456_4[i])*sin(theta1456_6[i])*pow(qd1456_1[i],2) - 4 * cos(theta1456_4[i])*cos(theta1456_6[i])*sin(theta1456_5[i])*sin(theta1456_6[i])*qd1456_1[i] * qd1456_4[i] + 4 * cos(theta1456_5[i])*cos(theta1456_6[i])*sin(theta1456_4[i])*sin(theta1456_6[i])*qd1456_1[i] * qd1456_5[i] - 4 * cos(theta1456_5[i])*pow(cos(theta1456_6[i]),2)*sin(theta1456_4[i])*sin(theta1456_5[i])*qd1456_1[i] * qd1456_4[i],					cos(theta1456_4[i])*cos(theta1456_6[i])*pow(qd1456_6[i],2) - cos(theta1456_4[i])*cos(theta1456_6[i])*pow(qd1456_4[i],2) - 2 * cos(theta1456_6[i])*qd1456_1[i] * qd1456_5[i] - cos(theta1456_6[i])*sin(theta1456_4[i])*qdd1456_4[i] + cos(theta1456_4[i])*sin(theta1456_6[i])*qdd1456_6[i] + 2 * pow(cos(theta1456_4[i]),2)*cos(theta1456_6[i])*qd1456_1[i] * qd1456_5[i] + 4 * pow(cos(theta1456_5[i]),2)*cos(theta1456_6[i])*qd1456_1[i] * qd1456_5[i] + 2 * cos(theta1456_5[i])*cos(theta1456_6[i])*sin(theta1456_5[i])*qdd1456_1[i] + cos(theta1456_4[i])*cos(theta1456_5[i])*sin(theta1456_6[i])*qdd1456_4[i] + cos(theta1456_5[i])*cos(theta1456_6[i])*sin(theta1456_4[i])*qdd1456_6[i] + sin(theta1456_4[i])*sin(theta1456_5[i])*sin(theta1456_6[i])*qdd1456_5[i] + 2 * pow(cos(theta1456_5[i]),2)*cos(theta1456_6[i])*sin(theta1456_4[i])*qdd1456_4[i] - cos(theta1456_5[i])*sin(theta1456_4[i])*sin(theta1456_6[i])*pow(qd1456_4[i],2) + cos(theta1456_5[i])*sin(theta1456_4[i])*sin(theta1456_6[i])*pow(qd1456_5[i],2) - cos(theta1456_5[i])*sin(theta1456_4[i])*sin(theta1456_6[i])*pow(qd1456_6[i],2) - 2 * sin(theta1456_5[i])*sin(theta1456_6[i])*qd1456_1[i] * qd1456_4[i] + 2 * cos(theta1456_4[i])*pow(cos(theta1456_5[i]),2)*cos(theta1456_6[i])*pow(qd1456_4[i],2) - 2 * cos(theta1456_5[i])*sin(theta1456_5[i])*sin(theta1456_6[i])*qd1456_1[i] * qd1456_6[i] + 4 * pow(cos(theta1456_4[i]),2)*sin(theta1456_5[i])*sin(theta1456_6[i])*qd1456_1[i] * qd1456_4[i] - 2 * pow(cos(theta1456_5[i]),2)*sin(theta1456_4[i])*sin(theta1456_6[i])*qd1456_4[i] * qd1456_6[i] + 2 * cos(theta1456_4[i])*sin(theta1456_4[i])*sin(theta1456_5[i])*sin(theta1456_6[i])*qdd1456_1[i] - 4 * pow(cos(theta1456_4[i]),2)*pow(cos(theta1456_5[i]),2)*cos(theta1456_6[i])*qd1456_1[i] * qd1456_5[i] - 2 * pow(cos(theta1456_4[i]),2)*cos(theta1456_5[i])*cos(theta1456_6[i])*sin(theta1456_5[i])*qdd1456_1[i] + 2 * cos(theta1456_4[i])*cos(theta1456_5[i])*cos(theta1456_6[i])*qd1456_4[i] * qd1456_6[i] + 2 * cos(theta1456_4[i])*cos(theta1456_5[i])*sin(theta1456_4[i])*sin(theta1456_6[i])*qd1456_1[i] * qd1456_5[i] + 2 * cos(theta1456_4[i])*cos(theta1456_6[i])*sin(theta1456_4[i])*sin(theta1456_5[i])*qd1456_1[i] * qd1456_6[i] - 4 * cos(theta1456_5[i])*cos(theta1456_6[i])*sin(theta1456_4[i])*sin(theta1456_5[i])*qd1456_4[i] * qd1456_5[i] + 2 * pow(cos(theta1456_4[i]),2)*cos(theta1456_5[i])*sin(theta1456_5[i])*sin(theta1456_6[i])*qd1456_1[i] * qd1456_6[i] + 4 * cos(theta1456_4[i])*cos(theta1456_5[i])*cos(theta1456_6[i])*sin(theta1456_4[i])*sin(theta1456_5[i])*qd1456_1[i] * qd1456_4[i],				sin(theta1456_5[i])*sin(theta1456_6[i])*pow(qd1456_6[i],2) - sin(theta1456_5[i])*sin(theta1456_6[i])*pow(qd1456_5[i],2) - cos(theta1456_6[i])*sin(2 * theta1456_5[i])*qdd1456_4[i] + cos(theta1456_5[i])*sin(theta1456_6[i])*qdd1456_5[i] - cos(theta1456_6[i])*sin(theta1456_5[i])*qdd1456_6[i] - 2 * cos(theta1456_6[i])*cos(2 * theta1456_5[i])*qd1456_4[i] * qd1456_5[i] + cos(theta1456_4[i])*cos(theta1456_5[i])*sin(theta1456_6[i])*qdd1456_1[i] + sin(theta1456_6[i])*sin(2 * theta1456_5[i])*qd1456_4[i] * qd1456_6[i] - (cos(theta1456_6[i])*sin(2 * theta1456_4[i])*sin(2 * theta1456_5[i])*pow(qd1456_1[i],2)) / 2 + cos(theta1456_6[i])*sin(theta1456_4[i])*cos(2 * theta1456_5[i])*qdd1456_1[i] + sin(theta1456_4[i])*sin(theta1456_6[i])*qd1456_1[i] * qd1456_6[i] - sin(theta1456_5[i])*sin(theta1456_6[i])*cos(2 * theta1456_4[i])*pow(qd1456_1[i],2) - 2 * cos(theta1456_4[i])*sin(theta1456_5[i])*sin(theta1456_6[i])*qd1456_1[i] * qd1456_5[i] - 2 * cos(theta1456_6[i])*sin(theta1456_4[i])*sin(2 * theta1456_5[i])*qd1456_1[i] * qd1456_5[i] - sin(theta1456_4[i])*sin(theta1456_6[i])*cos(2 * theta1456_5[i])*qd1456_1[i] * qd1456_6[i],					sin(theta1456_6[i])*qdd1456_6[i] + cos(theta1456_6[i])*pow(qd1456_6[i],2) - (cos(theta1456_6[i])*pow(qd1456_1[i],2)*(2 * pow(cos(theta1456_5[i]),2) - 1)) / 2 + cos(theta1456_6[i])*pow(qd1456_4[i],2)*(2 * pow(cos(theta1456_5[i]),2) - 1) + cos(theta1456_5[i])*sin(theta1456_6[i])*qdd1456_4[i] + sin(theta1456_4[i])*sin(theta1456_5[i])*sin(theta1456_6[i])*qdd1456_1[i] + 2 * cos(theta1456_5[i])*cos(theta1456_6[i])*qd1456_4[i] * qd1456_6[i] + (cos(theta1456_6[i])*pow(qd1456_1[i],2)*(2 * pow(cos(theta1456_4[i]),2) - 1)*(2 * pow(cos(theta1456_5[i]),2) - 1)) / 2 + 2 * cos(theta1456_4[i])*sin(theta1456_5[i])*sin(theta1456_6[i])*qd1456_1[i] * qd1456_4[i] + 2 * cos(theta1456_6[i])*sin(theta1456_4[i])*sin(theta1456_5[i])*qd1456_1[i] * qd1456_6[i] - cos(theta1456_4[i])*cos(theta1456_5[i])*sin(theta1456_4[i])*sin(theta1456_6[i])*pow(qd1456_1[i],2) + 4 * cos(theta1456_5[i])*cos(theta1456_6[i])*sin(theta1456_4[i])*sin(theta1456_5[i])*qd1456_1[i] * qd1456_4[i],							sin(theta1456_6[i])*qdd1456_5[i] + cos(theta1456_4[i])*sin(theta1456_6[i])*qdd1456_1[i] - cos(theta1456_6[i])*sin(theta1456_5[i])*qdd1456_4[i] + cos(theta1456_5[i])*cos(theta1456_6[i])*sin(theta1456_4[i])*qdd1456_1[i] + cos(theta1456_5[i])*sin(theta1456_5[i])*sin(theta1456_6[i])*pow(qd1456_1[i],2) - cos(theta1456_5[i])*sin(theta1456_5[i])*sin(theta1456_6[i])*pow(qd1456_4[i],2) - 2 * cos(theta1456_5[i])*cos(theta1456_6[i])*qd1456_4[i] * qd1456_5[i] - 2 * sin(theta1456_4[i])*sin(theta1456_6[i])*qd1456_1[i] * qd1456_4[i] - 2 * cos(theta1456_6[i])*sin(theta1456_4[i])*sin(theta1456_5[i])*qd1456_1[i] * qd1456_5[i] - pow(cos(theta1456_4[i]),2)*cos(theta1456_5[i])*sin(theta1456_5[i])*sin(theta1456_6[i])*pow(qd1456_1[i],2) + 2 * pow(cos(theta1456_5[i]),2)*sin(theta1456_4[i])*sin(theta1456_6[i])*qd1456_1[i] * qd1456_4[i] - cos(theta1456_4[i])*cos(theta1456_6[i])*sin(theta1456_4[i])*sin(theta1456_5[i])*pow(qd1456_1[i],2),			cos(theta1456_4[i])*sin(theta1456_6[i])*pow(qd1456_4[i],2) - cos(theta1456_4[i])*sin(theta1456_6[i])*pow(qd1456_6[i],2) + 2 * sin(theta1456_6[i])*qd1456_1[i] * qd1456_5[i] + cos(theta1456_4[i])*cos(theta1456_6[i])*qdd1456_6[i] + sin(theta1456_4[i])*sin(theta1456_6[i])*qdd1456_4[i] + cos(theta1456_4[i])*cos(theta1456_5[i])*cos(theta1456_6[i])*qdd1456_4[i] - 2 * pow(cos(theta1456_4[i]),2)*sin(theta1456_6[i])*qd1456_1[i] * qd1456_5[i] - 4 * pow(cos(theta1456_5[i]),2)*sin(theta1456_6[i])*qd1456_1[i] * qd1456_5[i] - 2 * cos(theta1456_5[i])*sin(theta1456_5[i])*sin(theta1456_6[i])*qdd1456_1[i] + cos(theta1456_6[i])*sin(theta1456_4[i])*sin(theta1456_5[i])*qdd1456_5[i] - cos(theta1456_5[i])*sin(theta1456_4[i])*sin(theta1456_6[i])*qdd1456_6[i] - cos(theta1456_5[i])*cos(theta1456_6[i])*sin(theta1456_4[i])*pow(qd1456_4[i],2) + cos(theta1456_5[i])*cos(theta1456_6[i])*sin(theta1456_4[i])*pow(qd1456_5[i],2) - cos(theta1456_5[i])*cos(theta1456_6[i])*sin(theta1456_4[i])*pow(qd1456_6[i],2) - 2 * pow(cos(theta1456_5[i]),2)*sin(theta1456_4[i])*sin(theta1456_6[i])*qdd1456_4[i] - 2 * cos(theta1456_6[i])*sin(theta1456_5[i])*qd1456_1[i] * qd1456_4[i] - 2 * cos(theta1456_4[i])*pow(cos(theta1456_5[i]),2)*sin(theta1456_6[i])*pow(qd1456_4[i],2) + 4 * pow(cos(theta1456_4[i]),2)*cos(theta1456_6[i])*sin(theta1456_5[i])*qd1456_1[i] * qd1456_4[i] - 2 * pow(cos(theta1456_5[i]),2)*cos(theta1456_6[i])*sin(theta1456_4[i])*qd1456_4[i] * qd1456_6[i] + 2 * cos(theta1456_4[i])*cos(theta1456_6[i])*sin(theta1456_4[i])*sin(theta1456_5[i])*qdd1456_1[i] + 4 * pow(cos(theta1456_4[i]),2)*pow(cos(theta1456_5[i]),2)*sin(theta1456_6[i])*qd1456_1[i] * qd1456_5[i] + 2 * pow(cos(theta1456_4[i]),2)*cos(theta1456_5[i])*sin(theta1456_5[i])*sin(theta1456_6[i])*qdd1456_1[i] - 2 * cos(theta1456_5[i])*cos(theta1456_6[i])*sin(theta1456_5[i])*qd1456_1[i] * qd1456_6[i] - 2 * cos(theta1456_4[i])*cos(theta1456_5[i])*sin(theta1456_6[i])*qd1456_4[i] * qd1456_6[i] + 2 * cos(theta1456_4[i])*cos(theta1456_5[i])*cos(theta1456_6[i])*sin(theta1456_4[i])*qd1456_1[i] * qd1456_5[i] - 2 * cos(theta1456_4[i])*sin(theta1456_4[i])*sin(theta1456_5[i])*sin(theta1456_6[i])*qd1456_1[i] * qd1456_6[i] + 4 * cos(theta1456_5[i])*sin(theta1456_4[i])*sin(theta1456_5[i])*sin(theta1456_6[i])*qd1456_4[i] * qd1456_5[i] + 2 * pow(cos(theta1456_4[i]),2)*cos(theta1456_5[i])*cos(theta1456_6[i])*sin(theta1456_5[i])*qd1456_1[i] * qd1456_6[i] - 4 * cos(theta1456_4[i])*cos(theta1456_5[i])*sin(theta1456_4[i])*sin(theta1456_5[i])*sin(theta1456_6[i])*qd1456_1[i] * qd1456_4[i],				cos(theta1456_6[i])*sin(theta1456_5[i])*pow(qd1456_6[i],2) - cos(theta1456_6[i])*sin(theta1456_5[i])*pow(qd1456_5[i],2) + sin(theta1456_6[i])*sin(2 * theta1456_5[i])*qdd1456_4[i] + cos(theta1456_5[i])*cos(theta1456_6[i])*qdd1456_5[i] + sin(theta1456_5[i])*sin(theta1456_6[i])*qdd1456_6[i] + cos(theta1456_4[i])*cos(theta1456_5[i])*cos(theta1456_6[i])*qdd1456_1[i] + 2 * sin(theta1456_6[i])*cos(2 * theta1456_5[i])*qd1456_4[i] * qd1456_5[i] + cos(theta1456_6[i])*sin(2 * theta1456_5[i])*qd1456_4[i] * qd1456_6[i] + (sin(theta1456_6[i])*sin(2 * theta1456_4[i])*sin(2 * theta1456_5[i])*pow(qd1456_1[i],2)) / 2 - sin(theta1456_4[i])*sin(theta1456_6[i])*cos(2 * theta1456_5[i])*qdd1456_1[i] + cos(theta1456_6[i])*sin(theta1456_4[i])*qd1456_1[i] * qd1456_6[i] - cos(theta1456_6[i])*sin(theta1456_5[i])*cos(2 * theta1456_4[i])*pow(qd1456_1[i],2) - cos(theta1456_6[i])*sin(theta1456_4[i])*cos(2 * theta1456_5[i])*qd1456_1[i] * qd1456_6[i] + 2 * sin(theta1456_4[i])*sin(theta1456_6[i])*sin(2 * theta1456_5[i])*qd1456_1[i] * qd1456_5[i] - 2 * cos(theta1456_4[i])*cos(theta1456_6[i])*sin(theta1456_5[i])*qd1456_1[i] * qd1456_5[i],		cos(theta1456_6[i])*qdd1456_6[i] - sin(theta1456_6[i])*pow(qd1456_6[i],2) + (sin(theta1456_6[i])*pow(qd1456_1[i],2)*(2 * pow(cos(theta1456_5[i]),2) - 1)) / 2 - sin(theta1456_6[i])*pow(qd1456_4[i],2)*(2 * pow(cos(theta1456_5[i]),2) - 1) + cos(theta1456_5[i])*cos(theta1456_6[i])*qdd1456_4[i] + cos(theta1456_6[i])*sin(theta1456_4[i])*sin(theta1456_5[i])*qdd1456_1[i] - 2 * cos(theta1456_5[i])*sin(theta1456_6[i])*qd1456_4[i] * qd1456_6[i] - (sin(theta1456_6[i])*pow(qd1456_1[i],2)*(2 * pow(cos(theta1456_4[i]),2) - 1)*(2 * pow(cos(theta1456_5[i]),2) - 1)) / 2 - 2 * sin(theta1456_4[i])*sin(theta1456_5[i])*sin(theta1456_6[i])*qd1456_1[i] * qd1456_6[i] - cos(theta1456_4[i])*cos(theta1456_5[i])*cos(theta1456_6[i])*sin(theta1456_4[i])*pow(qd1456_1[i],2) + 2 * cos(theta1456_4[i])*cos(theta1456_6[i])*sin(theta1456_5[i])*qd1456_1[i] * qd1456_4[i] - 4 * cos(theta1456_5[i])*sin(theta1456_4[i])*sin(theta1456_5[i])*sin(theta1456_6[i])*qd1456_1[i] * qd1456_4[i],				cos(theta1456_6[i])*qdd1456_5[i] + cos(theta1456_4[i])*cos(theta1456_6[i])*qdd1456_1[i] + sin(theta1456_5[i])*sin(theta1456_6[i])*qdd1456_4[i] - cos(theta1456_5[i])*sin(theta1456_4[i])*sin(theta1456_6[i])*qdd1456_1[i] + cos(theta1456_5[i])*cos(theta1456_6[i])*sin(theta1456_5[i])*pow(qd1456_1[i],2) - cos(theta1456_5[i])*cos(theta1456_6[i])*sin(theta1456_5[i])*pow(qd1456_4[i],2) - 2 * cos(theta1456_6[i])*sin(theta1456_4[i])*qd1456_1[i] * qd1456_4[i] + 2 * cos(theta1456_5[i])*sin(theta1456_6[i])*qd1456_4[i] * qd1456_5[i] + 2 * sin(theta1456_4[i])*sin(theta1456_5[i])*sin(theta1456_6[i])*qd1456_1[i] * qd1456_5[i] - pow(cos(theta1456_4[i]),2)*cos(theta1456_5[i])*cos(theta1456_6[i])*sin(theta1456_5[i])*pow(qd1456_1[i],2) + 2 * pow(cos(theta1456_5[i]),2)*cos(theta1456_6[i])*sin(theta1456_4[i])*qd1456_1[i] * qd1456_4[i] + cos(theta1456_4[i])*sin(theta1456_4[i])*sin(theta1456_5[i])*sin(theta1456_6[i])*pow(qd1456_1[i],2),				pow(cos(theta1456_4[i]),2)*pow(cos(theta1456_5[i]),2)*qdd1456_1[i] - pow(cos(theta1456_5[i]),2)*qdd1456_1[i] - sin(theta1456_4[i])*qd1456_4[i] * qd1456_5[i] - pow(cos(theta1456_4[i]),2)*pow(qdd1456_1[i],2) + sin(2 * theta1456_4[i])*qd1456_1[i] * qd1456_4[i] + sin(2 * theta1456_5[i])*qd1456_1[i] * qd1456_5[i] + sin(theta1456_4[i])*sin(theta1456_5[i])*qdd1456_6[i] + 2 * pow(cos(theta1456_5[i]),2)*sin(theta1456_4[i])*qd1456_4[i] * qd1456_5[i] + cos(theta1456_5[i])*sin(theta1456_4[i])*sin(theta1456_5[i])*qdd1456_4[i] + cos(theta1456_4[i])*cos(theta1456_5[i])*sin(theta1456_5[i])*pow(qd1456_4[i],2) + cos(theta1456_4[i])*sin(theta1456_5[i])*qd1456_4[i] * qd1456_6[i] + cos(theta1456_5[i])*sin(theta1456_4[i])*qd1456_5[i] * qd1456_6[i] - 2 * cos(theta1456_4[i])*pow(cos(theta1456_5[i]),2)*sin(theta1456_4[i])*qd1456_1[i] * qd1456_4[i] - 2 * pow(cos(theta1456_4[i]),2)*cos(theta1456_5[i])*sin(theta1456_5[i])*qd1456_1[i] * qd1456_5[i],				qdd1456_4[i] / 2 - (sin(2 * theta1456_4[i])*pow(qd1456_1[i],2)) / 4 + cos(theta1456_5[i])*qdd1456_6[i] + (cos(2 * theta1456_5[i])*qdd1456_4[i]) / 2 + (sin(theta1456_4[i])*sin(2 * theta1456_5[i])*qdd1456_1[i]) / 2 - sin(theta1456_5[i])*qd1456_5[i] * qd1456_6[i] - sin(2 * theta1456_5[i])*qd1456_4[i] * qd1456_5[i] + (cos(2 * theta1456_5[i])*sin(2 * theta1456_4[i])*pow(qd1456_1[i],2)) / 4 + sin(theta1456_4[i])*cos(2 * theta1456_5[i])*qd1456_1[i] * qd1456_5[i] - cos(theta1456_4[i])*sin(theta1456_5[i])*qd1456_1[i] * qd1456_6[i],			(sin(2 * theta1456_5[i])*pow(qd1456_4[i],2)) / 2 - (sin(2 * theta1456_5[i])*pow(qd1456_1[i],2)) / 4 + sin(theta1456_5[i])*qd1456_4[i] * qd1456_6[i] + (cos(2 * theta1456_4[i])*sin(2 * theta1456_5[i])*pow(qd1456_1[i],2)) / 4 - sin(theta1456_4[i])*cos(2 * theta1456_5[i])*qd1456_1[i] * qd1456_4[i] - cos(theta1456_5[i])*sin(theta1456_4[i])*qd1456_1[i] * qd1456_6[i],				qdd1456_6[i] + cos(theta1456_5[i])*qdd1456_4[i] - sin(theta1456_5[i])*qd1456_4[i] * qd1456_5[i] + sin(theta1456_4[i])*sin(theta1456_5[i])*qdd1456_1[i] + cos(theta1456_4[i])*sin(theta1456_5[i])*qd1456_1[i] * qd1456_4[i] + cos(theta1456_5[i])*sin(theta1456_4[i])*qd1456_1[i] * qd1456_5[i],						2 * r3*sin(theta1456_6[i])*qdd1456_1[i] - 2 * r2*cos(theta1456_6[i])*sin(theta1456_5[i])*qdd1456_1[i] - r2*cos(theta1456_4[i])*sin(theta1456_6[i])*qdd1456_4[i] - r2*cos(theta1456_6[i])*sin(theta1456_4[i])*qdd1456_6[i] + r4*cos(theta1456_6[i])*sin(theta1456_4[i])*qdd1456_4[i] - r4*cos(theta1456_4[i])*sin(theta1456_6[i])*qdd1456_6[i] + r4*cos(theta1456_4[i])*cos(theta1456_6[i])*pow(qd1456_4[i],2) - r4*cos(theta1456_4[i])*cos(theta1456_6[i])*pow(qd1456_6[i],2) - 2 * r3*pow(cos(theta1456_4[i]),2)*sin(theta1456_6[i])*qdd1456_1[i] + r2*sin(theta1456_4[i])*sin(theta1456_6[i])*pow(qd1456_4[i],2) + r2*sin(theta1456_4[i])*sin(theta1456_6[i])*pow(qd1456_6[i],2) + 2 * r3*cos(theta1456_6[i])*qd1456_1[i] * qd1456_6[i] + 2 * r4*cos(theta1456_6[i])*qd1456_1[i] * qd1456_5[i] - 2 * r2*cos(theta1456_5[i])*cos(theta1456_6[i])*qd1456_1[i] * qd1456_5[i] + 2 * r3*cos(theta1456_5[i])*cos(theta1456_6[i])*qd1456_1[i] * qd1456_4[i] - 2 * r2*cos(theta1456_4[i])*cos(theta1456_6[i])*qd1456_4[i] * qd1456_6[i] + 2 * r2*sin(theta1456_5[i])*sin(theta1456_6[i])*qd1456_1[i] * qd1456_6[i] + 2 * r4*sin(theta1456_5[i])*sin(theta1456_6[i])*qd1456_1[i] * qd1456_4[i] - 2 * r4*cos(theta1456_4[i])*pow(cos(theta1456_5[i]),2)*cos(theta1456_6[i])*pow(qd1456_4[i],2) - 2 * r3*pow(cos(theta1456_4[i]),2)*cos(theta1456_6[i])*qd1456_1[i] * qd1456_6[i] - 2 * r4*pow(cos(theta1456_4[i]),2)*cos(theta1456_6[i])*qd1456_1[i] * qd1456_5[i] - 4 * r4*pow(cos(theta1456_5[i]),2)*cos(theta1456_6[i])*qd1456_1[i] * qd1456_5[i] - r2*cos(theta1456_5[i])*cos(theta1456_6[i])*sin(theta1456_4[i])*qdd1456_4[i] - 2 * r4*cos(theta1456_5[i])*cos(theta1456_6[i])*sin(theta1456_5[i])*qdd1456_1[i] - r2*cos(theta1456_4[i])*cos(theta1456_6[i])*sin(theta1456_5[i])*qdd1456_5[i] + r3*cos(theta1456_4[i])*cos(theta1456_6[i])*sin(theta1456_5[i])*qdd1456_4[i] - r2*cos(theta1456_4[i])*cos(theta1456_5[i])*sin(theta1456_6[i])*qdd1456_6[i] - r3*cos(theta1456_5[i])*cos(theta1456_6[i])*sin(theta1456_4[i])*qdd1456_5[i] - r4*cos(theta1456_4[i])*cos(theta1456_5[i])*sin(theta1456_6[i])*qdd1456_4[i] - r4*cos(theta1456_5[i])*cos(theta1456_6[i])*sin(theta1456_4[i])*qdd1456_6[i] + r3*sin(theta1456_4[i])*sin(theta1456_5[i])*sin(theta1456_6[i])*qdd1456_6[i] - r4*sin(theta1456_4[i])*sin(theta1456_5[i])*sin(theta1456_6[i])*qdd1456_5[i] - r2*cos(theta1456_4[i])*cos(theta1456_5[i])*cos(theta1456_6[i])*pow(qd1456_4[i],2) - r2*cos(theta1456_4[i])*cos(theta1456_5[i])*cos(theta1456_6[i])*pow(qd1456_5[i],2) - r2*cos(theta1456_4[i])*cos(theta1456_5[i])*cos(theta1456_6[i])*pow(qd1456_6[i],2) - 2 * r4*pow(cos(theta1456_5[i]),2)*cos(theta1456_6[i])*sin(theta1456_4[i])*qdd1456_4[i] - r3*cos(theta1456_6[i])*sin(theta1456_4[i])*sin(theta1456_5[i])*pow(qd1456_4[i],2) + r3*cos(theta1456_6[i])*sin(theta1456_4[i])*sin(theta1456_5[i])*pow(qd1456_5[i],2) + r4*cos(theta1456_5[i])*sin(theta1456_4[i])*sin(theta1456_6[i])*pow(qd1456_4[i],2) + r3*cos(theta1456_6[i])*sin(theta1456_4[i])*sin(theta1456_5[i])*pow(qd1456_6[i],2) - r4*cos(theta1456_5[i])*sin(theta1456_4[i])*sin(theta1456_6[i])*pow(qd1456_5[i],2) + r4*cos(theta1456_5[i])*sin(theta1456_4[i])*sin(theta1456_6[i])*pow(qd1456_6[i],2) + 4 * r4*pow(cos(theta1456_4[i]),2)*pow(cos(theta1456_5[i]),2)*cos(theta1456_6[i])*qd1456_1[i] * qd1456_5[i] + 2 * r4*pow(cos(theta1456_4[i]),2)*cos(theta1456_5[i])*cos(theta1456_6[i])*sin(theta1456_5[i])*qdd1456_1[i] - 2 * r4*cos(theta1456_4[i])*cos(theta1456_5[i])*cos(theta1456_6[i])*qd1456_4[i] * qd1456_6[i] + 4 * r3*cos(theta1456_4[i])*sin(theta1456_4[i])*sin(theta1456_6[i])*qd1456_1[i] * qd1456_4[i] + 2 * r2*cos(theta1456_6[i])*sin(theta1456_4[i])*sin(theta1456_5[i])*qd1456_4[i] * qd1456_5[i] + 2 * r2*cos(theta1456_5[i])*sin(theta1456_4[i])*sin(theta1456_6[i])*qd1456_4[i] * qd1456_6[i] + 2 * r4*cos(theta1456_5[i])*sin(theta1456_5[i])*sin(theta1456_6[i])*qd1456_1[i] * qd1456_6[i] + 2 * r2*cos(theta1456_4[i])*sin(theta1456_5[i])*sin(theta1456_6[i])*qd1456_5[i] * qd1456_6[i] + 2 * r3*cos(theta1456_5[i])*sin(theta1456_4[i])*sin(theta1456_6[i])*qd1456_5[i] * qd1456_6[i] - 4 * r3*pow(cos(theta1456_4[i]),2)*cos(theta1456_5[i])*cos(theta1456_6[i])*qd1456_1[i] * qd1456_4[i] - 2 * r3*cos(theta1456_4[i])*cos(theta1456_5[i])*cos(theta1456_6[i])*sin(theta1456_4[i])*qdd1456_1[i] - 4 * r4*pow(cos(theta1456_4[i]),2)*sin(theta1456_5[i])*sin(theta1456_6[i])*qd1456_1[i] * qd1456_4[i] + 2 * r4*pow(cos(theta1456_5[i]),2)*sin(theta1456_4[i])*sin(theta1456_6[i])*qd1456_4[i] * qd1456_6[i] - 2 * r4*cos(theta1456_4[i])*sin(theta1456_4[i])*sin(theta1456_5[i])*sin(theta1456_6[i])*qdd1456_1[i] - 2 * r4*pow(cos(theta1456_4[i]),2)*cos(theta1456_5[i])*sin(theta1456_5[i])*sin(theta1456_6[i])*qd1456_1[i] * qd1456_6[i] + 2 * r3*cos(theta1456_4[i])*cos(theta1456_6[i])*sin(theta1456_4[i])*sin(theta1456_5[i])*qd1456_1[i] * qd1456_5[i] + 2 * r3*cos(theta1456_4[i])*cos(theta1456_5[i])*sin(theta1456_4[i])*sin(theta1456_6[i])*qd1456_1[i] * qd1456_6[i] - 2 * r4*cos(theta1456_4[i])*cos(theta1456_5[i])*sin(theta1456_4[i])*sin(theta1456_6[i])*qd1456_1[i] * qd1456_5[i] - 2 * r4*cos(theta1456_4[i])*cos(theta1456_6[i])*sin(theta1456_4[i])*sin(theta1456_5[i])*qd1456_1[i] * qd1456_6[i] + 4 * r4*cos(theta1456_5[i])*cos(theta1456_6[i])*sin(theta1456_4[i])*sin(theta1456_5[i])*qd1456_4[i] * qd1456_5[i] - 4 * r4*cos(theta1456_4[i])*cos(theta1456_5[i])*cos(theta1456_6[i])*sin(theta1456_4[i])*sin(theta1456_5[i])*qd1456_1[i] * qd1456_4[i],					2 * r3*sin(theta1456_6[i])*qdd1456_4[i] - (981 * sin(theta1456_4[i])*sin(theta1456_6[i])) / 100 + (981 * cos(theta1456_4[i])*cos(theta1456_5[i])*cos(theta1456_6[i])) / 100 - r3*sin(theta1456_6[i])*sin(2 * theta1456_4[i])*pow(qd1456_1[i],2) - r2*cos(theta1456_4[i])*sin(theta1456_6[i])*qdd1456_1[i] + r3*cos(theta1456_6[i])*sin(theta1456_5[i])*qdd1456_5[i] + r3*cos(theta1456_5[i])*sin(theta1456_6[i])*qdd1456_6[i] - r4*cos(theta1456_5[i])*sin(theta1456_6[i])*qdd1456_5[i] + r4*cos(theta1456_6[i])*sin(theta1456_5[i])*qdd1456_6[i] + r3*cos(theta1456_5[i])*cos(theta1456_6[i])*pow(qd1456_5[i],2) + r3*cos(theta1456_5[i])*cos(theta1456_6[i])*pow(qd1456_6[i],2) + r4*cos(theta1456_6[i])*sin(2 * theta1456_5[i])*qdd1456_4[i] + r4*sin(theta1456_5[i])*sin(theta1456_6[i])*pow(qd1456_5[i],2) - r4*sin(theta1456_5[i])*sin(theta1456_6[i])*pow(qd1456_6[i],2) + 2 * r3*cos(theta1456_6[i])*qd1456_4[i] * qd1456_6[i] - r4*sin(theta1456_4[i])*sin(theta1456_6[i])*qd1456_1[i] * qd1456_6[i] - 2 * r3*sin(theta1456_5[i])*sin(theta1456_6[i])*qd1456_5[i] * qd1456_6[i] + r3*cos(theta1456_5[i])*cos(theta1456_6[i])*cos(2 * theta1456_4[i])*pow(qd1456_1[i],2) + r4*sin(theta1456_5[i])*sin(theta1456_6[i])*cos(2 * theta1456_4[i])*pow(qd1456_1[i],2) + 2 * r4*cos(theta1456_6[i])*cos(2 * theta1456_5[i])*qd1456_4[i] * qd1456_5[i] - r2*cos(theta1456_5[i])*cos(theta1456_6[i])*sin(theta1456_4[i])*qdd1456_1[i] + r3*cos(theta1456_4[i])*cos(theta1456_6[i])*sin(theta1456_5[i])*qdd1456_1[i] - r4*cos(theta1456_4[i])*cos(theta1456_5[i])*sin(theta1456_6[i])*qdd1456_1[i] - r4*sin(theta1456_6[i])*sin(2 * theta1456_5[i])*qd1456_4[i] * qd1456_6[i] + (r4*cos(theta1456_6[i])*sin(2 * theta1456_4[i])*sin(2 * theta1456_5[i])*pow(qd1456_1[i],2)) / 2 - r4*cos(theta1456_6[i])*sin(theta1456_4[i])*cos(2 * theta1456_5[i])*qdd1456_1[i] + 2 * r3*cos(theta1456_4[i])*cos(theta1456_5[i])*cos(theta1456_6[i])*qd1456_1[i] * qd1456_5[i] - 2 * r3*cos(theta1456_4[i])*sin(theta1456_5[i])*sin(theta1456_6[i])*qd1456_1[i] * qd1456_6[i] + 2 * r4*cos(theta1456_4[i])*sin(theta1456_5[i])*sin(theta1456_6[i])*qd1456_1[i] * qd1456_5[i] + 2 * r4*cos(theta1456_6[i])*sin(theta1456_4[i])*sin(2 * theta1456_5[i])*qd1456_1[i] * qd1456_5[i] + r4*sin(theta1456_4[i])*sin(theta1456_6[i])*cos(2 * theta1456_5[i])*qd1456_1[i] * qd1456_6[i],				r3*cos(theta1456_6[i])*sin(theta1456_5[i])*qdd1456_4[i] - (981 * cos(theta1456_6[i])*sin(theta1456_4[i])*sin(theta1456_5[i])) / 100 - r4*cos(theta1456_6[i])*pow(qd1456_6[i],2) - r4*sin(theta1456_6[i])*qdd1456_6[i] - r4*cos(theta1456_5[i])*sin(theta1456_6[i])*qdd1456_4[i] + r2*cos(theta1456_5[i])*cos(theta1456_6[i])*pow(qd1456_1[i],2) + (r4*cos(theta1456_6[i])*cos(2 * theta1456_5[i])*pow(qd1456_1[i],2)) / 2 - r4*cos(theta1456_6[i])*cos(2 * theta1456_5[i])*pow(qd1456_4[i],2) - 2 * r4*cos(theta1456_5[i])*cos(theta1456_6[i])*qd1456_4[i] * qd1456_6[i] - (r3*cos(theta1456_6[i])*sin(theta1456_5[i])*sin(2 * theta1456_4[i])*pow(qd1456_1[i],2)) / 2 + (r4*cos(theta1456_5[i])*sin(theta1456_6[i])*sin(2 * theta1456_4[i])*pow(qd1456_1[i],2)) / 2 - r2*cos(theta1456_4[i])*cos(theta1456_6[i])*sin(theta1456_5[i])*qdd1456_1[i] - r3*cos(theta1456_5[i])*cos(theta1456_6[i])*sin(theta1456_4[i])*qdd1456_1[i] - (r4*cos(theta1456_6[i])*cos(2 * theta1456_4[i])*cos(2 * theta1456_5[i])*pow(qd1456_1[i],2)) / 2 - r4*sin(theta1456_4[i])*sin(theta1456_5[i])*sin(theta1456_6[i])*qdd1456_1[i] - 2 * r3*cos(theta1456_4[i])*cos(theta1456_5[i])*cos(theta1456_6[i])*qd1456_1[i] * qd1456_4[i] - 2 * r4*cos(theta1456_4[i])*sin(theta1456_5[i])*sin(theta1456_6[i])*qd1456_1[i] * qd1456_4[i] - 2 * r4*cos(theta1456_6[i])*sin(theta1456_4[i])*sin(theta1456_5[i])*qd1456_1[i] * qd1456_6[i] - 2 * r4*cos(theta1456_6[i])*sin(theta1456_4[i])*sin(2 * theta1456_5[i])*qd1456_1[i] * qd1456_4[i],					(981 * cos(theta1456_4[i])*cos(theta1456_6[i])) / 100 - r4*sin(theta1456_6[i])*qdd1456_5[i] - (981 * cos(theta1456_5[i])*sin(theta1456_4[i])*sin(theta1456_6[i])) / 100 - r3*cos(theta1456_6[i])*pow(qd1456_1[i],2) - r3*cos(theta1456_6[i])*pow(qd1456_4[i],2) - r2*cos(theta1456_6[i])*sin(theta1456_4[i])*qdd1456_1[i] - r4*cos(theta1456_4[i])*sin(theta1456_6[i])*qdd1456_1[i] + r3*cos(theta1456_5[i])*sin(theta1456_6[i])*qdd1456_4[i] + r4*cos(theta1456_6[i])*sin(theta1456_5[i])*qdd1456_4[i] - r2*sin(theta1456_5[i])*sin(theta1456_6[i])*pow(qd1456_1[i],2) + r3*pow(cos(theta1456_4[i]),2)*cos(theta1456_6[i])*pow(qd1456_1[i],2) + 2 * r4*cos(theta1456_5[i])*cos(theta1456_6[i])*qd1456_4[i] * qd1456_5[i] + 2 * r4*sin(theta1456_4[i])*sin(theta1456_6[i])*qd1456_1[i] * qd1456_4[i] - r2*cos(theta1456_4[i])*cos(theta1456_5[i])*sin(theta1456_6[i])*qdd1456_1[i] - r4*cos(theta1456_5[i])*cos(theta1456_6[i])*sin(theta1456_4[i])*qdd1456_1[i] + r3*sin(theta1456_4[i])*sin(theta1456_5[i])*sin(theta1456_6[i])*qdd1456_1[i] - r4*cos(theta1456_5[i])*sin(theta1456_5[i])*sin(theta1456_6[i])*pow(qd1456_1[i],2) + r4*cos(theta1456_5[i])*sin(theta1456_5[i])*sin(theta1456_6[i])*pow(qd1456_4[i],2) - r3*cos(theta1456_4[i])*cos(theta1456_5[i])*sin(theta1456_4[i])*sin(theta1456_6[i])*pow(qd1456_1[i],2) + r4*cos(theta1456_4[i])*cos(theta1456_6[i])*sin(theta1456_4[i])*sin(theta1456_5[i])*pow(qd1456_1[i],2) + 2 * r3*cos(theta1456_4[i])*sin(theta1456_5[i])*sin(theta1456_6[i])*qd1456_1[i] * qd1456_4[i] + 2 * r4*cos(theta1456_6[i])*sin(theta1456_4[i])*sin(theta1456_5[i])*qd1456_1[i] * qd1456_5[i] + r4*pow(cos(theta1456_4[i]),2)*cos(theta1456_5[i])*sin(theta1456_5[i])*sin(theta1456_6[i])*pow(qd1456_1[i],2) - 2 * r4*pow(cos(theta1456_5[i]),2)*sin(theta1456_4[i])*sin(theta1456_6[i])*qd1456_1[i] * qd1456_4[i],						2 * r3*cos(theta1456_6[i])*qdd1456_1[i] - r2*cos(theta1456_4[i])*cos(theta1456_6[i])*qdd1456_4[i] - r4*cos(theta1456_4[i])*cos(theta1456_6[i])*qdd1456_6[i] + 2 * r2*sin(theta1456_5[i])*sin(theta1456_6[i])*qdd1456_1[i] + r2*sin(theta1456_4[i])*sin(theta1456_6[i])*qdd1456_6[i] - r4*sin(theta1456_4[i])*sin(theta1456_6[i])*qdd1456_4[i] - 2 * r3*pow(cos(theta1456_4[i]),2)*cos(theta1456_6[i])*qdd1456_1[i] + r2*cos(theta1456_6[i])*sin(theta1456_4[i])*pow(qd1456_4[i],2) + r2*cos(theta1456_6[i])*sin(theta1456_4[i])*pow(qd1456_6[i],2) - r4*cos(theta1456_4[i])*sin(theta1456_6[i])*pow(qd1456_4[i],2) + r4*cos(theta1456_4[i])*sin(theta1456_6[i])*pow(qd1456_6[i],2) - 2 * r3*sin(theta1456_6[i])*qd1456_1[i] * qd1456_6[i] - 2 * r4*sin(theta1456_6[i])*qd1456_1[i] * qd1456_5[i] + r3*sin(theta1456_4[i])*sin(theta1456_5[i])*sin(theta1456_6[i])*pow(qd1456_4[i],2) - r3*sin(theta1456_4[i])*sin(theta1456_5[i])*sin(theta1456_6[i])*pow(qd1456_5[i],2) - r3*sin(theta1456_4[i])*sin(theta1456_5[i])*sin(theta1456_6[i])*pow(qd1456_6[i],2) + 2 * r2*cos(theta1456_5[i])*sin(theta1456_6[i])*qd1456_1[i] * qd1456_5[i] - 2 * r3*cos(theta1456_5[i])*sin(theta1456_6[i])*qd1456_1[i] * qd1456_4[i] + 2 * r2*cos(theta1456_6[i])*sin(theta1456_5[i])*qd1456_1[i] * qd1456_6[i] + 2 * r4*cos(theta1456_6[i])*sin(theta1456_5[i])*qd1456_1[i] * qd1456_4[i] + 2 * r2*cos(theta1456_4[i])*sin(theta1456_6[i])*qd1456_4[i] * qd1456_6[i] + 2 * r4*cos(theta1456_4[i])*pow(cos(theta1456_5[i]),2)*sin(theta1456_6[i])*pow(qd1456_4[i],2) - r2*cos(theta1456_4[i])*cos(theta1456_5[i])*cos(theta1456_6[i])*qdd1456_6[i] - r4*cos(theta1456_4[i])*cos(theta1456_5[i])*cos(theta1456_6[i])*qdd1456_4[i] + 2 * r3*pow(cos(theta1456_4[i]),2)*sin(theta1456_6[i])*qd1456_1[i] * qd1456_6[i] + 2 * r4*pow(cos(theta1456_4[i]),2)*sin(theta1456_6[i])*qd1456_1[i] * qd1456_5[i] + 4 * r4*pow(cos(theta1456_5[i]),2)*sin(theta1456_6[i])*qd1456_1[i] * qd1456_5[i] + r2*cos(theta1456_5[i])*sin(theta1456_4[i])*sin(theta1456_6[i])*qdd1456_4[i] + 2 * r4*cos(theta1456_5[i])*sin(theta1456_5[i])*sin(theta1456_6[i])*qdd1456_1[i] + r2*cos(theta1456_4[i])*sin(theta1456_5[i])*sin(theta1456_6[i])*qdd1456_5[i] - r3*cos(theta1456_4[i])*sin(theta1456_5[i])*sin(theta1456_6[i])*qdd1456_4[i] + r3*cos(theta1456_5[i])*sin(theta1456_4[i])*sin(theta1456_6[i])*qdd1456_5[i] + r3*cos(theta1456_6[i])*sin(theta1456_4[i])*sin(theta1456_5[i])*qdd1456_6[i] - r4*cos(theta1456_6[i])*sin(theta1456_4[i])*sin(theta1456_5[i])*qdd1456_5[i] + r4*cos(theta1456_5[i])*sin(theta1456_4[i])*sin(theta1456_6[i])*qdd1456_6[i] + r2*cos(theta1456_4[i])*cos(theta1456_5[i])*sin(theta1456_6[i])*pow(qd1456_4[i],2) + r2*cos(theta1456_4[i])*cos(theta1456_5[i])*sin(theta1456_6[i])*pow(qd1456_5[i],2) + r2*cos(theta1456_4[i])*cos(theta1456_5[i])*sin(theta1456_6[i])*pow(qd1456_6[i],2) + r4*cos(theta1456_5[i])*cos(theta1456_6[i])*sin(theta1456_4[i])*pow(qd1456_4[i],2) - r4*cos(theta1456_5[i])*cos(theta1456_6[i])*sin(theta1456_4[i])*pow(qd1456_5[i],2) + r4*cos(theta1456_5[i])*cos(theta1456_6[i])*sin(theta1456_4[i])*pow(qd1456_6[i],2) + 2 * r4*pow(cos(theta1456_5[i]),2)*sin(theta1456_4[i])*sin(theta1456_6[i])*qdd1456_4[i] - 4 * r4*pow((theta1456_4[i]),2)*pow(cos(theta1456_5[i]),2)*sin(theta1456_6[i])*qd1456_1[i] * qd1456_5[i] - 2 * r4*pow(cos(theta1456_4[i]),2)*cos(theta1456_5[i])*sin(theta1456_5[i])*sin(theta1456_6[i])*qdd1456_1[i] + 4 * r3*cos(theta1456_4[i])*cos(theta1456_6[i])*sin(theta1456_4[i])*qd1456_1[i] * qd1456_4[i] + 2 * r2*cos(theta1456_5[i])*cos(theta1456_6[i])*sin(theta1456_4[i])*qd1456_4[i] * qd1456_6[i] + 2 * r4*cos(theta1456_5[i])*cos(theta1456_6[i])*sin(theta1456_5[i])*qd1456_1[i] * qd1456_6[i] + 2 * r2*cos(theta1456_4[i])*cos(theta1456_6[i])*sin(theta1456_5[i])*qd1456_5[i] * qd1456_6[i] + 2 * r3*cos(theta1456_5[i])*cos(theta1456_6[i])*sin(theta1456_4[i])*qd1456_5[i] * qd1456_6[i] + 2 * r4*cos(theta1456_4[i])*cos(theta1456_5[i])*sin(theta1456_6[i])*qd1456_4[i] * qd1456_6[i] - 2 * r2*sin(theta1456_4[i])*sin(theta1456_5[i])*sin(theta1456_6[i])*qd1456_4[i] * qd1456_5[i] + 4 * r3*pow(cos(theta1456_4[i]),2)*cos(theta1456_5[i])*sin(theta1456_6[i])*qd1456_1[i] * qd1456_4[i] - 4 * r4*pow(cos(theta1456_4[i]),2)*cos(theta1456_6[i])*sin(theta1456_5[i])*qd1456_1[i] * qd1456_4[i] + 2 * r4*pow(cos(theta1456_5[i]),2)*cos(theta1456_6[i])*sin(theta1456_4[i])*qd1456_4[i] * qd1456_6[i] + 2 * r3*cos(theta1456_4[i])*cos(theta1456_5[i])*sin(theta1456_4[i])*sin(theta1456_6[i])*qdd1456_1[i] - 2 * r4*cos(theta1456_4[i])*cos(theta1456_6[i])*sin(theta1456_4[i])*sin(theta1456_5[i])*qdd1456_1[i] - 2 * r4*pow(cos(theta1456_4[i]),2)*cos(theta1456_5[i])*cos(theta1456_6[i])*sin(theta1456_5[i])*qd1456_1[i] * qd1456_6[i] + 2 * r3*cos(theta1456_4[i])*cos(theta1456_5[i])*cos(theta1456_6[i])*sin(theta1456_4[i])*qd1456_1[i] * qd1456_6[i] - 2 * r4*cos(theta1456_4[i])*cos(theta1456_5[i])*cos(theta1456_6[i])*sin(theta1456_4[i])*qd1456_1[i] * qd1456_5[i] - 2 * r3*cos(theta1456_4[i])*sin(theta1456_4[i])*sin(theta1456_5[i])*sin(theta1456_6[i])*qd1456_1[i] * qd1456_5[i] + 2 * r4*cos(theta1456_4[i])*sin(theta1456_4[i])*sin(theta1456_5[i])*sin(theta1456_6[i])*qd1456_1[i] * qd1456_6[i] - 4 * r4*cos(theta1456_5[i])*sin(theta1456_4[i])*sin(theta1456_5[i])*sin(theta1456_6[i])*qd1456_4[i] * qd1456_5[i] + 4 * r4*cos(theta1456_4[i])*cos(theta1456_5[i])*sin(theta1456_4[i])*sin(theta1456_5[i])*sin(theta1456_6[i])*qd1456_1[i] * qd1456_4[i],					2 * r3*cos(theta1456_6[i])*qdd1456_4[i] - (981 * cos(theta1456_4[i])*cos(theta1456_5[i])*sin(theta1456_6[i])) / 100 - (981 * cos(theta1456_6[i])*sin(theta1456_4[i])) / 100 - r3*cos(theta1456_6[i])*sin(2 * theta1456_4[i])*pow(qd1456_1[i],2) - r2*cos(theta1456_4[i])*cos(theta1456_6[i])*qdd1456_1[i] + r3*cos(theta1456_5[i])*cos(theta1456_6[i])*qdd1456_6[i] - r4*cos(theta1456_5[i])*cos(theta1456_6[i])*qdd1456_5[i] - r3*sin(theta1456_5[i])*sin(theta1456_6[i])*qdd1456_5[i] - r4*sin(theta1456_5[i])*sin(theta1456_6[i])*qdd1456_6[i] - r3*cos(theta1456_5[i])*sin(theta1456_6[i])*pow(qd1456_5[i],2) - r3*cos(theta1456_5[i])*sin(theta1456_6[i])*pow(qd1456_6[i],2) + r4*cos(theta1456_6[i])*sin(theta1456_5[i])*pow(qd1456_5[i],2) - r4*cos(theta1456_6[i])*sin(theta1456_5[i])*pow(qd1456_6[i],2) - r4*sin(theta1456_6[i])*sin(2 * theta1456_5[i])*qdd1456_4[i] - 2 * r3*sin(theta1456_6[i])*qd1456_4[i] * qd1456_6[i] - r4*cos(theta1456_6[i])*sin(theta1456_4[i])*qd1456_1[i] * qd1456_6[i] - 2 * r3*cos(theta1456_6[i])*sin(theta1456_5[i])*qd1456_5[i] * qd1456_6[i] - r3*cos(theta1456_5[i])*sin(theta1456_6[i])*cos(2 * theta1456_4[i])*pow(qd1456_1[i],2) + r4*cos(theta1456_6[i])*sin(theta1456_5[i])*cos(2 * theta1456_4[i])*pow(qd1456_1[i],2) - r4*cos(theta1456_4[i])*cos(theta1456_5[i])*cos(theta1456_6[i])*qdd1456_1[i] - 2 * r4*sin(theta1456_6[i])*cos(2 * theta1456_5[i])*qd1456_4[i] * qd1456_5[i] - r4*cos(theta1456_6[i])*sin(2 * theta1456_5[i])*qd1456_4[i] * qd1456_6[i] + r2*cos(theta1456_5[i])*sin(theta1456_4[i])*sin(theta1456_6[i])*qdd1456_1[i] - r3*cos(theta1456_4[i])*sin(theta1456_5[i])*sin(theta1456_6[i])*qdd1456_1[i] - (r4*sin(theta1456_6[i])*sin(2 * theta1456_4[i])*sin(2 * theta1456_5[i])*pow(qd1456_1[i],2)) / 2 + r4*sin(theta1456_4[i])*sin(theta1456_6[i])*cos(2 * theta1456_5[i])*qdd1456_1[i] - 2 * r3*cos(theta1456_4[i])*cos(theta1456_5[i])*sin(theta1456_6[i])*qd1456_1[i] * qd1456_5[i] - 2 * r3*cos(theta1456_4[i])*cos(theta1456_6[i])*sin(theta1456_5[i])*qd1456_1[i] * qd1456_6[i] + 2 * r4*cos(theta1456_4[i])*cos(theta1456_6[i])*sin(theta1456_5[i])*qd1456_1[i] * qd1456_5[i] + r4*cos(theta1456_6[i])*sin(theta1456_4[i])*cos(2 * theta1456_5[i])*qd1456_1[i] * qd1456_6[i] - 2 * r4*sin(theta1456_4[i])*sin(theta1456_6[i])*sin(2 * theta1456_5[i])*qd1456_1[i] * qd1456_5[i],						(981 * sin(theta1456_4[i])*sin(theta1456_5[i])*sin(theta1456_6[i])) / 100 + r4*sin(theta1456_6[i])*pow(qd1456_6[i],2) - r4*cos(theta1456_6[i])*qdd1456_6[i] - (r4*sin(theta1456_6[i])*cos(2 * theta1456_5[i])*pow(qd1456_1[i],2)) / 2 + r4*sin(theta1456_6[i])*cos(2 * theta1456_5[i])*pow(qd1456_4[i],2) - r4*cos(theta1456_5[i])*cos(theta1456_6[i])*qdd1456_4[i] - r3*sin(theta1456_5[i])*sin(theta1456_6[i])*qdd1456_4[i] - r2*cos(theta1456_5[i])*sin(theta1456_6[i])*pow(qd1456_1[i],2) + 2 * r4*cos(theta1456_5[i])*sin(theta1456_6[i])*qd1456_4[i] * qd1456_6[i] + (r4*cos(theta1456_5[i])*cos(theta1456_6[i])*sin(2 * theta1456_4[i])*pow(qd1456_1[i],2)) / 2 + (r3*sin(theta1456_5[i])*sin(theta1456_6[i])*sin(2 * theta1456_4[i])*pow(qd1456_1[i],2)) / 2 + r2*cos(theta1456_4[i])*sin(theta1456_5[i])*sin(theta1456_6[i])*qdd1456_1[i] + r3*cos(theta1456_5[i])*sin(theta1456_4[i])*sin(theta1456_6[i])*qdd1456_1[i] - r4*cos(theta1456_6[i])*sin(theta1456_4[i])*sin(theta1456_5[i])*qdd1456_1[i] + (r4*sin(theta1456_6[i])*cos(2 * theta1456_4[i])*cos(2 * theta1456_5[i])*pow(qd1456_1[i],2)) / 2 + 2 * r3*cos(theta1456_4[i])*cos(theta1456_5[i])*sin(theta1456_6[i])*qd1456_1[i] * qd1456_4[i] - 2 * r4*cos(theta1456_4[i])*cos(theta1456_6[i])*sin(theta1456_5[i])*qd1456_1[i] * qd1456_4[i] + 2 * r4*sin(theta1456_4[i])*sin(theta1456_5[i])*sin(theta1456_6[i])*qd1456_1[i] * qd1456_6[i] + 2 * r4*sin(theta1456_4[i])*sin(theta1456_6[i])*sin(2 * theta1456_5[i])*qd1456_1[i] * qd1456_4[i],					r3*sin(theta1456_6[i])*pow(qd1456_1[i],2) - (981 * cos(theta1456_5[i])*cos(theta1456_6[i])*sin(theta1456_4[i])) / 100 - (981 * cos(theta1456_4[i])*sin(theta1456_6[i])) / 100 + r3*sin(theta1456_6[i])*pow(qd1456_4[i],2) - r4*cos(theta1456_6[i])*qdd1456_5[i] - r3*pow(cos(theta1456_4[i]),2)*sin(theta1456_6[i])*pow(qd1456_1[i],2) - r4*cos(theta1456_4[i])*cos(theta1456_6[i])*qdd1456_1[i] + r3*cos(theta1456_5[i])*cos(theta1456_6[i])*qdd1456_4[i] + r2*sin(theta1456_4[i])*sin(theta1456_6[i])*qdd1456_1[i] - r4*sin(theta1456_5[i])*sin(theta1456_6[i])*qdd1456_4[i] - r2*cos(theta1456_6[i])*sin(theta1456_5[i])*pow(qd1456_1[i],2) + 2 * r4*cos(theta1456_6[i])*sin(theta1456_4[i])*qd1456_1[i] * qd1456_4[i] - 2 * r4*cos(theta1456_5[i])*sin(theta1456_6[i])*qd1456_4[i] * qd1456_5[i] - r2*cos(theta1456_4[i])*cos(theta1456_5[i])*cos(theta1456_6[i])*qdd1456_1[i] + r3*cos(theta1456_6[i])*sin(theta1456_4[i])*sin(theta1456_5[i])*qdd1456_1[i] + r4*cos(theta1456_5[i])*sin(theta1456_4[i])*sin(theta1456_6[i])*qdd1456_1[i] - r4*cos(theta1456_5[i])*cos(theta1456_6[i])*sin(theta1456_5[i])*pow(qd1456_1[i],2) + r4*cos(theta1456_5[i])*cos(theta1456_6[i])*sin(theta1456_5[i])*pow(qd1456_4[i],2) - r3*cos(theta1456_4[i])*cos(theta1456_5[i])*cos(theta1456_6[i])*sin(theta1456_4[i])*pow(qd1456_1[i],2) - r4*cos(theta1456_4[i])*sin(theta1456_4[i])*sin(theta1456_5[i])*sin(theta1456_6[i])*pow(qd1456_1[i],2) + 2 * r3*cos(theta1456_4[i])*cos(theta1456_6[i])*sin(theta1456_5[i])*qd1456_1[i] * qd1456_4[i] - 2 * r4*sin(theta1456_4[i])*sin(theta1456_5[i])*sin(theta1456_6[i])*qd1456_1[i] * qd1456_5[i] + r4*pow(cos(theta1456_4[i]),2)*cos(theta1456_5[i])*cos(theta1456_6[i])*sin(theta1456_5[i])*pow(qd1456_1[i],2) - 2 * r4*pow(cos(theta1456_5[i]),2)*cos(theta1456_6[i])*sin(theta1456_4[i])*qd1456_1[i] * qd1456_4[i],								pow(cos(theta1456_5[i]),2)*qdd1456_1[i] + sin(theta1456_4[i])*qd1456_4[i] * qd1456_5[i] - pow(cos(theta1456_4[i]),2)*pow(cos(theta1456_5[i]),2)*qdd1456_1[i] - sin(2 * theta1456_5[i])*qd1456_1[i] * qd1456_5[i] - 2 * pow(cos(theta1456_5[i]),2)*sin(theta1456_4[i])*qd1456_4[i] * qd1456_5[i] - cos(theta1456_5[i])*sin(theta1456_4[i])*sin(theta1456_5[i])*qdd1456_4[i] - cos(theta1456_4[i])*cos(theta1456_5[i])*sin(theta1456_5[i])*pow(qd1456_4[i],2) + 2 * cos(theta1456_4[i])*pow(cos(theta1456_5[i]),2)*sin(theta1456_4[i])*qd1456_1[i] * qd1456_4[i] + 2 * pow(cos(theta1456_4[i]),2)*cos(theta1456_5[i])*sin(theta1456_5[i])*qd1456_1[i] * qd1456_5[i],								qdd1456_4[i] / 2 - (sin(2 * theta1456_4[i])*pow(qd1456_1[i],2)) / 4 - (cos(2 * theta1456_5[i])*qdd1456_4[i]) / 2 - (sin(theta1456_4[i])*sin(2 * theta1456_5[i])*qdd1456_1[i]) / 2 + sin(2 * theta1456_5[i])*qd1456_4[i] * qd1456_5[i] - (cos(2 * theta1456_5[i])*sin(2 * theta1456_4[i])*pow(qd1456_1[i],2)) / 4 - sin(theta1456_4[i])*cos(2 * theta1456_5[i])*qd1456_1[i] * qd1456_5[i],											(sin(2 * theta1456_5[i])*pow(qd1456_1[i],2)) / 2 - (sin(2 * theta1456_5[i])*pow(qd1456_4[i],2)) / 2 - (pow(cos(theta1456_4[i]),2)*sin(2 * theta1456_5[i])*pow(qd1456_1[i],2)) / 2 + sin(theta1456_4[i])*cos(2 * theta1456_5[i])*qd1456_1[i] * qd1456_4[i],				0,					sin(theta1456_4[i])*qdd1456_4[i] + cos(theta1456_4[i])*pow(qd1456_4[i],2) - 2 * pow(cos(theta1456_5[i]),2)*sin(theta1456_4[i])*qdd1456_4[i] - 2 * cos(theta1456_4[i])*pow(cos(theta1456_5[i]),2)*pow(qd1456_4[i],2) - 2 * pow(cos(theta1456_4[i]),2)*qd1456_1[i] * qd1456_5[i] - 4 * pow(cos(theta1456_5[i]),2)*qd1456_1[i] * qd1456_5[i] + 4 * pow(cos(theta1456_4[i]),2)*pow(cos(theta1456_5[i]),2)*qd1456_1[i] * qd1456_5[i] + 2 * pow(cos(theta1456_4[i]),2)*cos(theta1456_5[i])*sin(theta1456_5[i])*qdd1456_1[i] - 2 * sin(2 * theta1456_5[i])*qd1456_1[i] * qd1456_5[i] * qdd1456_1[i] + 4 * cos(theta1456_5[i])*sin(theta1456_4[i])*sin(theta1456_5[i])*qd1456_4[i] * qd1456_5[i] - 4 * cos(theta1456_4[i])*cos(theta1456_5[i])*sin(theta1456_4[i])*sin(theta1456_5[i])*qd1456_1[i] * qd1456_4[i],								sin(2 * theta1456_5[i])*qdd1456_4[i] + (sin(2 * theta1456_4[i])*sin(2 * theta1456_5[i])*pow(qd1456_1[i],2)) / 2 - sin(theta1456_4[i])*cos(2 * theta1456_5[i])*qdd1456_1[i] + 2 * cos(2 * theta1456_5[i])*qd1456_4[i] * qd1456_5[i] + 2 * sin(theta1456_4[i])*sin(2 * theta1456_5[i])*qd1456_1[i] * qd1456_5[i] },
    		{  cos(2 * theta1456_5[i])*pow(qd1456_1[i],2) - cos(2 * theta1456_5[i])*pow(qd1456_4[i],2) - pow(cos(theta1456_4[i]),2)*cos(2 * theta1456_5[i])*pow(qd1456_1[i],2) - 2 * sin(theta1456_4[i])*sin(2 * theta1456_5[i])*qd1456_1[i] * qd1456_4[i],				0,			sin(theta1456_4[i])*sin(theta1456_5[i])*pow(qd1456_4[i],2) - sin(theta1456_4[i])*sin(theta1456_5[i])*pow(qd1456_5[i],2) - 2 * cos(theta1456_5[i])*qd1456_1[i] * qd1456_4[i] - cos(theta1456_4[i])*sin(theta1456_5[i])*qdd1456_4[i] + cos(theta1456_5[i])*sin(theta1456_4[i])*qdd1456_5[i] + 4 * pow(cos(theta1456_4[i]),2)*cos(theta1456_5[i])*qd1456_1[i] * qd1456_4[i] + 2 * cos(theta1456_4[i])*cos(theta1456_5[i])*sin(theta1456_4[i])*qdd1456_1[i] - 2 * cos(theta1456_4[i])*sin(theta1456_4[i])*sin(theta1456_5[i])*qd1456_1[i] * qd1456_5[i],							-sin(theta1456_5[i])*qdd1456_5[i] - cos(theta1456_5[i])*pow(qd1456_5[i],2) - cos(theta1456_5[i])*pow(qd1456_1[i],2)*(2 * pow(cos(theta1456_4[i]),2) - 1) - cos(theta1456_4[i])*sin(theta1456_5[i])*qdd1456_1[i] - 2 * cos(theta1456_4[i])*cos(theta1456_5[i])*qd1456_1[i] * qd1456_5[i],						cos(theta1456_5[i])*sin(theta1456_4[i])*qdd1456_1[i] - sin(theta1456_5[i])*qdd1456_4[i] + cos(theta1456_4[i])*sin(theta1456_4[i])*sin(theta1456_5[i])*pow(qd1456_1[i],2) + 2 * cos(theta1456_4[i])*cos(theta1456_5[i])*qd1456_1[i] * qd1456_4[i],					0,		cos(theta1456_5[i])*sin(theta1456_4[i])*pow(qd1456_4[i],2) - cos(theta1456_5[i])*sin(theta1456_4[i])*pow(qd1456_5[i],2) + 2 * sin(theta1456_5[i])*qd1456_1[i] * qd1456_4[i] - cos(theta1456_4[i])*cos(theta1456_5[i])*qdd1456_4[i] - sin(theta1456_4[i])*sin(theta1456_5[i])*qdd1456_5[i] - 4 * pow(cos(theta1456_4[i]),2)*sin(theta1456_5[i])*qd1456_1[i] * qd1456_4[i] - 2 * cos(theta1456_4[i])*sin(theta1456_4[i])*sin(theta1456_5[i])*qdd1456_1[i] - 2 * cos(theta1456_4[i])*cos(theta1456_5[i])*sin(theta1456_4[i])*qd1456_1[i] * qd1456_5[i],					sin(theta1456_5[i])*pow(qd1456_5[i],2) - cos(theta1456_5[i])*qdd1456_5[i] + sin(theta1456_5[i])*pow(qd1456_1[i],2)*(2 * pow(cos(theta1456_4[i]),2) - 1) - cos(theta1456_4[i])*cos(theta1456_5[i])*qdd1456_1[i] + 2 * cos(theta1456_4[i])*sin(theta1456_5[i])*qd1456_1[i] * qd1456_5[i],					cos(theta1456_4[i])*cos(theta1456_5[i])*sin(theta1456_4[i])*pow(qd1456_1[i],2) - sin(theta1456_4[i])*sin(theta1456_5[i])*qdd1456_1[i] - cos(theta1456_5[i])*qdd1456_4[i] - 2 * cos(theta1456_4[i])*sin(theta1456_5[i])*qd1456_1[i] * qd1456_4[i],					0,				cos(theta1456_4[i])*qdd1456_5[i] + pow(cos(theta1456_4[i]),2)*qdd1456_1[i] - sin(theta1456_4[i])*qd1456_4[i] * qd1456_5[i] - 2 * cos(theta1456_4[i])*sin(theta1456_4[i])*qd1456_1[i] * qd1456_4[i],							(sin(2 * theta1456_4[i])*pow(qd1456_1[i],2)) / 2 + sin(theta1456_4[i])*qd1456_1[i] * qd1456_5[i],							qdd1456_5[i] + cos(theta1456_4[i])*qdd1456_1[i] - sin(theta1456_4[i])*qd1456_1[i] * qd1456_4[i],							0,				r3*cos(theta1456_4[i])*sin(theta1456_5[i])*qdd1456_4[i] - r2*cos(theta1456_5[i])*sin(theta1456_4[i])*qdd1456_4[i] - r2*cos(theta1456_4[i])*sin(theta1456_5[i])*qdd1456_5[i] - 2 * r2*sin(theta1456_5[i])*qdd1456_1[i] - r3*cos(theta1456_5[i])*sin(theta1456_4[i])*qdd1456_5[i] - r2*cos(theta1456_4[i])*cos(theta1456_5[i])*pow(qd1456_4[i],2) - r2*cos(theta1456_4[i])*cos(theta1456_5[i])*pow(qd1456_5[i],2) - r3*sin(theta1456_4[i])*sin(theta1456_5[i])*pow(qd1456_4[i],2) + r3*sin(theta1456_4[i])*sin(theta1456_5[i])*pow(qd1456_5[i],2) - 2 * r2*cos(theta1456_5[i])*qd1456_1[i] * qd1456_5[i] + 2 * r3*cos(theta1456_5[i])*qd1456_1[i] * qd1456_4[i] + 2 * r2*sin(theta1456_4[i])*sin(theta1456_5[i])*qd1456_4[i] * qd1456_5[i] - 4 * r3*pow(cos(theta1456_4[i]),2)*cos(theta1456_5[i])*qd1456_1[i] * qd1456_4[i] - 2 * r3*cos(theta1456_4[i])*cos(theta1456_5[i])*sin(theta1456_4[i])*qdd1456_1[i] + 2 * r3*cos(theta1456_4[i])*sin(theta1456_4[i])*sin(theta1456_5[i])*qd1456_1[i] * qd1456_5[i],						(981 * cos(theta1456_4[i])*cos(theta1456_5[i])) / 100 + r3*sin(theta1456_5[i])*qdd1456_5[i] + r3*cos(theta1456_5[i])*pow(qd1456_5[i],2) - r2*cos(theta1456_5[i])*sin(theta1456_4[i])*qdd1456_1[i] + r3*cos(theta1456_4[i])*sin(theta1456_5[i])*qdd1456_1[i] + r3*cos(theta1456_5[i])*pow(qd1456_1[i],2)*(2 * pow(cos(theta1456_4[i]),2) - 1) + 2 * r3*cos(theta1456_4[i])*cos(theta1456_5[i])*qd1456_1[i] * qd1456_5[i],						r3*sin(theta1456_5[i])*qdd1456_4[i] - (981 * sin(theta1456_4[i])*sin(theta1456_5[i])) / 100 + r2*cos(theta1456_5[i])*pow(qd1456_1[i],2) - r2*cos(theta1456_4[i])*sin(theta1456_5[i])*qdd1456_1[i] - r3*cos(theta1456_5[i])*sin(theta1456_4[i])*qdd1456_1[i] - 2 * r3*cos(theta1456_4[i])*cos(theta1456_5[i])*qd1456_1[i] * qd1456_4[i] - r3*cos(theta1456_4[i])*sin(theta1456_4[i])*sin(theta1456_5[i])*pow(qd1456_1[i],2),						0,					r3*cos(theta1456_4[i])*cos(theta1456_5[i])*qdd1456_4[i] - r2*cos(theta1456_4[i])*cos(theta1456_5[i])*qdd1456_5[i] - 2 * r2*cos(theta1456_5[i])*qdd1456_1[i] + r2*sin(theta1456_4[i])*sin(theta1456_5[i])*qdd1456_4[i] + r3*sin(theta1456_4[i])*sin(theta1456_5[i])*qdd1456_5[i] + r2*cos(theta1456_4[i])*sin(theta1456_5[i])*pow(qd1456_4[i],2) + r2*cos(theta1456_4[i])*sin(theta1456_5[i])*pow(qd1456_5[i],2) - r3*cos(theta1456_5[i])*sin(theta1456_4[i])*pow(qd1456_4[i],2) + r3*cos(theta1456_5[i])*sin(theta1456_4[i])*pow(qd1456_5[i],2) + 2 * r2*sin(theta1456_5[i])*qd1456_1[i] * qd1456_5[i] - 2 * r3*sin(theta1456_5[i])*qd1456_1[i] * qd1456_4[i] + 2 * r2*cos(theta1456_5[i])*sin(theta1456_4[i])*qd1456_4[i] * qd1456_5[i] + 4 * r3*pow(cos(theta1456_4[i]),2)*sin(theta1456_5[i])*qd1456_1[i] * qd1456_4[i] + 2 * r3*cos(theta1456_4[i])*sin(theta1456_4[i])*sin(theta1456_5[i])*qdd1456_1[i] + 2 * r3*cos(theta1456_4[i])*cos(theta1456_5[i])*sin(theta1456_4[i])*qd1456_1[i] * qd1456_5[i],						r3*cos(theta1456_5[i])*qdd1456_5[i] - r3*sin(theta1456_5[i])*pow(qd1456_5[i],2) - (981 * cos(theta1456_4[i])*sin(theta1456_5[i])) / 100 + r3*cos(theta1456_4[i])*cos(theta1456_5[i])*qdd1456_1[i] + r2*sin(theta1456_4[i])*sin(theta1456_5[i])*qdd1456_1[i] - r3*sin(theta1456_5[i])*pow(qd1456_1[i],2)*(2 * pow(cos(theta1456_4[i]),2) - 1) - 2 * r3*cos(theta1456_4[i])*sin(theta1456_5[i])*qd1456_1[i] * qd1456_5[i],						r3*cos(theta1456_5[i])*qdd1456_4[i] - r2*sin(theta1456_5[i])*pow(qd1456_1[i],2) - (981 * cos(theta1456_5[i])*sin(theta1456_4[i])) / 100 - r2*cos(theta1456_4[i])*cos(theta1456_5[i])*qdd1456_1[i] + r3*sin(theta1456_4[i])*sin(theta1456_5[i])*qdd1456_1[i] + 2 * r3*cos(theta1456_4[i])*sin(theta1456_5[i])*qd1456_1[i] * qd1456_4[i] - r3*cos(theta1456_4[i])*cos(theta1456_5[i])*sin(theta1456_4[i])*pow(qd1456_1[i],2),						0,			sin(2 * theta1456_4[i])*qd1456_1[i] * qd1456_4[i] - pow(cos(theta1456_4[i]),2)*pow(qdd1456_1[i],2),								-(sin(2 * theta1456_4[i])*pow(qd1456_1[i],2)) / 2,						0,					0,				2 * qd1456_1[i] * qd1456_4[i] * (2 * pow(cos(theta1456_4[i]),2) - sin(2 * theta1456_4[i])*qdd1456_1[i]),						-cos(2 * theta1456_4[i])*pow(qd1456_1[i],2),						0,				0,					cos(theta1456_4[i])*pow(qd1456_4[i],2) + sin(theta1456_4[i])*qdd1456_4[i],							sin(theta1456_4[i])*qdd1456_1[i],						0,				0 },
    		{  cos(theta1456_4[i])*qdd1456_4[i] - sin(theta1456_4[i])*pow(qd1456_4[i],2),			cos(theta1456_4[i])*qdd1456_1[i],			0,			0,			0,				qdd1456_4[i],				0,			0,			-r2*(cos(theta1456_4[i])*pow(qd1456_4[i],2) + sin(theta1456_4[i])*qdd1456_4[i]),			(981 * cos(theta1456_4[i])) / 100 - r2*sin(theta1456_4[i])*qdd1456_1[i],					0,			0,		-r2*(cos(theta1456_4[i])*qdd1456_4[i] - sin(theta1456_4[i])*pow(qd1456_4[i],2)),				-(981 * sin(theta1456_4[i])) / 100 - r2*cos(theta1456_4[i])*qdd1456_1[i],				0,			0,			qdd1456_1[i],			0,				0,				0,			qd1456_1[i] / fabs(qd1456_1[i]) + 1,				0,				0,			0,			qd1456_1[i] / fabs(qd1456_1[i]) - 1,				0,				0,				0,				qd1456_1[i],				0,				0,					0,			0,			qd1456_4[i] / fabs(qd1456_4[i]) + 1 },
    		{  0,			0,			0,			qd1456_4[i] / fabs(qd1456_4[i]) - 1,			0,			0,			0,			qd1456_4[i] ,			0,			0,			0,			0,			qd1456_5[i] / fabs(qd1456_5[i]) + 1,				0,				0,				0,				qd1456_5[i] / fabs(qd1456_5[i]) - 1,			0,			0,			0,			qd1456_5[i],			0,			0,			0,			0,			qd1456_6[i] / fabs(qd1456_6[i]) + 1,				0,				0,			0,			qd1456_6[i] / fabs(qd1456_6[i]) - 1,			0,			0,			0,		qd1456_6[i] }
    		};
    		H1456[0][2] = qdd1456_5[i] / 2 + (sin(2 * theta1456_5[i])*pow(qd1456_1[i], 2)) / 4 - (sin(2 * theta1456_5[i])*pow(qd1456_4[i], 2)) / 4 + (cos(theta1456_4[i])*qdd1456_1[i]) / 2 - (cos(2 * theta1456_6[i])*qdd1456_5[i]) / 2 - (cos(theta1456_4[i])*cos(2 * theta1456_6[i])*qdd1456_1[i]) / 2 - (sin(theta1456_5[i])*sin(2 * theta1456_6[i])*qdd1456_4[i]) / 2 - sin(theta1456_4[i])*qd1456_1[i] * qd1456_4[i] + sin(2 * theta1456_6[i])*qd1456_5[i] * qd1456_6[i] + cos(theta1456_4[i])*sin(2 * theta1456_6[i])*qd1456_1[i] * qd1456_6[i] - sin(theta1456_5[i])*cos(2 * theta1456_6[i])*qd1456_4[i] * qd1456_6[i] + pow(cos(theta1456_5[i]), 2)*sin(theta1456_4[i])*qd1456_1[i] * qd1456_4[i] + (cos(theta1456_5[i])*sin(theta1456_4[i])*sin(2 * theta1456_6[i])*qdd1456_1[i]) / 2 + (cos(theta1456_5[i])*sin(theta1456_5[i])*cos(2 * theta1456_6[i])*pow(qd1456_1[i], 2)) / 2 - (cos(theta1456_5[i])*sin(theta1456_5[i])*cos(2 * theta1456_6[i])*pow(qd1456_4[i], 2)) / 2 - (pow(cos(theta1456_4[i]), 2)*cos(theta1456_5[i])*sin(theta1456_5[i])*pow(qd1456_1[i], 2)) / 2 + (cos(theta1456_4[i])*sin(theta1456_4[i])*sin(theta1456_5[i])*sin(2 * theta1456_6[i])*pow(qd1456_1[i], 2)) / 2 + cos(theta1456_4[i])*cos(theta1456_5[i])*sin(2 * theta1456_6[i])*qd1456_1[i] * qd1456_4[i] + cos(theta1456_5[i])*sin(theta1456_4[i])*cos(2 * theta1456_6[i])*qd1456_1[i] * qd1456_6[i] - (pow(cos(theta1456_4[i]), 2)*cos(theta1456_5[i])*sin(theta1456_5[i])*cos(2 * theta1456_6[i])*pow(qd1456_1[i], 2)) / 2 + pow(cos(theta1456_5[i]), 2)*sin(theta1456_4[i])*cos(2 * theta1456_6[i])*qd1456_1[i] * qd1456_4[i];
    		double temp1456[4][34];
    		for (int ii = 0; ii <4; ii++)
    		{ 
    			for (int jj = 0; jj<34; jj++)
    			{ 
    				temp1456[ii][jj] = H1456[ii][jj];
    			}
    		}
    		for (int tempj = 0; tempj < 34; tempj++)
    		{ 
    			for (int tempi = 0; tempi < 4; tempi++)
    			{ 
    				H1456[tempi][tempj] = temp1456[((tempj * 4) + tempi) / 34][((tempj * 4) + tempi) % 34];
    			}
    		}
    		for (int i = 0; i < 4; i++)
    		{ 
    			for (int j = 0; j < 34; j++)
    			{ 
    				H_final1456[temp_sum1][j] = H1456[i][j];
    			}
    			temp_sum1++;
    		}
    
    	}
    
    	//set123计算方法；
    	double **H_final123_trans = NULL;
    	matrixTranspose((double **)H_final123, &H_final123_trans, 6000, 24);
    	double **mutipl_123_trans_biachainv123 = NULL;
    	mutipleMatrix(H_final123_trans, biaocha_ffinv123, &mutipl_123_trans_biachainv123, 24, 6000, 6000, 6000, 0);
    
    	if (H_final123_trans != NULL)//释放H_final123_trans
    	{ 
    		for (int i = 0; i < 24; i++)
    		{ 
    			free(H_final123_trans[i]);
    		}
    		free(H_final123_trans);
    	}//！！！释放H_final123_trans
    
    	double **inv_123_pre = NULL;
    	mutipleMatrix(mutipl_123_trans_biachainv123, (double **)H_final123, &inv_123_pre, 24, 6000, 6000, 24, 1);
    	//des_inv_123 Out
    	double **des_inv_123 = (double **)calloc(24, sizeof(double *));
    	if (des_inv_123 == NULL)
    	{ 
    		printf("des_inv_123 calloc error\n");
    		return;
    	}
    	for (int i = 0; i<24; i++)
    	{ 
    		des_inv_123[i] = (double *)calloc(24, sizeof(double));
    	}
    	// !!!des_inv_123 Out
    	Matrix_Inv(inv_123_pre, 24, des_inv_123);
    	if (inv_123_pre != NULL)//释放inv_123_pre
    	{ 
    		for (int i = 0; i < 24; i++)
    		{ 
    			free(inv_123_pre[i]);
    		}
    		free(inv_123_pre);
    	}//！！！释放inv_123_pre
    	double **end_right = NULL;
    	mutipleMatrix(mutipl_123_trans_biachainv123, (double **)t_final123, &end_right, 24, 6000, 6000, 1, 1);
    	double **set123 = NULL;
    	mutipleMatrix((double **)des_inv_123, end_right, &set123, 24, 24, 24, 1, 0);
    	if (end_right != NULL)//释放end_right
    	{ 
    		for (int i = 0; i < 24; i++)
    		{ 
    			free(end_right[i]);
    		}
    		free(end_right);
    	}//！！！释放end_right
    	if (mutipl_123_trans_biachainv123 != NULL)//释放mutipl_123_trans_biachainv123
    	{ 
    		for (int i = 0; i < 24; i++)
    		{ 
    			free(mutipl_123_trans_biachainv123[i]);
    		}
    		free(mutipl_123_trans_biachainv123);
    	}//！！！释放mutipl_123_trans_biachainv123
    	 // ！！！set123计算方法；
    
    
    
    	 //set1456计算方法；
    	double **H_final1456_trans = NULL;
    	matrixTranspose((double **)H_final1456, &H_final1456_trans, 8000, 34);
    	double **mutipl_1456_trans_biachainv1456 = NULL;
    	mutipleMatrix(H_final1456_trans, biaocha_ffinv456, &mutipl_1456_trans_biachainv1456, 34, 8000, 8000, 8000, 0);
    	if (H_final1456_trans != NULL)//释放H_final1456_trans
    	{ 
    		for (int i = 0; i < 34; i++)
    		{ 
    			free(H_final1456_trans[i]);
    		}
    		free(H_final1456_trans);
    	}//！！！释放H_final1456_trans
    	double **inv_1456_pre = NULL;
    	mutipleMatrix(mutipl_1456_trans_biachainv1456, (double **)H_final1456, &inv_1456_pre, 34, 8000, 8000, 34, 1);
    
    	//will_inv_1456 Out
    	double **des_inv_1456 = (double **)calloc(34, sizeof(double *));
    	if (des_inv_1456 == NULL)
    	{ 
    		printf("des_inv_123 calloc error\n");
    		return;
    	}
    	for (int i = 0; i < 34; i++)
    	{ 
    		des_inv_1456[i] = (double *)calloc(34, sizeof(double));
    	}
    	// !!!will_inv_1456 Out
    	Matrix_Inv(inv_1456_pre, 34, des_inv_1456);
    
    	if (inv_1456_pre != NULL)//释放H_final1456_trans
    	{ 
    		for (int i = 0; i < 34; i++)
    		{ 
    			free(inv_1456_pre[i]);
    		}
    		free(inv_1456_pre);
    	}//！！！释放H_final1456_trans
    	double **end_right_1456 = NULL;
    	mutipleMatrix(mutipl_1456_trans_biachainv1456, (double **)t_final1456, &end_right_1456, 34, 8000, 8000, 1, 1);
    	double **set1456 = NULL;
    	mutipleMatrix((double **)des_inv_1456, end_right_1456, &set1456, 34, 34, 34, 1, 0);
    	if (end_right_1456 != NULL)//释放H_final1456_trans
    	{ 
    		for (int i = 0; i < 34; i++)
    		{ 
    			free(end_right_1456[i]);
    		}
    		free(end_right_1456);
    	}//！！！释放H_final1456_trans
    	if (mutipl_1456_trans_biachainv1456 != NULL)//释放mutipl_1456_trans_biachainv1456
    	{ 
    		for (int i = 0; i < 34; i++)
    		{ 
    			free(mutipl_1456_trans_biachainv1456[i]);
    		}
    		free(mutipl_1456_trans_biachainv1456);
    	}//！！！释放mutipl_1456_trans_biachainv1456
    	 // ！！！set1456计算方法；
    
    	double *myresult1 = (double *)calloc(24, sizeof(double));
    	double *myresult2 = (double *)calloc(34, sizeof(double));
    	for (int i = 0; i < 24; i++)
    	{ 
    		myresult1[i] = set123[i][0];
    	}
    	for (int j = 0; j < 34; j++)
    	{ 
    		myresult2[j] = set1456[j][0];
    	}
    	*result1 = myresult1; *result2 = myresult2;
    	myresult1 = NULL; myresult2 = NULL;
    	if (set123 != NULL)
    	{ 
    		for (int i = 0; i < 24; i++)
    		{ 
    			free(set123[i]);
    		}
    		free(set123);
    	}
    	if (set1456 != NULL)
    	{ 
    		for (int i = 0; i < 34; i++)
    		{ 
    			free(set1456[i]);
    		}
    		free(set1456);
    	}
    }