Ребёнок вращается вокруг родительских осей вместо своих
Итак, у меня есть проект 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"));
}
Решение
Я не уверен, какую библиотеку вы используете, поэтому постараюсь дать несколько общих советов по этому вопросу.
Глобальные против локальных вращений — это просто вопрос порядка умножения матриц. Позволять R быть окончательной матрицей вращения. Когда вы умножаете матрицы X, Y и Z, используя следующий порядок R=X*Y*Zтогда это даст вам Глобальные вращения, в то время как R=Z*Y*X дам тебе Местные вращения.
Проблема с вышесказанным заключается в том, что он ограничивает вашу свободу локальных вращений конкретным порядком Z-Y-X. Например, если вы хотите повернуть, сначала на ось х, то на ось Y, а затем на Ось Z, выше, будет работать нормально. Все остальное не даст вам желаемых результатов. Вам придется изменить порядок умножения матриц.
Если вы хотите вращаться вокруг оси, скажем, ось ординат, который является локальным для вашего объекта, то вам нужно знать, где находится эта ось. Вам необходимо сохранять ссылку на текущие оси после каждого преобразования, а затем использовать Матрица вращения от оси и угла вращаться о вашем текущем местном ось ординат.
/* 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);
И все повороты будут локальными для вашего объекта.
Другие решения
Других решений пока нет …