Goal yaw

[giorgiovaccarino]

Hi, I want to share a video about a Gazebo+PX4 SITL simulation where the drone avoids a part of obstacles and impacts one: https://youtu.be/kiCl-1SBb7Y

The behavior is different from time to time but it seems to me that the yaw settings are never correct. It seems to me that there is too much rotations. Probably I do something wrong in the transformation.

My controller posts a message to /state with the odometry (which comes from PX4 via /mavros/local_position/odom). The transformation between base_link and world is done by PX4 (the EKF2 estimator).

uav_state = State()
uav_state.header.frame_id = "world"
uav_state.pos.x = self.current_odom.pose.pose.position.x
uav_state.pos.y = self.current_odom.pose.pose.position.y
uav_state.pos.z = self.current_odom.pose.pose.position.z
uav_state.quat.x = self.current_odom.pose.pose.orientation.x
uav_state.quat.y = self.current_odom.pose.pose.orientation.y
uav_state.quat.z = self.current_odom.pose.pose.orientation.z
uav_state.quat.w = self.current_odom.pose.pose.orientation.w
uav_state.vel.x = self.current_odom.twist.twist.linear.x
uav_state.vel.y = self.current_odom.twist.twist.linear.y
uav_state.vel.z = self.current_odom.twist.twist.linear.z
self.uav_state_pub.publish(uav_state)

and the controller receives the message /goalx from faster (type Goal) and publishes a PoseStamped message to: /mavros/setpoint_position/local:

self.desired_position.pose.position.x = data.p.x
self.desired_position.pose.position.y = data.p.y
self.desired_position.pose.position.z = data.p.z
q = quaternion_from_euler(0, 0, data.yaw)
self.desired_position.pose.orientation.x = q[0]
self.desired_position.pose.orientation.y = q[1]
self.desired_position.pose.orientation.z = q[2]
self.desired_position.pose.orientation.w = q[3]

... publish...

[giorgiovaccarino]

Modifying the following parameters I obtained a trajectory with very small rotations: faster.yaml w_max: 0.2 #4.0 # [rd/s] Maximum angular velocity. ~4.0 for Hardware alpha_filter_dyaw: 0.99 #0 #was 0.92 #[] Filter parameter for dyaw, \in [0,1]. Higher--> More aggressive filtering v_max: 1 # 5.0 #[m/s] 1.4 a_max: 1 #5.0 #[m/s2] 1.4 j_max: 35 #8.0 #[m/s3] 35

Does anyone have any further advice?

[jtorde]

Hi @giorgiovaccarino,

When you do this:

self.desired_position.pose.position.x = data.p.x
self.desired_position.pose.position.y = data.p.y
self.desired_position.pose.position.z = data.p.z
q = quaternion_from_euler(0, 0, data.yaw)
self.desired_position.pose.orientation.x = q[0]
self.desired_position.pose.orientation.y = q[1]
self.desired_position.pose.orientation.z = q[2]
self.desired_position.pose.orientation.w = q[3]

I think you are not taking into account that the acceleration of the goal message is determining two degrees of freedom of the orientation of the drone. The last degree of freedom is determined by yaw.

You can find an example of how to map acceleration and yaw to a quaternion in this file, which is the one used by FASTER in the simulation. More info is available in Table 3 of this paper.

Hope this helps!

[jtorde]

Closing this for now, feel free to reopen it if you are still facing this issue