hello,
I have added a payload under my drone, which I want to control. I placed a imu sensor into the load, as you can see later in my post. Is there any way, how can I figure out, with respect to which frame are being the IMU values measured? I thought It should be with respect to local frame. For my controller I need those data in local frame, so I did the transformation, but it does not seem to be working properly. The code snippet you can see at the end of my post.
model.sdf file:
<link name='pendulum'>
<inertial>
<mass>0.01</mass>
</inertial>
<pose>0 0 -0.185 0 0 0</pose>
<collision name='collision'>
<geometry>
<box>
<size>.01 .01 .4</size>
</box>
</geometry>
</collision>
<visual name='visual'>
<geometry>
<box>
<size>.01 .01 .4</size>
</box>
</geometry>
</visual>
</link>
joint type="ball" name="pendulum_hinge">
<parent>base_link</parent>
<child>pendulum</child>
<pose>0 0 0.24 0 0 0</pose>
</joint>
<link name="load">
<inertial>
<mass>0.25</mass>
<inertia>
<ixx>0.00025</ixx>
<ixy>0.0</ixy>
<ixz>0.0</ixz>
<iyy>0.00025</iyy>
<iyz>0.0</iyz>
<izz>0.00025</izz>
</inertia>
</inertial>
<pose>0 0 -0.35 0 0 0</pose>
<collision name='collision'>
<geometry>
<sphere>
<radius>0.05</radius>
</sphere>
</geometry>
</collision>
<visual name='visual'>
<geometry>
<sphere>
<radius>0.05</radius>
</sphere>
</geometry>
</visual>
<sensor name="imu_sensor" type="imu">
<pose>0 0 0 0 0 0</pose>
<always_on>1</always_on>
<update_rate>30</update_rate>
<visualize>true</visualize>
<topic>load_imu</topic>
</sensor>
</link>
<joint type="fixed" name="fixed_load">
<parent>pendulum</parent>
<child>load</child>
</joint>
coordinate frame transformation code snippet:
void update_load_angular_velocity() {
if (!load_imu_ || !drone_pose_) {
RCLCPP_ERROR(this->get_logger(), "Load IMU or drone pose data not available");
return;
}
tf2::Quaternion load_orientation(
load_imu_->orientation.x,
load_imu_->orientation.y,
load_imu_->orientation.z,
load_imu_->orientation.w
);
load_orientation.normalize();
double load_roll, load_pitch, load_yaw;
tf2::Matrix3x3(load_orientation).getRPY(load_roll, load_pitch, load_yaw);
tf2::Matrix3x3 rotation_matrix;
rotation_matrix.setRPY(0, 0, load_yaw);
tf2::Vector3 local_angular_velocity(
load_imu_->angular_velocity.x,
load_imu_->angular_velocity.y,
load_imu_->angular_velocity.z
);
RCLCPP_INFO(this->get_logger(), "yaw: %.2f",load_yaw);
tf2::Vector3 rotated_load_imu = rotation_matrix * local_angular_velocity;
state_x(3) = rotated_load_imu.x();
state_y(3) = rotated_load_imu.y();
}