Modern Robotics刚体运动算法（C语言）

最新推荐文章于 2021-05-25 08:11:24 发布
最新推荐文章于 2021-05-25 08:11:24 发布
阅读量979 收藏 8
点赞数 2
分类专栏： 机器人 文章标签： modern robotics 刚体运动 齐次矩阵 运动学
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/Galaxy_Robot/article/details/109141697
版权

机器人运动学之刚体运动算法（C语言）

在这里插入图片描述

摘要

在上一篇博客 刚体运动的矩阵表示和指数坐标表示总结了刚体运动的矩阵表示和指数坐标表示，介绍了它们之间的联系和相互转换的。这些算法在教材《 Modern Robotics Mechanics,Planning , and Control》给出了Matlab和python语言的实现。我在这里再给出这些算法的C语言实现。欢迎反馈存在的bug。 源码不断更新完善，最新代码可从gihub 下载–链接

头文件

提供的算法函数包括如下头文件AlgorithmModule.h声明的函数。

/**
 * @brief			Description: Algorithm module of robotics, according to the
 *					book[modern robotics : mechanics, planning, and control].
 * @file:			RobotAlgorithmModule.h
 * @author:			Brian
 * @date:			2019/03/01 12:23
 * Copyright(c) 	2019 Brian. All rights reserved.
 *					  
 * Contact 			https://blog.csdn.net/Galaxy_Robot
 * @note:     
 * @warning: 		
*/
#ifndef RobotALGORITHMMODULE_H_
#define RobotALGORITHMMODULE_H_
#ifdef __cplusplus
extern "C" {
#endif
	#define		PI					3.14159265358979323846
	//if the norm of vector is near zero(< 1.0E-6),regard as zero.
	#define		ZERO_VALUE			1.0E-6		
	//number of joints,be used when Compute the Jacobian.	

	/**
	*@brief			Description:use GrublersFormula calculate The number of degrees of
	*				freedom of a mechanism with links and joints.
	*@param[in]		N		the number of links( including the ground ).
	*@param[in]		m		the number of degrees of freedom of a rigid body
	*						(m = 3 for planar mechanisms and m = 6 for spatial mechanisms).
	*@param[in]		J		the number of joints.
	*@param[in]		f		the number of freedoms provided by each joint..
	*@return				The number of degrees of freedom of a mechanism
	*@note:					This formula holds only if all joint constraints are independent.
	*						If they are not independent then the formula provides a lower
	*						bound on the number of degrees of freedom.
	*@waring:
	*/
	int GrublersFormula(int m, int N,  int J, int *f);

	/**
	*@brief			Computes the inverse of the rotation matrix R.
	*@param[in]		R		rotation matrix .
	*@param[out]	InvR	inverse matrix of R.
	*@note					Input R must be a 3x3 rotation matrix.
	*/
	void RotInv(double R[3][3], double InvR[3][3]);

	/**
	*@brief			Description:Returns the 3 x 3 skew-symmetric matrix corresponding to omg.
	*@param[in]		omg		a unit 3-vector.
	*@param[out]	so3Mat	the 3 x 3 skew-symmetric matrix corresponding to omg.
	*@note:
	*@waring:
	*/
	void VecToso3(double omg[3], double so3Mat[3][3]);

	/**
	*@brief			Description:Returns the 3-vector corresponding to the 3×3 skew-symmetric matrix so3mat.
	*@param[in]		so3Mat		the 3×3 skew-symmetric matrix so3mat. 
	*@param[out]	omg			the 3-vector.
	*@note:
	*@waring:
	*/
	void so3ToVec(double so3Mat[3][3], double omg[3]);

	/**
	*@brief			Description:Extracts  the unit rotation axis omghat and the corresponding rotation angle theta from
	*				exponential coordinates omghat*theta for rotation, expc3.
	*@param[in]		expc3
	*@param[out]	omghat		the unit vector of rotation axis .
	*@param[out]	theta		rotation angle.
	*@retval        0			success.
	*@retval		1			failure,normal of expc3 is zero or near zero(<1.0E-6).
	*@note:
	*@waring:
	*/
	void AxisAng3(double expc3[3], double omghat[3], double *theta);

	/**
	*@brief			Description:Computes the rotation matrix R in SO(3) corresponding to
	*				the matrix exponential of so3mat in so(3).
	*@param[in]		so3Mat		[omghat]*theta,matrix exponential of so3mat in so(3).
	*@param[out]	R			rotation matrix R in SO(3).
	*@note:
	*@waring:
	*/
	void MatrixExp3(double so3Mat[3][3], double R[3][3]);

	/**
	*@brief			Description:Computes the matrix logarithm so3mat in so(3) of the rotation matrix R in SO(3).
	*@param[in]		R		the rotation matrix
	*@param[out]	so3Mat	matrix logarithm
	*@note:
	*@waring:
	*/
	void MatrixLog3(double R[3][3], double so3Mat[3][3]);

	/**
	*@brief			Description:Builds the homogeneous transformation matrix T corresponding to a rotation
	*				matrix R in SO(3) and a position vector p in R3.
	*@param[in]		R		a rotation matrix R in SO(3).
	*@param[in]		p		a position vector p in R3.
	*@param[out]	T		the homogeneous transformation matrix T.
	*@note:
	*@waring:
	*/
	void RpToTrans(double R[3][3], double p[3], double T[4][4]);

	/**
	*@brief			Description: Extracts the rotation matrix R and position vector p
	*				from a homogeneous transformation matrix T.
	*@param[in]		T		a homogeneous transformation matrix.
	*@param[out]	R		the rotation matrix.
	*@param[out]	p		position vector.
	*@note:
	*@waring:
	*/
	void TransToRp(double T[4][4], double R[3][3], double p[3]);

	/**
	*@brief			Description:Computes the inverse of a homogeneous transformation matrix T.
	*@param[in]		T		a homogeneous transformation matrix.
	*@param[out]	InvT	the inverse of T.
	*@note:
	*@waring:
	*/
	void TransInv(double T[4][4], double InvT[4][4]);

	/**
	*@brief			Description:Computes the se(3) matrix corresponding to a 6 - vector twist V.
	*@param[in]		V		a 6 - vector twist V.
	*@param[out]	se3Mat	the se(3) matrix.
	*@note:
	*@waring:
	*/
	void VecTose3(double V[6], double se3Mat[4][4]);

	/**
	*@brief			Description:Computes the 6-vector twist corresponding to an se(3) matrix se3mat.
	*@param[in]		se3Mat			an se(3) matrix.
	*@param[out]	V				he 6-vector twist.
	*@note:
	*@waring:
	*/
	void se3ToVec(double se3Mat[4][4], double V[6]);

	/**
	*@brief			Description:Computes the 6 × 6 adjoint representation [AdT ] of the homogeneous transformation matrix T.
	*@param[in]		T		a homogeneous transformation matrix.
	*@param[out]	AdT		the 6 × 6 adjoint representation [AdT ].
	*@note:
	*@waring:
	*/
	void Adjoint(double T[4][4], double AdT[6][6]);

	/**
	*@brief			Description: Extracts the normalized screw axis S and the distance traveled along the screw
	*				theta from the 6-vector of exponential coordinates S*theta.
	*@param[in]		expc6		the 6-vector of exponential coordinates.
	*@param[out]	S			the normalized screw axis.
	*@param[out]	theta		the distance traveled along the screw.
	*@retval		0			success;
	*@retval		1			failure,because the input expc6 is a zero vector
	*@note:
	*@waring:
	*/
	void AxisAng6(double expc6[6], double S[6], double *theta);

	/**
	*@brief			Description: Computes the homogeneous transformation matrix T in SE(3) corresponding to
	*				the matrix exponential of se3mat in se(3).
	*@param[in]		se3mat			the matrix exponential of se3mat in se(3).
	*@param[out]	T				the homogeneous transformation matrix T in SE(3).
	*@note:
	*@waring:
	*/
	void MatrixExp6(double se3Mat[4][4], double T[4][4]);

	/**
	*@brief			Description: Computes the matrix logarithm se3mat in se(3) of
	*				the homogeneous transformation matrix T in SE(3)
	*@param[in]		T			the homogeneous transformation matrix.
	*@param[out]	se3Mat		the matrix logarithm of T.
	*@note:
	*@waring:
	*/
	void MatrixLog6(double T[4][4], double se3Mat[4][4]);

实现文件

以上函数的在如下的文件RobotAlgorithmModule.c中实现

/**
 * @brief			Description: Algorithm module of robotics, according to the
 *					book[modern robotics : mechanics, planning, and control].
 * @file:			RobotAlgorithmModule.c
 * @author:			Brian
 * @date:			2019/03/01 12:23
 * Copyright(c) 	2019 Brian. All rights reserved.
 *					  
 * Contact 			https://blog.csdn.net/Galaxy_Robot
 * @note:     
 * @warning: 		
*/

#include "RobotAlgorithmModule.h"
#include <math.h>
#include <string.h>
#include <stdlib.h>
#include <stdio.h>

/**
*@brief Description:use GrublersFormula calculate The number of degrees of 
freedom of a mechanism with links and joints.
*@param[in]		N		the number of links( including the ground ).
*@param[in]		m		the number of degrees of freedom of a rigid body
*						(m = 3 for planar mechanisms and m = 6 for spatial mechanisms).
*@param[in]		J		the number of joints.
*@param[in]		f		the number of freedoms provided by each joint..
*@return				The number of degrees of freedom of a mechanism
*@note:					This formula holds only if all joint constraints are independent.
*						If they are not independent then the formula provides a lower 
*						bound on the number of degrees of freedom.
*@waring:
*/
int GrublersFormula(int m, int N, int J, int *f)
{
	int i;
	int dof;
	int c = 0;
	for (i=1;i<=J;i++)
	{
		c = c + f[i-1];
	}
	dof = m*(N - 1 - J) + c;
	return dof;
}

/**
*@brief Description: Make  matrix b equal to  matrix a.
*@param[in]			a		a 3 x 3  matrix.
*@param[out]		b		result,b=a.
*@return		No return value.
*@note:
*@waring:
*/
void Matrix3Equal(double a[][3], double b[][3])
{
	int i;
	int j;
	for (i = 0; i < 3; i++)
	{
		for (j = 0; j < 3; j++)
		{
			b[i][j] = a[i][j];
		}
	}
	return;
}

/**
*@brief Description: Make  matrix b equal to  matrix a.
*@param[in]			a		a 4 x 4  matrix.
*@param[out]		b		result,b=a.
*@return		No return value.
*@note:
*@waring:
*/
void Matrix4Equal(double a[][4], double b[][4])
{
	int i;
	int j;
	for (i = 0; i < 4; i++)
	{
		for (j = 0; j < 4; j++)
		{
			b[i][j] = a[i][j];
		}
	}
	return;
}


/**
*@brief Description:Calculate a 3 x 3  matrix add a 3 x 3  matrix.
*@param[in]		a		a 3 x 3  matrix.
*@param[in]		b		a 3 x 3  matrix.
*@param[out]	c		result of a+b.
*@return		No return value.
*@note:
*@waring:
*/
void Matrix3Add(double a[][3], double b[][3], double c[][3])
{
	int i;
	int j;
	for (i = 0; i < 3; i++)
	{
		for (j = 0; j < 3; j++)
		{
			c[i][j] = a[i][j] + b[i][j];
		}
	}
	return;
}

/**
*@brief Description:Calculate a 3 x 3  matrix Subtract a 3 x 3  matrix.
*@param[in]		a		a 3 x 3  matrix.
*@param[in]		b		a 3 x 3  matrix.
*@param[out]	c		result of a-b.
*@return		No return value.
*@note:
*@waring:
*/
void Matrix3Sub(double a[][3], double b[][3], double c[][3])
{
	int i;
	int j;
	for (i = 0; i < 3; i++)
	{
		for (j = 0; j < 3; j++)
		{
			c[i][j] = a[i][j] - b[i][j];
		}
	}
	return;
}

/**
*@brief Description: Calculate two 3 x 3  matrix multiplication. 
*@param[in]		a		a 3 x 3  matrix.		
*@param[in]		b		a 3 x 3  matrix.	
*@param[out]	c		result of a*b.
*@return		No return value.
*@note:
*@waring:
*/
void Matrix3Mult(double a[][3],double b[][3],double c[][3])
{
	int i;
	int j;
	int k;
	for (i=0;i<3;i++)
	{		
		for (j=0;j<3;j++)
		{
			c[i][j] = 0.0;
			for (k=0;k<3;k++)
			{
				c[i][j] = c[i][j] + a[i][k] * b[k][j];
			}

		}
	}
	return;
}

/**
*@brief Description: Calculate two 4 x 4  matrix multiplication.
*@param[in]		a		a 4 x 4  matrix.
*@param[in]		b		a 4 x 4  matrix.
*@param[out]	c		result of a*b.
*@return		No return value.
*@note:
*@waring:
*/
void Matrix4Mult(double a[][4], double b[][4], double c[][4])
{
	int i;
	int j;
	int k;
	for (i = 0; i < 4; i++)
	{
		for (j = 0; j < 4; j++)
		{
			c[i][j] = 0.0;
			for (k = 0; k < 4; k++)
			{
				c[i][j] = c[i][j] + a[i][k] * b[k][j];
			}

		}
	}
	return;
}


/**
*@brief Description: Calculate  3 x 3  matrix multiply a value.
*@param[in]		a		a 3 x 3  matrix.
*@param[in]		Value	a scalar value.
*@param[out]	c		result of a*Value.
*@return		No return value.
*@note:
*@waring:
*/
void Matrix3MultValue(double a[][3], double Value, double c[][3])
{
	int i;
	int j;
	for (i=0;i<3;i++)
	{
		for (j=0;j<3;j++)
		{
			c[i][j] = a[i][j] * Value;
		}
	}
	return;
}

/**
*@brief Description: Calculate  4 x 4  matrix multiply a value.
*@param[in]		a		a 4 x 4  matrix.
*@param[in]		Value	a scalar value.
*@param[out]	c		result of a*Value.
*@return		No return value.
*@note:
*@waring:
*/
void Matrix4MultValue(double a[][4], double Value, double c[][4])
{
	int i;
	int j;
	for (i = 0; i < 4; i++)
	{
		for (j = 0; j < 4; j++)
		{
			c[i][j] = a[i][j] * Value;
		}
	}
	return;
}


/**
*@brief Description:Computes the result of a 3 x 3 Matrix multiply a 3-vector.
*@param[in]		R			a 3 x 3 Matrix.
*@param[in]		vec1		an input of 3-vector.
*@param[out]	vec2		the output result of 3-vector.
*@return		No return value.
*@note:
*@waring:
*/
void Matrix3MultVec(double R[3][3], double vec1[3], double vec2[3])
{
	int i;
	int j;
	for (i=0;i<3;i++)
	{
		vec2[i] = 0;
		for (j=0;j<3;j++)
		{
			vec2[i] = vec2[i] + R[i][j] * vec1[j];
		}
	}
	return;
}

/**
*@brief Description:Computes a 3-vector add a 3-vector.
*@param[in]		vec1		a 3-vector.
*@param[in]		vec2		a 3-vector.
*@param[out]	vec3		result of vec1 + vec2;
*@return		No return value.
*@note:
*@waring:
*/
void Vec3Add(double vec1[3], double vec2[3], double vec3[3])
{
	vec3[0] = vec1[0] + vec2[0];
	vec3[1] = vec1[1] + vec2[1];
	vec3[2] = vec1[2] + vec2[2];
	return;
}


/**
*@brief Description:Computes result of two 3-vector cross product.
*@param[in]		vec1		a first 3-vector.
*@param[in]		vec2		a first 3-vector.
*@param[out]	vec3		result of vec1 cross product vec2.
*@return		No return value.
*@note:
*@waring:
*/
void Vec3Cross(double vec1[3],double vec2[3],double vec3[3])
{
	vec3[0] = vec1[1] * vec2[2] - vec2[1] * vec1[2];
	vec3[1] = -(vec1[0] * vec2[2] - vec2[0] * vec1[2]);
	vec3[2] = vec1[0] * vec2[1] - vec2[0] * vec1[1];
	return;
}

/**
*@brief Description:Computes a 3-vector multiply a scalar value.
*@param[in]		vec1			a 3-vector.
*@param[in]		value			a scalar value.
*@param[out]	vec2			result of vec1 * value. 
*@return		No return value.
*@note:
*@waring:
*/
void Vec3MultValue(double vec1[3], double value, double vec2[3])
{
	vec2[0] = vec1[0] * value;
	vec2[1] = vec1[1] * value;
	vec2[2] = vec1[2] * value;
	return;
}

/**
*@brief Description: Computes the norm of a 3-vector.
*@param[in]		vec			an input 3-vector.
*@retval		the norm of a an input 3-vector.
*@return		No return value.
*@note:
*@waring:
*/
double Vec3Norm(double vec[3])
{
	return sqrt(vec[0] * vec[0] + vec[1] * vec[1] + vec[2] * vec[2]);
}

/**
*@brief			Copy  matrix from a to  b .
*@param[in]		a		First matrix.
*@param[in]		m		rows of matrix a.
*@param[in]		n		columns of matrix a.
*@param[in]		b		Second matrix.
*@return		No return value.
*@note:
*@waring:
*/
void MatrixCopy(const double *a, int m, int n, double *b)
{
	memcpy(b, a, m*n * sizeof(double));
	return;
}

/**
*@brief			Computes Matrix transpose.
*@param[in]		a			a matrix.
*@param[in]		m			rows of matrix a.
*@param[in]		n			columns of matrix a.
*@param[out]	b			transpose of matrix a.
*@return		No return value.
*@note:
*@waring:
*/
void MatrixT(double *a, int m, int n, double *b)
{
	int i, j;
	for (i = 0; i < m; i++)
	{
		for (j = 0; j < n; j++)
		{
			b[j*m + i] = a[i*n + j];
		}
	}
	return;
}



/**
*@brief			Computes matrix multiply.
*@param[in]		a		First matrix.
*@param[in]		m		rows of matrix a.
*@param[in]		n		columns of matrix b.
*@param[in]		b		Second matrix.
*@param[in]		l		columns of matrix b.
*@param[out]	c		result of a*b.
*@return		No return value.
*@note:
*@waring:
*/
void MatrixMult(const double *a, int m, int n, const double *b, int l, double *c)
{
	int i, j, k;
	for (i = 0; i < m; i++)
	{
		for (j = 0; j < l; j++)
		{
			c[i*l + j] = 0.0;
			for (k = 0; k < n; k++)
			{
				c[i*l + j] = c[i*l + j] + a[i*n + k] * b[k*l + j];
			}
		}
	}
	return;
}



/**
*@brief			Computes the inverse of the rotation matrix R.
*@param[in]		R		rotation matrix .
*@param[out]	InvR	inverse matrix of R.
*@return		No return value.
*@note					Input R must be a 3 x 3 rotation matrix.
*/
void RotInv(double R[3][3], double InvR[3][3])
{
	int i;
	int j;
	for (i=0;i<3;i++)
	{
		for (j=0;j<3;j++)
		{
			InvR[i][j] = R[j][i];
		}
	}
	return;
}

/**
*@brief Description:Returns the 3 x 3 skew-symmetric matrix corresponding to omg.
*@param[in]		omg		a 3-vector.
*@param[out]	so3Mat	the 3 x 3 skew-symmetric matrix corresponding to omg.
*@return		No return value.
*@note:
*@waring:
*/
void VecToso3(double omg[3],double so3Mat[3][3])
{
	so3Mat[0][0] = 0.0;		so3Mat[0][1] = -omg[2]; so3Mat[0][2] = omg[1];
	so3Mat[1][0] = omg[2];	so3Mat[1][1] = 0;		so3Mat[1][2] = -omg[0];
	so3Mat[2][0] = -omg[1];	so3Mat[2][1] = omg[0];	so3Mat[2][2] = 0.0;
	return;
}


/**
*@brief			Description:Returns the 3-vector corresponding to the 3 x 3 skew-symmetric matrix so3mat.
*@param[in]		so3Mat		a 3 x 3 skew-symmetric matrix so3mat.
*@param[out]	omg			the 3-vector.
*@return		No return value.
*@note:
*@waring:
*/
void so3ToVec(double so3Mat[3][3], double omg[3])
{
	omg[0] = so3Mat[2][1]; omg[1] = so3Mat[0][2]; omg[2] = so3Mat[1][0];
	return;
}


/**
*@brief			Description:Extracts  the unit rotation axis omghat and the corresponding
*				rotation angle theta from exponential coordinates omghat*theta for rotation, expc3.
*@param[in]		expc3			
*@param[out]	omghat		the unit vector of rotation axis .
*@param[out]	theta		rotation angle.
*@return		No return value.
*@note:
*@waring:
*/
void AxisAng3(double expc3[3],double omghat[3],double *theta)
{
	int i;
	int ret = 0;
	*theta = sqrt(expc3[0] * expc3[0] + expc3[1] * expc3[1] + expc3[2] * expc3[2]);
	if (*theta<ZERO_VALUE)
	{
		omghat[0] = 0.0;
		omghat[1] = 0.0;
		omghat[2] = 0.0;
		*theta = 0.0;
		return;
	}
	for (i=0;i<3;i++)
	{
		omghat[i]=expc3[i]/(*theta);
	}
	return ;
}

/**
*@brief			Description:Computes the rotation matrix R in SO(3) corresponding to
*				the matrix exponential of so3mat in so(3).
*@param[in]		so3Mat		[omghat]*theta,matrix exponential of so3mat in so(3).			
*@param[out]	R			rotation matrix R in SO(3).
*@return		No return value.
*@note:
*@waring:
*/
void MatrixExp3(double so3Mat[3][3],double R[3][3])
{
	double omgtheta[3];
	double omghat[3] = { 0 };
	double theta = 0;
	int ret = 0;
	int i;
	int j;
	double MatI3[3][3] =
	{
		1,0,0,
		0,1,0,
		0,0,1
	};
	so3ToVec(so3Mat, omgtheta);
	AxisAng3(omgtheta, omghat, &theta);
	if (theta<ZERO_VALUE)
	{
		Matrix3Equal(MatI3, R);
		return ;
	}
	else
	{

		//calculate formula(3.51) in book [modern robotics : mechanics,planning,and control]
		double omgmat[3][3];
		double temp[3][3];
		Matrix3MultValue(so3Mat, 1.0 / theta, omgmat);
		Matrix3Mult(omgmat, omgmat, temp);
		for (i=0;i<3;i++)
		{
			for (j=0;j<3;j++)
			{
				R[i][j] = MatI3[i][j] + sin(theta)*so3Mat[i][j] / theta + (1 - cos(theta))*temp[i][j];
			}
		}
	}
	return;
}

/**
*@brief			Description:Computes the matrix logarithm so3mat in so(3) of the rotation matrix R in SO(3).
*@param[in]		R		the rotation matrix.
*@param[out]	so3Mat	matrix logarithm.
*@return		No return value.
*@note:
*@waring:
*/
void MatrixLog3(double R[3][3],double so3Mat[3][3])
{
	double omg[3] = { 0 };
	double acosinput = (R[0][0] + R[1][1] + R[2][2] - 1.0) / 2;
	if (acosinput > 1.0)
	{
		memset(so3Mat, 0, 9 * sizeof(double));
	}
	else if (acosinput<=-1.0)
	{
		if ((1.0+R[2][2])>= ZERO_VALUE)
		{
			omg[0] = 1.0 / sqrt(2 * (1.0 + R[2][2]))*R[0][2];
			omg[1] = 1.0 / sqrt(2 * (1.0 + R[2][2]))*R[1][2];
			omg[2] = 1.0 / sqrt(2 * (1.0 + R[2][2]))*(1.0 + R[2][2]);
		}
		else if ((1.0+R[1][1]>=ZERO_VALUE))
		{
			omg[0] = 1.0 / sqrt(2 * (1.0 + R[1][1]))*R[0][1];
			omg[1] = 1.0 / sqrt(2 * (1.0 + R[1][1]))*(1.0 + R[1][1]);
			omg[2] = 1.0 / sqrt(2 * (1.0 + R[1][1]))*R[2][1];
		}
		else 
		{
			omg[0] = 1.0 / sqrt(2 * (1.0 + R[0][0]))*(1.0 + R[0][0]);
			omg[1] = 1.0 / sqrt(2 * (1.0 + R[0][0]))*R[1][0];
			omg[2] = 1.0 / sqrt(2 * (1.0 + R[0][0]))*R[2][0];
		}
		omg[0] = PI*omg[0];
		omg[1] = PI*omg[1];
		omg[2] = PI*omg[2];
		VecToso3(omg, so3Mat);
	}
	else
	{
		int i;
		int j;
		double theta = acos(acosinput);
		for (i=0;i<3;i++)
		{
			for (j=0;j<3;j++)
			{
				so3Mat[i][j] = 1.0 / (2.0*sin(theta))*(R[i][j] - R[j][i])*theta;
			}
		}
	}

	return;
}

/**
*@brief					Description:Builds the homogeneous transformation matrix T corresponding to a rotation 
*						matrix R in SO(3) and a position vector p in R3.
*@param[in]		R		a rotation matrix R in SO(3).
*@param[in]		p		a position vector p in R3.
*@param[out]	T		the homogeneous transformation matrix T.
*@return		No return value.
*@note:
*@waring:
*/
void RpToTrans(double R[3][3], double p[3], double T[4][4])
{
	int i;
	int j;
	for (i=0;i<3;i++)
	{
		for (j=0;j<3;j++)
		{
			T[i][j] = R[i][j];
		}
	}
	T[0][3] = p[0];
	T[1][3] = p[1];
	T[2][3] = p[2];
	T[3][0] = 0.0; 
	T[3][1] = 0.0; 
	T[3][2] = 0.0;
	T[3][3] = 1.0;
	return;
}

/**
*@brief Description: Extracts the rotation matrix R and position vector p 
from a homogeneous transformation matrix T.
*@param[in]		T		a homogeneous transformation matrix.
*@param[out]	R		the rotation matrix.
*@param[out]	p		position vector.
*@return		No return value.
*@note:
*@waring:
*/
void TransToRp(double T[4][4], double R[3][3], double p[3])
{
	int i;
	int j;
	for (i=0;i<3;i++)
	{
		for (j=0;j<3;j++)
		{
			R[i][j] = T[i][j];
		}
	}
	p[0] = T[0][3];
	p[1] = T[1][3];
	p[2] = T[2][3];
	return;
}

/**
*@brief Description:Computes the inverse of a homogeneous transformation matrix T.
*@param[in]		T		a homogeneous transformation matrix.
*@param[out]	InvT	the inverse of T.
*@return		No return value.
*@note:
*@waring:
*/
void TransInv(double T[4][4],double InvT[4][4])
{
	double R[3][3];
	double InvR[3][3];
	double p[3];
	double p2[3];
	TransToRp(T, R, p);
	RotInv(R, InvR);
	Matrix3MultValue(InvR, -1.0, R);
	Matrix3MultVec(R, p, p2);
	RpToTrans(InvR, p2, InvT);
	return;
}

/**
*@brief Description:Computes the se(3) matrix corresponding to a 6-vector twist V.
*@param[in]		V		a 6-vector twist V.
*@param[out]	se3Mat	the se(3) matrix.
*@return		No return value.
*@note:
*@waring:
*/
void VecTose3(double V[6], double se3Mat[4][4])
{
	double so3Mat[3][3];
	double omg[3];
	int i;
	int j;
	omg[0] = V[0];
	omg[1] = V[1];
	omg[2] = V[2];
	VecToso3(omg, so3Mat);
	for (i=0;i<3;i++)
	{
		for (j=0;j<3;j++)
		{
			se3Mat[i][j] = so3Mat[i][j];
		}

	}
	se3Mat[0][3] = V[3];
	se3Mat[1][3] = V[4];
	se3Mat[2][3] = V[5];
	se3Mat[3][0] = 0.0;
	se3Mat[3][1] = 0.0;
	se3Mat[3][2] = 0.0;
	se3Mat[3][3] = 0.0;
	return;
}

/**
*@brief Description:Computes the 6-vector twist corresponding to an se(3) matrix se3mat.
*@param[in]		se3Mat			an se(3) matrix.
*@param[out]	V				he 6-vector twist.
*@return		No return value.
*@note:
*@waring:
*/
void se3ToVec(double se3Mat[4][4],double V[6])
{
	V[0] = se3Mat[2][1];
	V[1] = se3Mat[0][2];
	V[2] = se3Mat[1][0];
	V[3] = se3Mat[0][3];
	V[4] = se3Mat[1][3];
	V[5] = se3Mat[2][3];
	return;
}

/**
*@brief Description:Computes the 6 x 6 adjoint representation [AdT ] 
of the homogeneous transformation matrix T.
*@param[in]		T		a homogeneous transformation matrix.
*@param[out]	AdT		the 6 x 6 adjoint representation [AdT ].
*@return		No return value.
*@note:
*@waring:
*/
void Adjoint(double T[4][4], double AdT[6][6])
{
	double R[3][3];
	double p[3];
	double so3Mat[3][3];
	int i;
	int j;
	TransToRp(T, R, p);
	VecToso3(p, so3Mat);
	
	for (i=0;i<3;i++)
	{
		for (j=0;j<3;j++)
		{
			AdT[i][j] = R[i][j];
		}
	}
	for (i = 0; i < 3; i++)
	{
		for (j = 3; j < 6; j++)
		{
			AdT[i][j] = 0.0;
		}
	}
	for (i=3;i<6;i++)
	{
		for (j = 3; j < 6; j++)
		{
			AdT[i][j] = R[i-3][j-3];
		}
	}
	double mat[3][3];
	Matrix3Mult(so3Mat, R, mat);
	for (i = 3; i < 6; i++)
	{
		for (j = 0; j < 3; j++)
		{
			AdT[i][j] = mat[i-3][j];
		}
	}

	return;
}

/**
*@brief			Description: Computes a normalized screw axis representation S of a screw described by 
*				a unit vector s in the direction of the screw axis, located at the point q, with pitch h.
*@param[in]		q		a point lying on the screw axis.
*@param[in]		s		a unit vector in the direction of the screw axis.
*@param[in]		h		the pitch of the screw axis.
*@param[in]		S		a normalized screw axis representation.
*@return		No return value.
*@note:
*@waring:
*/
void ScrewToAxis(double q[3], double s[3], double h,double S[6])
{
	double v[3];
	double temp[3];
	Vec3Cross(s, q, temp);
	Vec3MultValue(temp, -1.0, temp);
	Vec3MultValue(s, h, v);
	Vec3Add(v, temp, v);
	S[0] = s[0];
	S[1] = s[1];
	S[2] = s[2];
	S[3] = v[0];
	S[4] = v[1];
	S[5] = v[2];
	return;
}

/**
*@brief Description: Extracts the normalized screw axis S and the distance traveled along the screw
theta from the 6-vector of exponential coordinates S*theta.
*@param[in]		expc6		the 6-vector of exponential coordinates.
*@param[out]	S			the normalized screw axis.
*@param[out]	theta		the distance traveled along the screw.
*@return		No return value.
*@note:
*@waring:
*/
void AxisAng6(double expc6[6],double S[6],double *theta)
{
	*theta = Vec3Norm(expc6);
	if (*theta<ZERO_VALUE)
	{
		*theta = Vec3Norm(&expc6[3]);
		if (*theta<ZERO_VALUE)
		{
			*theta=0.0;
			//S is undefine,no motion at all.
			S[0] = 0.0; S[1] = 0.0; S[2] = 0.0; S[3] = 0.0; S[4] = 0.0; S[5] = 0.0;
			return;
		}
	}
	Vec3MultValue(expc6, 1.0 / (*theta), S);
	Vec3MultValue(&expc6[3], 1.0 / (*theta), &S[3]);
	return ;
}

/**
*@brief			Description: Computes the homogeneous transformation matrix T in SE(3) corresponding to
*				the matrix exponential of se3mat in se(3).
*@param[in]		se3mat			the matrix exponential of se3mat in se(3).
*@param[out]	T				the homogeneous transformation matrix T in SE(3).
*@return		No return value.
*@note:
*@waring:
*/
void MatrixExp6(double se3Mat[4][4], double T[4][4])
{
	int i;
	int j;
	double so3mat[3][3];
	double omgmat[3][3];
	double temp[3][3];
	double Gtheta[3][3];
	double p[3];
	double v[3];
	double omgtheta[3];
	double omghat[3];
	double theta;
	double MatI3[3][3] =
	{
		1.0,0.0,0.0,
		0.0,1.0,0.0,
		0.0,0.0,1.0
	};
	TransToRp(se3Mat, so3mat, p);//extracts so3mat from se3mat
	so3ToVec(so3mat, omgtheta);
	if (Vec3Norm(omgtheta)<ZERO_VALUE)
	{
		for (i=0;i<3;i++)
		{
			for (j=0;j<3;j++)
			{
				T[i][j] = MatI3[i][j];
			}
		}
		T[0][3] = se3Mat[0][3];
		T[1][3] = se3Mat[1][3];
		T[2][3] = se3Mat[2][3];
		T[3][0] = 0.0;
		T[3][1] = 0.0;
		T[3][2] = 0.0;
		T[3][3] = 1.0;
	}
	else
	{
		AxisAng3(omgtheta, omghat, &theta);
		MatrixExp3(so3mat, temp);
		for (i = 0; i < 3; i++)
		{
			for (j = 0; j < 3; j++)
			{
				T[i][j] = temp[i][j];
			}
		}
		Matrix3MultValue(so3mat, 1.0/theta,omgmat);
		Matrix3Mult(omgmat, omgmat, temp);
		for (i = 0; i < 3; i++)
		{
			for (j = 0; j < 3; j++)
			{
				Gtheta[i][j] = MatI3[i][j] * theta + (1.0 - cos(theta))*omgmat[i][j] + (theta - sin(theta))*temp[i][j];
			}
		}	
		v[0] = p[0] / theta; 
		v[1] = p[1] / theta;
		v[2] = p[2] / theta;
		Matrix3MultVec(Gtheta, v, p);
		T[0][3] = p[0]; T[1][3] = p[1]; T[2][3] = p[2];
		T[3][0] = 0.0;
		T[3][1] = 0.0;
		T[3][2] = 0.0;
		T[3][3] = 1.0;
	}
	return;
}


/**
*@brief Description: Computes the matrix logarithm se3mat in se(3) of 
the homogeneous transformation matrix T in SE(3)
*@param[in]		T			the homogeneous transformation matrix.			
*@param[out]	se3Mat		the matrix logarithm of T.
*@return		No return value.
*@note:
*@waring:
*/
void MatrixLog6(double T[4][4], double se3Mat[4][4])
{
	int i;
	int j;
	double R[3][3];
	double so3mat[3][3];
	double p[3];
	TransToRp(T, R, p);
	MatrixLog3(R, so3mat);
	int flag = 0;
	for (i=0;i<3;i++)
	{
		for (j=0;j<3;j++)
		{
			if (fabs(so3mat[i][j])<ZERO_VALUE)
			{
				continue;
			}
			else
			{
				flag = 1;
				break;
			}
		}
	}
	if (!flag)
	{
		for (i=0;i<3;i++)
		{
			for (j=0;j<3;j++)
			{
				se3Mat[i][j] = 0.0;
			}

		}
		se3Mat[0][3] = T[0][3];
		se3Mat[1][3] = T[1][3];
		se3Mat[2][3] = T[2][3];
		se3Mat[3][0] = 0.0;
		se3Mat[3][1] = 0.0;
		se3Mat[3][2] = 0.0;
		se3Mat[3][3] = 0.0;
	}
	else
	{
		double MatI3[3][3] =
		{
			1.0,0.0,0.0,
			0.0,1.0,0.0,
			0.0,0.0,1.0
		};
		double v[3];
		double temp[3][3];
		double InvGtheta[3][3];
		double theta = acos((R[0][0] + R[1][1] + R[2][2] - 1.0) / 2.0);
		for (i=0;i<3;i++)
		{
			for (j=0;j<3;j++)
			{
				se3Mat[i][j] = so3mat[i][j];
			}
		}

		Matrix3Mult(so3mat, so3mat, temp);
		for (i = 0; i < 3; i++)
		{
			for (j = 0; j < 3; j++)
			{
				InvGtheta[i][j] = MatI3[i][j] - 0.5*so3mat[i][j] + (1.0 / theta - 0.5*1.0 / tan(0.5*theta))*temp[i][j] / theta;
			}
		}
		Matrix3MultVec(InvGtheta, p, v);
		se3Mat[0][3] = v[0];
		se3Mat[1][3] = v[1];
		se3Mat[2][3] = v[2];
		se3Mat[3][0] = 0.0;
		se3Mat[3][1] = 0.0;
		se3Mat[3][2] = 0.0;
		se3Mat[3][3] = 0.0;
	}
	return ;
}

测试demo

对头文件所列的每个函数均进行了测试，测试demo如下，每个函数的测试程序以test为前缀。如果要修改某个函数的测试参数，直接在test_xxx函数里修改，在main函数调用它就可以了。

/**
 * @brief			Description: Test Demos for robot algorithm modules.
 * @file:			TestDemo.c
 * @author:			Brian
 * @date:			2019/03/01 12:23
 * Copyright(c) 	2019 Brian. All rights reserved.
 *					  
 * Contact 			https://blog.csdn.net/Galaxy_Robot
 * @note:     
 * @warning: 		
*/
#include <stdio.h>
#include <math.h>
#include "RobotAlgorithmModule.h"
#define M_PI  3.14159265358979323846
void test_GrublersFormula();
void test_RotInv();
void test_VecToso3();
void test_so3ToVec();
void test_AxisAng3();
void test_MatrixExp3();
void test_MatrixLog3();
void test_RpToTrans();
void test_TransToRp();
void test_TransInv();
void test_VecTose3();
void test_se3ToVec();
void test_Adjoint();
void test_AxisAng6();
void test_MatrixExp6();
void test_MatrixLog6();
int main()
{
	test_JacobianSpace();
	return 0;
}

void test_GrublersFormula()
{
	int N = 8;
	int m = 3;
	int J = 9;
	int f[9] = { 1,1,1,1,1,1,1,1,1 };
	int dof = GrublersFormula(m, N, J, f);
	printf("dof=%d\n", dof);
}

void test_RotInv()
{
	double R[3][3] =
	{
		0,0,1,
		1,0,0,
		0,1,0
	};
	double InvR[3][3] = { {0} };
	RotInv(R, InvR);
	int i;
	printf("R:\n");
	for (i = 0; i < 3; i++)
	{
		printf("%lf %lf %lf\n", R[i][0], R[i][1], R[i][2]);
	}
	printf("InvR:\n");
	for (i=0;i<3;i++)
	{
		printf("%lf %lf %lf\n", InvR[i][0], InvR[i][1], InvR[i][2]);
	}
}

void test_VecToso3()
{
	double omg[3] = {1,2,3};
	double so3[3][3] = { {0} };
	int i;
	VecToso3(omg, so3);
	printf("omg:\n");

	printf("%lf %lf %lf\n", omg[0], omg[1], omg[2]);

	printf("so3:\n");
	for (i = 0; i < 3; i++)
	{
		printf("%lf %lf %lf\n", so3[i][0], so3[i][1], so3[i][2]);
	}
}

void test_so3ToVec()
{
	double so3[3][3] = 
	{
		0, -3, 2,
		3, 0, -1,
		-2, 1, 0
	};
	double omg[3] = { 0 }; 
	int i;
	so3ToVec(so3, omg);

	printf("so3:\n");
	for (i = 0; i < 3; i++)
	{
		printf("%lf %lf %lf\n", so3[i][0], so3[i][1], so3[i][2]);
	}
	printf("omg:\n");
	printf("%lf %lf %lf\n", omg[0], omg[1], omg[2]);

}

void test_AxisAng3()
{
	double expc3[3] = { 1,2,3 };
	double omghat[3] = { 0 };
	double theta = 0;
	AxisAng3(expc3, omghat, &theta);
	printf("expc3:\n");
	printf("%lf %lf %lf\n", expc3[0], expc3[1], expc3[2]);
	printf("omg:\n");
	printf("%lf %lf %lf\n", omghat[0], omghat[1], omghat[2]);
	printf("theta:\n");
	printf("%lf\n", theta);
	return;
}


void test_MatrixExp3()
{
	double so3mat[3][3] = 
	{
		0, -3, 2,
		3, 0, -1,
		-2, 1, 0 
	};
	double R[3][3] = {{ 0 }};
	MatrixExp3(so3mat, R);
	printf("R:\n");
	int i;
	for (i = 0; i < 3; i++)
	{
		printf("%lf %lf %lf\n", R[i][0], R[i][1], R[i][2]);
	}

	return;
}


void test_MatrixLog3()
{
	double theta = 30 * PI / 180;
	double R[3][3] =
	{
		0, 0, 1,
		1, 0, 0,
		0, 1, 0
	};
	double so3Mat[3][3] = { 0 };
	MatrixLog3(R, so3Mat);
	printf("so3Mat:\n");
	int i;
	for (i = 0; i < 3; i++)
	{
		printf("%lf %lf %lf\n", so3Mat[i][0], so3Mat[i][1], so3Mat[i][2]);
	}
	return;
}

void test_RpToTrans()
{
	double R[3][3] =
	{
		1,0,0,
		0,0,-1,
		0,1,0
	};
	double p[3] = { 1,2,5 };
	double T[4][4] = { {0} };
	RpToTrans(R, p, T);
	printf("T:\n");
	int i;
	for (i = 0; i < 4; i++)
	{
		printf("%lf %lf %lf %lf\n", T[i][0], T[i][1], T[i][2], T[i][3]);
	}
	return;
}

void test_TransToRp()
{
	double T[4][4] = {
		1, 0, 0, 0,
		0, 0, -1, 0,
		0, 1, 0, 3,
		0, 0, 0, 1
	};
	double R[3][3] = { {0} };
	double p[3] = { 0 };
	TransToRp(T, R, p);
	printf("R:\n");
	int i;
	for (i = 0; i < 3; i++)
	{
		printf("%lf %lf %lf\n", R[i][0], R[i][1], R[i][2]);
	}
	printf("p:\n");
	printf("%lf %lf %lf\n", p[0],p[1],p[2]);
	return;

}

void test_TransInv()
{
	double T[4][4] = {
		1, 0, 0, 0,
		0, 0, -1, 0,
		0, 1, 0, 3,
		0, 0, 0, 1	
	};
	double InvT[4][4];
	TransInv(T, InvT);
	printf("InvT:\n");
	int i;
	for (i = 0; i < 4; i++)
	{
		printf("%lf %lf %lf %lf\n", InvT[i][0], InvT[i][1], InvT[i][2], InvT[i][3]);
	}
	return;
}

void test_VecTose3()
{
	double V[6] = { 1,2,3,4,5,6 };
	double se3Mat[4][4];
	VecTose3(V, se3Mat);
	printf("se3Mat:\n");
	int i;
	for (i = 0; i < 4; i++)
	{
		printf("%lf %lf %lf %lf\n", se3Mat[i][0], se3Mat[i][1], se3Mat[i][2], se3Mat[i][3]);
	}
	return;
}


void test_se3ToVec()
{
	double V[6] = { 0 };
	double se3Mat[4][4] = {
		0, -3, 2, 4,
		3, 0, -1, 5,
		-2, 1, 0, 6,
		0, 0, 0, 0
	};
	se3ToVec(se3Mat, V);
	int i;
	printf("V:\n");
	for (i=0;i<6;i++)
	{
		printf("%lf\n", V[i]);
	}
	return;
}


void test_Adjoint()
{
	double T[4][4] = {
		1, 0, 0, 0,
		0, 0, -1, 0,
		0, 1, 0, 3,
		0, 0, 0, 1
	};
	double AdT[6][6] = { {0} };
	Adjoint(T, AdT);
	int i;
	int j;
	printf("AdT:\n");
	for (i = 0; i < 6; i++)
	{
		for(j = 0; j < 6; j++)
		{
			printf("%lf  ", AdT[i][j]);
		}
		printf("\n");
	}

	return;
}

void test_AxisAng6()
{
	double expc6[6] = { 0, 0, 0, 0, 0, 0 };
	double S[6];
	double theta;
	AxisAng6(expc6, S, &theta);

	printf("S:\n");
	int i;
	for (i = 0; i < 6; i++)
	{
		printf("%lf\n", S[i]);
	}
	printf("theta:\n");
	printf("%lf\n", theta);
	return;
}


void test_MatrixExp6()
{
	double se3Mat[4][4] =
	{
		0, 0, 0, 0,
		0, 0, -1.570796, 2.3562,
		0, 1.570796, 0, 2.3562,
		0, 0, 0, 0
	};
	double T[4][4];
	MatrixExp6(se3Mat, T);
	int i;
	printf("T:\n");
	for (i = 0; i < 4; i++)
	{
		printf("%lf %lf %lf %lf\n", T[i][0], T[i][1], T[i][2], T[i][3]);
	}
	return;
}


void test_MatrixLog6()
{
	double T[4][4] =
	{
		1, 0, 0, 0,
		0, 0, -1, 0,
		0, 1, 0, 3,
		0, 0, 0, 1
	};
	double se3mat[4][4];
	MatrixLog6(T, se3mat);
	int i;
	printf("se3mat:\n");
	for (i = 0; i < 4; i++)
	{
		printf("%lf %lf %lf %lf\n", se3mat[i][0], se3mat[i][1], se3mat[i][2], se3mat[i][3]);
	}

	return;
}

接下来继续学习机器人的正运动学和逆运动学，以及基本算法的实现。

参考文献

Kenvin M. Lynch , Frank C. Park, Modern Robotics Mechanics,Planning , and Control. May 3, 2017

Galaxy_Robot
关注
  • 2
    点赞
  • 踩
  • 8
    收藏
    觉得还不错? 一键收藏
  • 打赏
    打赏
  • 2
    评论
专栏目录
基于C语言实现的迷宫算法示例
08-29
该算法可以应用于多种领域，如游戏、robotics、地图导航等。迷宫算法的实现可以通过各种编程语言，如C语言、Java、Python等。 二、基于C语言实现的迷宫算法 在本文中，我们将使用C语言来实现迷宫算法。首先，我们...
无人机刚体运动学方程
EagleLY5894的博客
02-03 3660
无人机刚体运动学方程平动运动学转动运动学欧拉角和方向余弦矩阵四元数姿态微分方程四元数的归一化 平动运动学 由无人机平动动力学方程得到无人机的速度后，经过积分即可得到位置信息。 在NED坐标系下 XN=∫vNE=∫[u v w]T \bm{X}_N=\int \bm{v}^E_N=\int [u\ v\ w]^T XN​=∫vNE​=∫[u v w]T 在WG...
刚体的转动惯量的c语言题目,刚体的转动惯量题.doc
weixin_32722209的博客
05-25 867
刚体的转动惯量专题1.刚体的转动惯量的三要素刚体对某轴的转动惯量，是描述刚体在绕该轴的转动过程中转动惯性的物理量. 有转动惯量的定义式可看出，刚体的转动惯量是与下列三个因素有关的.(1)与刚体的质量有关. 例如半径相同的两个圆柱体，而它们的质量不同，显然，对于相应的转轴，质量大的转动惯量也较大.(2)在质量一定的情况下，与质量的分布有关. 例如，质量相同、半径也相同的圆盘与圆环，二者的质量分布不同...
刚体运动学公式_11.18 |【知识打卡】理论力学运动学
weixin_42358261的博客
01-10 9252
小千儿说：今天是2020年11月19日，星期四，距离明年勘察设计考试还有300多天，时间看起来还很长，但是计算到每天可以学习的时间就很少了。从今天开始，每天和我一起知识打卡吧！勘察备考 争分夺秒公共基础◆理论力学——第二节 运动学二、刚体的基本运动刚体的基本运动包括刚体的平行移动(简称移动或平动)和定轴转动，它主要研究刚体的运动规律和刚体的运动与其体上各点运动之间的关系。(一)刚体的平...
c语言运动算法,C语言 - 螺旋方阵：运动算法求任意n阶螺旋方阵
weixin_28339527的博客
05-23 365
如果你有任何不同看法，或更好的解决方案，可以留言给我，请不吝赐教！【算法思想】1、2、3、4、5、6... ... 按照数值的递增，依次判断每个值应该填在二维数组的那个位置(i, j)简单说就是运动填充一个二维数组[n][n]的过程，要注意方向的控制，以及退出循环的时机。【实例代码】/* 求任意n阶螺旋方阵：运动算法 */#include #include /* 定义空指针 */#ifndef N...
刚体运动学公式_经典运动学公式汇总
weixin_35676939的博客
12-24 2799
线运动表达式转动表达式位移r角位移速度dtrdv角速度dtd加速度22dtrddtvda角加速度22dtddtdeaeaannrrvan22rdtdva匀变速直线运动地计算公式atvv0匀变速转动地计算公式t020021attvxx20021tt)(20202xxavv)(20202...
modern robotics
06-28
It was at the IEEE International Conference on Robotics and Automation in Pasadena in 2008 when, over a beer, we decided to write an undergraduate textbook on robotics. Since 1996, Frank had been ...
Modern Robotics- Mechanics, Planning, and Control 现代机器人学：力学，规划与控制
01-24
现代机器人学，包含最新的机器人学基本知识，适合自学
Modern_Robotics
03-18
在这个"Modern_Robotics"项目中，我们使用MATLAB进行运动可视化。这可能包括模拟机器人的运动轨迹、关节角度变化、速度和加速度分析等。通过运行main.m文件，我们可以启动一个完整的程序，该程序将展示机器人在特定...
pinocchio:刚体动力学算法及其解析导数的快速灵活实现
05-12
Pinocchio基于重新审阅的Roy Featherstone算法为多关节系统实例化了最新的刚体算法。 此外，匹诺曹提供了主要的刚体算法的解析导数，例如递归牛顿-欧拉算法或铰接式身体算法。 Pinocchio首先是为机器人应用程序量身定制的，但它可以用于其他环境（生物力学，计算机图形学，视觉等）。 它建立在用于线性代数的Eigen和用于碰撞检测的FCL的基础上。 Pinocchio带有Python接口，可用于快速代码原型化，通过。 Pinocchio现在是各种机器人软件的核心，例如 ，开放式源代码和高效的机器人差分动态编程求解器，，开源和通用的分层控制器框架或 （开放式）。运动和操纵计划的源代码软件。 如果您想了解皮诺奇的内部行为和主要特征的更多信息，我们邀请您阅读相关。 如果您想直接进入Pinocchio ，只需一行即可（假设您拥有Conda）： conda安装pinocch
C语言编写的四轴伺服机器人轨迹仿真程序
04-24
利用C语言编写的四轴伺服机器人轨迹仿真程序，自己开发后搭建西门子S7-300/V90实验平台亲测好用！附件包括C程序、算法说明、程序使用说明，c语言编程软件未附在内，可自行下载，希望有帮助！
C语言机器人图形仿真
10-11
机器人图形仿真程序，曾经在学校C课设大赛中获奖
刚体运动知识
dingjianfeng2014的博客
06-08 1184
点，向量，坐标 内积：描述向量间的投影关系，结果是一个实数（点乘） 外积：外积的方向垂直于这两个向量，是两个向量张成的四边形的有向面积（叉乘） 旋转：用一个向量来描述三维空间中两个向量的旋转关系。在右手法则下，我们用右手的四个指头从a向量转到b向量，大拇指朝向就是旋转向量的方向，也就是外积的方向，其大小由a向量和b向量的夹角决定。 欧式变换：由旋转和平移两部分组成，同一个向量在各个坐标系下的长...
物理运动学公式汇总
weixin_34062469的博客
05-22 2099
 一、 直线运动,自由落体运动 二、 曲线运动、万有引力三、 力（常见的力、力的合成与分解）四、 动力学（运动和力）五、 振动和波（机械振动与机械振动的传播）六、冲量与动量(物体的受力与动量的变化） 一、质点的运动（1）------直线运动 1）匀变速直线运动 1.平均速度V＝s/t（定义式) 2.有用推论Vt*Vt-Vo*Vo＝2as 3.中间时刻速度Vt/2＝V平＝(Vt+Vo...
五自由度机械臂正逆运动学算法（Ｃ语言+Matlab）
热门推荐
xuuyann
07-13 2万+
/* * MyMatrix.h * * Created on: Jul 13, 2019 * Author: xuuyann */ #ifndef HEADER_MYMATRIX_H_ #define HEADER_MYMATRIX_H_ typedef struct MNode *PtrToMNode; struct MNode { int row; int col...
3D视觉（三）刚体运动及matlab实现
满城风絮
11-13 7862
刚体运动（rigid motion）：在三维空间中， 几何体作旋转和平移运动，称为刚体运动。 刚体运动可以理解为保持长度、角度、面积等特征不变的仿射变换，即内积和度量不变；具有物理意义的量，如梯度，散度和旋度都保持不变。从坐标变换上看， 旋转矩阵为正交矩阵。从群的角度看，刚体变换全体构成一个群。 （一）补一下数学知识： 正交矩阵 （1）正交矩阵每一列都是单位矩阵，并且两两正交。最简单的
三维空间刚体运动编程问题总结（SLAM第三章）
qq_35665380的博客
03-20 654
1.归一化中.normalized和.normailze的区别 转载：http://www.it610.com/article/4812836.htm 共同点：实现规范化，让一个向量保持相同的方向，但它的长度为1.0，如果这个向量太小而不能被规范化，一个零向量将会被返回。 不同点：Vector3.normalized的作特点是当前向量是不改变的并且返回一个新的规范化的向量；Vector3.Norm...
MODERN ROBOTICS MECHANICS 第三章 Rigid-Body Motions（刚体运动）
fengyu19930920的博客
03-14 1856
这一章非常重要，内容有些多，是后续章节的基础 刚体的速度由表示（twist）：3个角速度，3个角速度 刚体的受力由表示（wrench）：3个力矩，3个力 free vector：只有长度和方向的向量，例如线速度向量就是自由向量 space frame{s}:可以理解为基座上的坐标系，定系，也叫fixed frame boby frame{b}:可以理解为末端执行器上的坐标系，动系 ...
modern robotics pdf csdn
最新发布
10-16
"Modern Robotics PDF"是CSDN平台上提供的一个关于现代机器人的PDF文件。这个PDF文件是为了帮助人们了解机器人技术的最新发展而制作的。其中包含了关于机器人学的基础知识、算法和应用方面的内容。 这份PDF文件涵盖了多个主题，包括机器人的感知、控制、运动规划和学习等方面。它介绍了现代机器人系统的基本结构和组成部分，并详细讲解了机器人感知技术，包括视觉、声音和力觉等。此外，它还介绍了机器人控制技术，包括运动控制和力控制。 在这份PDF文件中，还有关于机器人运动规划的内容。它详细解释了机器人的路径规划、轨迹跟踪和运动计划等方面的技术。此外，它还介绍了机器人学习的方法，包括监督学习、强化学习和无监督学习等。这些内容对于想要研究和开发机器人系统的人们来说都非常有价值。 总而言之，"Modern Robotics PDF"是CSDN平台上提供的一份关于现代机器人的PDF文件。它涵盖了机器人学的基础知识、算法和应用方面的内容，对于想要深入了解现代机器人技术的人们来说是一份非常有用的资料。

“相关推荐”对你有帮助么？

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 2
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

Galaxy_Robot

你的鼓励将是我创作的最大动力！

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付：¥1
获取中
扫码支付

您的余额不足，请更换扫码支付或充值

打赏作者

实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值