利用moveit实现n阶贝塞尔曲线

最新推荐文章于 2024-08-22 08:57:57 发布

山木Hubery

最新推荐文章于 2024-08-22 08:57:57 发布

阅读量349

点赞数

文章标签： c++ vscode 机器人

本文链接：https://blog.csdn.net/Hubery12138/article/details/127796479

版权

坐标求取目标角度

已知点的坐标(x,y,z)，求机械臂的目标角度（ $\theta_{1}$ ， $\theta _{2}$ ， $\theta _{3}$ ），

利用三角公式，可得如下：

$\theta _{1}=\arctan (y/x)$

$\theta _{2}=\arccos ((l_{2}^{2}+x^{2}+y^{2}+(z-l_{1})^{2}-l_{3}^{2})/(2*l_{2}*\sqrt{x^{2}+y^{2}+(z-l_{1})^{2}}))+\arctan ((z-l_{1})/\sqrt{x^{2}+y^{2}})-\pi /2$

$\theta _{3}=\arccos ((l_{2}^{2}+l_{3}^{2}-x^{2}-y^{2}-(z-l_{1})^{2})/2*l_{2}*l_{3})-\pi/2$

利用以上三个公式即可对三个关节的 $\theta$ 角进行求取

void gettheta(double *des,double *destheta){
    double x = des[0] ;
    double y = des[1] ;
    double z = des[2] ;
    destheta[0] = atan2(des[1],des[0]);
    destheta[1] = acos((l2 * l2 + x * x + y * y + ( z - l1) * ( z - l1 ) - l3 *l3)/(2 * l2 * sqrt( x * x +y *y +(z - l1)*(z - l1)))) + atan2((z - l1),sqrt(x * x + y * y)) - pi / 2 ;
    destheta[2] = acos((l2 * l2 + l3 * l3 - x * x -y * y - (z - l1)*(z - l1))/(2 * l2 *l3)) - pi/2 ;
}

贝塞尔曲线规划路径

通过设定的起始点与目标点，我们即可获得贝塞尔曲线的 $P_{0}$ 和 $P_{n}$ ，再通过自行添加(n-1)个沿途经过的点，即可获得n阶的贝塞尔曲线

具体公式如上

贝塞尔公式学习链接：

c贝塞尔曲线（Bezier Curve）原理、公式推导及matlab代码实现_beijing_txr的博客-CSDN博客_贝塞尔曲线matlaba1. 定义贝塞尔曲线(Bezier curve)，又称贝兹曲线或贝济埃曲线，是应用于二维图形应用程序的数学曲线。一般的矢量图形软件通过它来精确画出曲线，贝兹曲线由线段与节点组成，节点是可拖动的支点，线段像可伸缩的皮筋，我们在绘图工具上看到的钢笔工具就是来做这种矢量曲线的。贝塞尔曲线是计算机图形学中相当重要的参数曲线，在一些比较成熟的位图软件中也有贝塞尔曲线工具，如PhotoShop等。贝塞尔曲线的一些特性：使用n个控制点来控制曲线的形状曲线经过起点和终点，但不经过中间点~2.直观理解..https://blog.csdn.net/sinat_35676815/article/details/120884682

贝塞尔曲线实现：

//二项式系数求取
int c(int n , int k){
  if( k == 0 || k == n)
  return 1;
  return c( n -1 , k -1 ) + c( n - 1 , k );
}
//路径规划
void gettrajectory(double des[t][3],double destrajectary[t0+1][3]){
  double destheta[t][3];
  double desposition[t0+1][3];
  //求出路径上所有规划点坐标 
  for(int i = 0 ; i <= t0 ; i++){
    double tc = (double(i) )/ t0 ;
    desposition[i][0] = 0;
    desposition[i][1] = 0;
    desposition[i][2] = 0;
    for(int j = 0 ; j < t ; j ++){
      desposition[i][0] += (c( t - 1 ,j) * pow( 1- tc , t - j - 1) * pow( tc , j) * des[j][0]);
      desposition[i][1] += (c( t - 1 ,j) * pow( 1- tc , t - j - 1) * pow( tc , j) * des[j][1]);
      desposition[i][2] += (c( t - 1 ,j) * pow( 1- tc , t - j - 1) * pow( tc , j) * des[j][2]);
      int a=c(t - 1 ,j);
    }
  }
  //将规划点坐标转换为角度
  for(int i = 0 ; i <= t0 ; i ++ ){
    gettheta(desposition[i],destrajectary[i]);
  }

}

moveit::planning_interface::MoveGroupInterface::Plan的使用

    // 定义存放规划路径相关变量
    moveit::planning_interface::MoveGroupInterface::Plan plan;
    moveit_msgs::RobotTrajectory trajectory;
    trajectory_msgs::JointTrajectoryPoint waypoint;
    //存放关节名
    trajectory.joint_trajectory.joint_names.push_back("joint_1");
    trajectory.joint_trajectory.joint_names.push_back("joint_2");
    trajectory.joint_trajectory.joint_names.push_back("joint_3");
    trajectory.joint_trajectory.joint_names.push_back("joint_4");
    trajectory.joint_trajectory.joint_names.push_back("joint_5");
    trajectory.joint_trajectory.joint_names.push_back("joint_6");
    //存放关节角
    for(int i = 0 ; i  <= t0 ; i++ ){
      for(int j = 0 ; j < 3 ; j++){
        waypoint.positions.push_back(destrajectary[i][j]);
      }
      for(int j = 0 ; j < 3 ; j++){
        waypoint.positions.push_back(0.00);
      }
      //存放运行时间
      waypoint.time_from_start = ros::Duration(0.12 * i);
      trajectory.joint_trajectory.points.push_back(waypoint);
      waypoint.positions.clear();
    }
    //存放路径
    plan.trajectory_ = trajectory ;

如上，我们将算出的贝塞尔曲线的轨迹点通过waypoint.positions.push_back存放至waypoint中，再将期望的单段距离运行时间存放至waypoint.time_from_start中，之后再把整个waypoint通过trajectory.points.push_back存至trajectory中，最后再把trajectory存至plan.trajectory_中，至此就可完成贝塞尔曲线的存放，再利用arm.execute(plan),就可执行该轨迹