HFUT_Firmament_EC
/
Customer_controller
11
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
Customer_controller/USER/RobotAlgorithmModule.h

753 lines
26 KiB
C
Raw Permalink Normal View History

完成正运动学解算和逆运动学中两个关节的解算
2024-10-03 21:43:35 +08:00
/**
* @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 PINV_MAX 200 //求矩阵伪逆允许最大维数
#define DEBUGMODE 1 //打印调试信息
#define PI 3.14159265358979323846
//if the norm of vector is near zero(< 1.0E-6),regard as zero.
#define ZERO_VECTOR 1.0E-6
#define ZERO_ELEMENT 1.0E-6
#define ZERO_ANGLE 1.0E-6
#define ZERO_DISTANCE 1.0E-6
#define ZERO_VALUE 1.0E-6
/**
*@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 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:
*@warning:
*/
void Matrix3Equal(double a[][3], double b[][3]);
/**
*@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:
*@warning:
*/
void Matrix4Equal(double a[][4], double b[][4]);
/**
*@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:
*@warning:
*/
void Matrix3Add(double a[][3], double b[][3], double c[][3]);
/**
*@brief Description:Calculate a 4 x 4 matrix add a 4 x 4 matrix.
*@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:
*@warning:
*/
void Matrix4Add(double a[][4], double b[][4], double c[][4]);
/**
*@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:
*@warning:
*/
void Matrix3Sub(double a[][3], double b[][3], double c[][3]);
/**
*@brief Description:Calculate a 4 x 4 matrix Subtract a 4 x 4 matrix.
*@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:
*@warning:
*/
void Matrix4Sub(double a[][4], double b[][4], double c[][4]);
/**
*@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:
*@warning:
*/
void Matrix3Mult(double a[][3], double b[][3], double c[][3]);
/**
*@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:
*@warning:
*/
将外设的初始化放到单独的文件中,增加bsp_can
2024-10-06 19:49:15 +08:00
void Matrix4Mult(double a[][4], double b[][4], double c[][4]);
完成正运动学解算和逆运动学中两个关节的解算
2024-10-03 21:43:35 +08:00
/**
*@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:
*@warning:
*/
void Matrix3MultValue(double a[][3], double Value, double c[][3]);
/**
*@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:
*@warning:
*/
void Matrix4MultValue(double a[][4], double Value, double c[][4]);
/**
*@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:
*@warning:
*/
void Matrix3MultVec(double R[3][3], double vec1[3], double vec2[3]);
/**
*@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]);
/**
*@brief Description: Computes the end-effector frame given the zero position of the end-effector M,
* the list of joint screws Slist expressed in the fixed-space frame, and the list of joint values thetalist.
*@param[in] M the zero position of the end-effector expressed in the fixed-space frame.
*@param[in] Joint Num the number of joints.
*@param[in] Slist the list of joint screws Slist expressed in the fixed-space frame.
* in the format of a matrix with the screw axes as the column.
*@param[in] thetalist the list of joint values.
*@param[out] T the end-effector frame expressed in the fixed-space frame.
*@return No return value.
*@note: when Slist is a matrix ,make sure that columns number of Slist is equal to JointNum,
* rows number of Slist 6 .The function call should be written as
* FKinSpace(...,(double *)Slist,...).
*@waring:
*/
void FKinSpace(double M[4][4], int JointNum, double *Slist, double *thetalist, double T[4][4]);
/**
*@brief Description:Computes the end-effector frame given the zero position of the end-effector M,
* the list of joint screws Blist expressed in the end-effector frame, and the list of joint values thetalist.
*@param[in] M the zero position of the end-effector expressed in the end-effector frame.
*@param[in] JointNum the number of joints.
*@param[in] Blist the list of joint screws Slist expressed in the end-effector frame.
* in the format of a matrix with the screw axes as the column.
*@param[in] thetalist the list of joint values.
*@param[out] T the end-effector frame expressed in the end-effector frame.
*@return No return value.
*@note: when Blist is a matrix ,make sure that columns number of Slist is equal to JointNum,
* rows number of Slist 6 .The function call should be written as
* FKinBody(...,(double *)Blist,...).
*@waring:
*/
void FKinBody(double M[4][4], int JointNum, double *Blist, double thetalist[], double T[4][4]);
/**
*@brief Description: Computes the body Jacobian Jb(theta) in 6×n given a list of joint screws Bi
expressed in the body frame and a list of joint angles.
*@param[in] Blist The joint screw axes in the end - effector frame when the manipulator is
* at the home position, in the format of a matrix with the screw axes as the row.
*@param[in] thetalist A list of joint coordinates.
*@param[out] Jb Body Jacobian matrix.
*@return No return value.
*@note: when Blist and Jb are matrixes ,make sure that columns number of Slist or Jb is equal to JointNum,
* rows number of Slist or Jb is 6 .The function call should be written as
* JacobianSpace(JointNum,(double *)Slist,thetalist,(double *)Jb).
*@waring:
*/
void JacobianBody(int JointNum, double *Blist, double *thetalist, double *Jb);
/**
*@brief Description:Computes the space Jacobian Js(theta) in R6 x n given a list of joint screws Si
* expressed in the fixed space frame and a list of joint angles.
*@param[in] Slist The joint screw axes expressed in the fixed space frame when the manipulator is
* at the home position, in the format of a matrix with the screw axes as the column.
*@param[in] thetalist A list of joint coordinates.
*@param[out] Js Space Jacobian matrix.
*@return No return value.
*@note: when Slist and Js are matrixes ,make sure that columns number of Slist or Js is equal to JointNum,
* rows number of Slist or Js is 6 .The function call should be written as
* JacobianSpace(JointNum,(double *)Slist,thetalist,(double *)Js).
*@waring:
*/
void JacobianSpace(int JointNum, double *Slist, double *thetalist, double *Js);
/**
*@brief Given the matrix a,this routine computes its singular value decomposition.
*@param[in/out] a input matrix a or output matrix u.
*@param[in] m number of rows of matrix a.
*@param[in] n number of columns of matrix a.
*@param[in] tol any singular values less than a tolerance are treated as zero.
*@param[out] w output vector w.
*@param[out] v output matrix v.
*@return No return value.
*@note: input a must be a m rows and n columns matrix,w is n-vector,v is a n rows and n columns matrix.
*@waring:
*/
int svdcmp(double *a, int m, int n,double tol, double *w, double *v);
/**
*@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.
*@note:
*@waring:
*/
void MatrixCopy(const double *a, int m, int n, double *b);
/**
*@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.
*@note:
*@waring:
*/
void MatrixMult(const double *a, int m, int n, const double *b, int l, double *c);
/**
*@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.
*@note:
*@waring:
*/
void MatrixT(double *a, int m, int n, double *b);
/**
*@brief Computes Moore-Penrose pseudoinverse by Singular Value Decomposition (SVD) algorithm.
*@param[in] a an arbitrary order matrix.
*@param[in] m rows.
*@param[in] n columns.
*@param[in] tol any singular values less than a tolerance are treated as zero.
*@param[out] b Moore-Penrose pseudoinverse of matrix a.
*@return 0:success,Nonzero:failure.
*@retval 0 success.
*@retval 1 failure when use malloc to allocate memory.
*@note:
*@waring:
*/
int MatrixPinv(const double *a, int m, int n, double tol, double *b);
/**
*@brief Description:use an iterative Newton-Raphson root-finding method.
*@param[in] JointNum Number of joints.
*@param[in] Blist The joint screw axes in the Body frame when the manipulator
* is at the home position, in the format of a matrix with the
* screw axes as the columns.
*@param[in] M The home configuration of the end-effector.
*@param[in] T The desired end-effector configuration Tsd.
*@param[in] thetalist0 An initial guess of joint angles that are close to satisfying Tsd.
*@param[in] eomg A small positive tolerance on the end-effector orientation error.
* The returned joint angles must give an end-effector orientation error less than eomg.
*@param[in] ev A small positive tolerance on the end-effector linear position error. The returned
* joint angles must give an end-effector position error less than ev.
*@param[in] maxiter The maximum number of iterations before the algorithm is terminated.
*@param[out] thetalist Joint angles that achieve T within the specified tolerances.
*@return 0:success,Nonzero:failure
*@retval 0 success.
*@retval 1 The maximum number of iterations reach before the algorithm is terminated.
*@retval 2 failure to allocate memory for Jacobian matrix.
*@note:
*@waring:
*/
int IKinBodyNR(int JointNum, double *Blist, double M[4][4], double T[4][4], double *thetalist0, double eomg, double ev, int maxiter, double *thetalist);
/**
*@brief Description:use an iterative Newton-Raphson root-finding method.
*@param[in] JointNum Number of joints.
*@param[in] Slist The joint screw axes in the space frame when the manipulator
* is at the home position, in the format of a matrix with the
* screw axes as the columns.
*@param[in] M The home configuration of the end-effector.
*@param[in] T The desired end-effector configuration Tsd.
*@param[in] thetalist0 An initial guess of joint angles that are close to satisfying Tsd.
*@param[in] eomg A small positive tolerance on the end-effector orientation error.
* The returned joint angles must give an end-effector orientation error less than eomg.
*@param[in] ev A small positive tolerance on the end-effector linear position error. The returned
* joint angles must give an end-effector position error less than ev.
*@param[in] maxiter The maximum number of iterations before the algorithm is terminated.
*@param[out] thetalist Joint angles that achieve T within the specified tolerances.
*@return 0:success,Nonzero:failure
*@retval 0
*@note:
*@waring:
*/
int IKinSpaceNR(int JointNum, double *Slist, double M[4][4], double T[4][4], double *thetalist0, double eomg, double ev, int maxiter, double *thetalist);
/**
* @brief Description: Algorithm for Computing the roll-pitch-yaw angles(rotate around fix reference X,Y,Z axis).
* @param[in] R Rotation matrix.
* @param[out] roll Angles for rotate around fix reference X axis.
* @param[out] pitch Angles for rotate around fix reference Y axis.
* @param[out] yaw Angles for rotate around fix reference Z axis.
* @return No return value.
* @note:
*@warning:
*/
void RotToRPY(double R[3][3], double *roll, double *pitch, double *yaw);
/**
* @brief Description: Algorithm for Computing the rotation matrix of the roll-pitch-yaw angles.
* @param[in] roll Angles for rotate around fix reference X axis.
* @param[in] pitch Angles for rotate around fix reference Y axis.
* @param[in] yaw Angles for rotate around fix reference Z axis.
* @param[out] R Rotation matrix.
* @return No return value.
* @note:
*@warning:
*/
void RPYToRot(double roll, double pitch, double yaw, double R[3][3]);
/**
* @brief Description: Algorithm for Computing the ZYX Euler Angles according to rotation matrix.
* @param[in] R Rotation matrix.
* @param[out] alpha Angles for rotation around Z axis.
* @param[out] beta Angles for rotation around Y axis.
* @param[out] gamma Angles for rotation around X axis.
* @return No return value.
* @note:
* @waring:
*/
void RotToZYXEulerAngle(double R[3][3], double *alpha, double *beta, double *gamma);
/**
* @brief Description: Algorithm for Computing the rotation matrix of ZYX Euler Angles.
* @param[in] alpha Angles for rotation around Z axis.
* @param[in] beta Angles for rotation around Y axis.
* @param[in] gamma Angles for rotation around X axis.
* @param[out] R Rotation matrix.
* @return No return value.
* @note:
* @waring:
*/
void ZYXEulerAngleToRot(double alpha, double beta, double gamma, double R[3][3]);
/**
* @brief Description: Computes the unit vector of Euler axis and rotation angle corresponding to rotation matrix.
* @param[in] R A rotation matrix.
* @param[out] omghat the unit vector of Euler axis .
* @param[out] theta the rotation angle.
* @return No return value.
* @retval 0
* @note: if theta is zero ,the unit axis is undefined and set it as a zero vector [0;0;0].
* @waring:
*/
void RotToAxisAng(double R[3][3], double omghat[3], double *theta);
/**
* @brief Description: Computes the unit quaternion corresponding to the Euler axis and rotation angle.
* @param[in] omg Unit vector of Euler axis.
* @param[in] theta Rotation angle.
* @param[in] q The unit quaternion
* @return No return value.
* @note:
* @warning:
*/
void AxisAngToQuaternion(double omg[3], double theta, double q[4]);
/**
* @brief Description:Computes the unit quaternion corresponding to a rotation matrix.
* @param[in] q Unit quaternion.
* @param[out] R Rotation matrix.
* @return No return value.
* @note:
* @warning:
*/
void QuaternionToRot(double q[4], double R[3][3]);
/**
* @brief Description: Computes the unit quaternion corresponding to the rotation matrix.
* @param[in] R The rotation matrix.
* @param[out] q The unit quaternion.
* @return No return value.
* @note:
* @warning:
*/
void RotToQuaternion(double R[3][3], double q[4]);
/**
* @brief 单位四元数转换为旋转矩阵。
* @param[in] q 单位四元数[ox,oy,oz,ow].
* @param[out] R 旋转矩阵.
* @return 无返回值.
* @note:
* @warning: (1)此函数不对输入参数做检查，需保证输入为单位四元数。
(2)四元数顺序为[ox,oy,oz,w]。
*/
void q2rot(double q[4], double R[3][3]);
/**
* @brief 旋转矩阵转单位四元数。
* @param[in] R 旋转矩阵
* @param[out] q 单位四元数.[ox,oy,oz,ow].
* @return 无返回值.
* @note:
* @warning: (1)此函数不对输入参数做检查，需保证输入为旋转矩阵。
(2)四元数顺序为[ox,oy,oz,w]。
*/
void rot2q(double R[3][3], double q[4]);
/**
* @brief Description: structure of LinePath interpolation parameters.
*/
typedef struct
{
double p1[3];
double p2[3];
double L;
double t[3];
double pi[3];
double Li;
int InpFlag;
}LineInpParam;
/**
* @brief Description: structure of arc interpolation parameters.
*/
typedef struct
{
double p1[3];
double p2[3];
double p3[3];
double N[3];
double C[3];
double theta;
double R;
}ArcInpParam;
/**
* @brief Description: structure of orientation interpolation parameters.
*/
typedef struct
{
double Rs[3][3];//Start orientation Rotation matrix.
double Re[3][3];//End orientation Rotation matrix.
double R[3][3]; //Matrix rotation from Start orientation to End orientation.
double omg[3]; //unit vector of Euler axis.
double theta; //Total interpolation angle.
double Ri[3][3];//Current orientation rotation Matrix.
double thetai; //Current angle.
int InpFlag; //1:finish initial ,2;interpolating ,3;finish interpolation
}OrientInpParam;
/**
* @brief Description: structure of line Path and Orientation (PO) interpolation parameters.
*/
typedef struct
{
LineInpParam Line;
OrientInpParam Orient;
double Ts[4][4];
double Te[4][4];
double Ti[4][4];
int InpFlag;
}LinePOParam;
/**
* @brief Description:Computes the parameters of interpolation between two orientations.
* @param[in] Rs Start posture Rotation matrix.
* @param[in] Re End posture Rotation matrix.
* @param[in] Param structure of orientation interpolation parameters..
* @return No return value.
* @note:
* @warning:
*/
void InitialOrientInpParam(double Rs[3][3], double Re[3][3], OrientInpParam *Param);
/**
* @brief Description: Computes orientations interpolation.
* @param[in] Param Interpolation parameter structure.
* @param[out] dtheta angle need to rotate from previous orientation to next orientation in next time step.
* @return Ri1 orientations in next time step.
* @retval 0
* @note:
* @warning:
*/
void QuaternionOrientInp(OrientInpParam *Param, double dtheta, double Ri1[3][3]);
/**
* @brief Description:Computes the parameters of line path for interpolation.
* @param[in] p1 Coordinates of start point.
* @param[in] p2 Coordinates of end point.
* @param[out] p Line path parameters structure.
* @return No return value.
* @note:
* @warning:
*/
void InitialLinePathParam(double p1[3], double p2[3], LineInpParam *p);
/**
* @brief Description:Computes the line path interpolation coordinates in each interpolation cycle.
* @param[in] p Line path parameters structure.
* @param[in] dL step length in next interpolation cycle.
* @param[in] pi1 coordinates in next interpolation cycle.
* @return No return value.
* @note:
* @warning:
*/
void LinePathInp(LineInpParam *p, double dL, double pi1[3]);
/**
* @brief Description: Computes the parameters of both line path and orientation for interpolation.
* @param[in] p1 Start coordinates,including x,y,z coordinates and orientation angles roll-pitch-yaw angles.
* @param[in] p2 End coordinates,including x,y,z coordinates and orientation angles roll-pitch-yaw angles.
* @param[out] LPO Parameters of both line path and orientation for interpolation.
* @return No return value.
* @note:
* @warning:
*/
void InitialLinePOInpParam(double p1[6], double p2[6], LinePOParam *LPO);
/**
* @brief Description:Computes the line path interpolation coordinates and orientation in each interpolation cycle.
* @param[out] LPO Line path and orientation parameters structure.
* @param[in] dL Line path interpolation step length.
* @param[out] dtheta angle interpolation step length for Orientation interpolation.
* @return No return value.
* @note:
* @warning:
*/
void LinePOInp(LinePOParam *LPO, double dL, double dtheta, double Ti[4][4]);
#ifdef __cplusplus
}
#endif
#endif//RobotALGORITHMMODULE_H_