Ребенок вращается вокруг родительских осей, а не своих - PullRequest
Новая
       10

Ребенок вращается вокруг родительских осей, а не своих

0 голосов
/ 15 октября 2018

Итак, у меня есть проект C ++, который имитирует автомобиль.Моя программа работает в 2D (только в плоскости XY), она передается с данными одометрии, предоставленными rosbag, давая ему его положение в плоскости XY в зависимости от происхождения мира.Все хорошо в 2D.

Но когда я в 3D, это означает, что я могу вращать свою машину вокруг нескольких осей, а не только по оси Z.Я понял, что моя машина вращается вокруг осей "мировых" осей, когда я хотел бы, чтобы они поворачивали оси моего транспортного средства.

Чтобы проверить это, я сделал фиктивный код, где находится мое транспортное средствопредположим, что нужно сделать вращение на 90 градусов по оси Z, а затем на 90 градусов по оси Y.

Можно ли сделать так, чтобы мое транспортное средство вращалось вокруг его собственных осей?Как бы математика была?

Далее два поворота выполняются вокруг осей мира.

Вот фрагмент моего кода для иллюстрации: 

void NodeImpl::callback_odometry(motion_common_msg::Odometry input_msg)
{
  //Getting Direction3D

//---
    frame_id = input_msg.header.frame_id;
    stamp = input_msg.header.stamp;

    transform_from_odometry.setOrigin(new_position_3D);

    tf::Quaternion q;
    q.setRPY(0.0, 0.0, 0.0);
    transform_from_odometry.setRotation(q);

    if(input_msg.pos_x >= 5.0)
    {
        double cos90 = 0;
        double sin90 = 1;

        tf::Matrix3x3 Rz90;
        tf::Matrix3x3 Ry90;

        Rz90.setValue(cos90, - sin90, 0, sin90, cos90, 0, 0, 0, 1);
        Ry90.setValue(cos90, 0, sin90, 0, 1, 0, -sin90, 0, cos90);

        tf::Quaternion qz90;
        tf::Quaternion qy90;

        Rz90.getRotation(qz90);
        Ry90.getRotation(qy90);

        qz90.normalize();
        qy90.normalize();

        tf::Transform tf_new_rot;

        tf_new_rot.setRotation(qz90);

        transform_from_odometry.setRotation(qy90);

        transform_from_odometry.mult (transform_from_odometry, tf_new_rot);
    }

    broadcast();
}

void NodeImpl::broadcast()
{
     static tf::TransformBroadcaster br;
     br.sendTransform(tf::StampedTransform(transform_from_odometry, stamp, frame_id, "ego_vehicle_rear_axis"));
}

1 Ответ

0 голосов
/ 15 октября 2018

Я не уверен, какую библиотеку вы используете, поэтому постараюсь дать несколько общих советов по этому вопросу.

Глобальное и локальное вращение - это вопрос порядка умножения матриц.Пусть R будет конечной матрицей вращения.Когда вы умножаете матрицы X, Y и Z, используя следующий порядок R=X*Y*Z, это даст вам Глобальные вращения , тогда как R=Z*Y*X даст вам Локальные вращения .

Проблема с вышесказанным заключается в том, что оно ограничивает вашу свободу локальных вращений конкретным порядком ZYX.Например, если вы хотите повернуть, сначала по по оси X, затем по по оси Y, а затем по по оси Z,выше будет работать нормально.Все остальное не даст вам желаемых результатов.Вам придется изменить порядок умножения матриц.

Если вы хотите вращаться вокруг оси, скажем, ось y , которая является локальной для вашего объекта, тогда вы 'Мне нужно знать, где находится эта ось.Вам необходимо сохранять привязку к текущим осям после каждого преобразования, а затем использовать матрицу вращения от оси и угла , чтобы вращаться вокруг текущей локальной оси y . 

/* from the wiki link above */    
Mat3 Axis_Matrix(float angle_, const Vec3 &axis_)
    {
        return Mat3{ cos(angle_)+pow(axis_.x,2)*(1.0-cos(angle_))              , (axis_.x*axis_.y*(1.0-cos(angle_)))-(axis_.z*sin(angle_)) , (axis_.x*axis_.z*(1.0-cos(angle_)))+(axis_.y*sin(angle_)),
                     (axis_.y*axis_.x*(1.0-cos(angle_)))+(axis_.z*sin(angle_)) , cos(angle_)+pow(axis_.y,2)*(1.0 - cos(angle_))            , (axis_.y*axis_.z*(1.0-cos(angle_)))-(axis_.x*sin(angle_)),
                     (axis_.z*axis_.x*(1.0-cos(angle_)))-(axis_.y*sin(angle_)) , (axis_.z*axis_.y*(1.0-cos(angle_)))+(axis_.x*sin(angle_)) , cos(angle_)+pow(axis_.z,2)*(1.0 - cos(angle_)) };
    }

Вы можете создать свою собственную структуру, которая делает все это: 

struct CoordinateSystem
{
    Vec3 current_x_axis;
    Vec3 current_y_axis;
    Vec3 current_z_axis;

    Mat3 local;

    void setRotationX(float angle_)
    {
        local *= Axis_Matrix(angle_, current_x_axis);
        update();
    }

    void setRotationY(float angle_)
    {
        local *= Axis_Matrix(angle_, current_y_axis);
        update();
    }

    void setRotationZ(float angle_)
    {
        local *= Axis_Matrix(angle_, current_z_axis);
        update();
    }

    void update()
    {
        current_x_axis = {-1.f, 0.f, 0.f} * local;
        current_y_axis = { 0.f, 1.f, 0.f} * local;
        current_z_axis = { 0.f, 0.f,-1.f} * local;
    }
};

Тогда вы можете просто сказать: 

setRotationX(45);
setRotationZ(10);
setRotationY(62);
setRotationX(34);
setRotationY(81);

И все повороты будут локальными для вашегообъект.

...