ros2阶段的Lissajos曲线

Question

我正在尝试使用差分驱动在ros2阶段使用以下代码绘制Lissajos曲线：但问题是由于某种原因它不遵循该线，有什么帮助吗？ 我给出参数化曲线。

    // TODO: Compute the robot's linear and angular velocities
  int A=5;
  int B=4;
  int a = 3;
  //int b = 3;
  int num_points = static_cast<int>(duration*frequency);
  double dt = duration/num_points;

  for (int i=0; i < num_points; ++i) {
    double t = i*dt;
    double x = A * sin(a * t);
    double y = B*sin(2*a*t);

    double dx = A*cos(a*t);
    double dy = B*2*cos(2*a*t);

    double dx2 = A*sin(a*t);
    double dy2 = B*4*sin(2*a*t);

    double w1 = dy2*dx - dy*dx2;
    double w2 = dx*dx + dy*dy;

    double v = sqrt(dx*dx + dy*dy);
    double w = w1/w2;

    double theta1 = atan2(dy, dx);
    geometry_msgs::msg::Twist pose;
    pose.linear.x = v;
    pose.linear.y = 0;
    pose.linear.z = 0;

    pose.angular.x = 0;
    pose.angular.y = 0;
    pose.angular.z = w;

    cmd_vel_msgs_.push_back(pose);
  }

Answer 1

dx2和dy2可能是错误的， cos 的 d/dx = - sin 你有 + sin

还要检查 num_points，因为转换为 int 可能会降低精度，或者在向上和向下舍入时出现其他问题。

“持续时间”是否在范围中定义？因为它没有在此页面上列出？

检查您的参数方程以及您的三角恒等式是否正确。

祝你好运