Including Yaw setpoint in the FlatTarget Msg

[mzahana]

Hi @Jaeyoung-Lim

I was looking at the flattargetCallback, and I don't see it accepting yaw setpoint. I know there is another callback just for yaw setpoint yawtargetCallback. I was wondering why not to also add yaw setpoint in the FlatTarget.msg in order to allow all setpoints to be set in one message and published to a single topic with the same timestamp instead of having them separated in two different topics which could possibly be published at different timestamps (even for the same setpoints). Is there a reason for that?

Thanks.

[Jaeyoung-Lim]

@mzahana You are right, this was something that was at the back of my head, but didn't have time to do.

One note is that we need a way to invalidate yaw setpoint in case you want to ignore it, but should be quite straight forward to add

[FaboNo]

@Jaeyoung-Lim Following @mzahana if we consider that the drone is equipped with a camera, it will be great to include the yaw and yaw velocity in the flattargetCallback and thus in the computeBodyRateCmd. What will be the modification in the equations and if we want to implement it, which paper(s) do we have to read? The idea is to couple fastplanner (from hkust) and mavros_controller.

[mzahana]

@FaboNo Have you looked at px4_fast_planner package where I did the required interface between FastPlanner and mavros_controllers?

[mzahana]

The missing piece is the yaw rate which is not implemented in the geometric_controller, but it's doable.

[Jaeyoung-Lim]

@FaboNo Nothing much to add since yaw is already handled in the controller through a different message.

As @mzahana mentioned, he already integrated into the HKUST fast planner in https://github.com/mzahana/px4_fast_planner so might be worth looking into that.

On adding yaw rate, I am not sure if is worth it. Quadrotor systems are differentially flat and yaw rate is usually not part of the flat outputs - meaning that yaw rate commands are unnecessary in most cases. - This might be worth discussing: why do you need to control yaw rates?

[FaboNo]

@mzahana thank you I will have a look
@Jaeyoung-Lim yes you are right, but I was thinking it may be interesting to limit the rate to avoid slam issues (camera blurring or too large rotation in case of Lidar)

[Jaeyoung-Lim]

@FaboNo Right 😄 If you are looking into reducing the yaw gain, it might be worth digging this back up: #35

The intention was to respect vehicle limits, since quadrotors usually suffer from large yaw step inputs but I think this will be also applicable for perception aware control

[FaboNo]

@Jaeyoung-Lim Ah interesting I will dig into it:)

[Jaeyoung-Lim]

@FaboNo Any updates? I would be happy to support you on this

[FaboNo]

@Jaeyoung-Lim

I read the two articles you mentioned to understand how you implemented the geometric controller and I have few questions:

in void geometricCtrl::computeBodyRateCmd(bool ctrl_mode)
a_rd = R_ref * D_.asDiagonal() * R_ref.transpose() * targetVel_; is always equal to 0 since D_ is a null matrix (dx,dy,dz) = (0,0,0)

The geometric controller with mavros/PX4 implement the control laws from Faessler et al. paper but without the rotor drag components because it is easier to integrate with Mavros/px4? Indeed other implementation such as Lee et al requires the knowledge of J (inertia matrix), mass ...

In Eigen::Vector4d geometricCtrl::acc2quaternion(Eigen::Vector3d vector_acc, double yaw) you wrote:

if (velocity_yaw_) 
  		proj_xb_des = targetVel_.normalized();
  	else 
  		proj_xb_des << std::cos(yaw), std::sin(yaw), 0.0;

  	zb_des = vector_acc / vector_acc.norm();
  	yb_des = zb_des.cross(proj_xb_des) / (zb_des.cross(proj_xb_des)).norm();   
  	xb_des = yb_des.cross(zb_des) / ( yb_des.cross(zb_des) ).norm();

But in the Faessler et al. paper :
xb_des = y_c x z_b_des/ (y_c x z_b_des).norm with y_c = (-sin(phi), cos(phi), 0)
I guess it is ok because you took y_b_des instead with x_b_projected.
but in any case xb_des = yb_des.cross(zb_des), i.e. no need to normalized since the two vectors (yb_des and zb_des) are already normalized.

In Eigen::Vector4d geometricCtrl::attcontroller(Eigen::Vector4d &ref_att, Eigen::Vector3d &ref_acc, Eigen::Vector4d &curr_att) I do not see from where the following equation comes from:
ratecmd(3) = std::max(0.0, std::min(1.0, norm_thrust_const_ * ref_acc.dot(zb)))
what is the role of the norm_thrust_const_ here?

On another thread, regarding a real implementation on a drone, you mentioned three steps:
a. You need to make sure the rate controller of the px4 side is tuned properly so that it is capable of tracking the angular rate commands you are providing.
b. You need to make sure the normalized thrust inputs on the geometric_controller side is tuned properly
c. Only when a,b is done you need to tune the K_pos and K_vel terms

Can you tell me what do you mean in point (b) ?

I started to look at the pr #35 and look at this link:
8076712

Does it mean that you implemented the possibility to include a yaw_rate in the controller in another branch?

Thanks again for your support

[Jaeyoung-Lim]

@FaboNo I think the confusion comes from the expectation that this package is a direct implementation of some of the literatures, while it is not. It is definitely based on some of the main ideas mentioned on both of the papers but the details have been adapted to better work with real vehicles running PX4.

in void geometricCtrl::computeBodyRateCmd(bool ctrl_mode)
a_rd = R_ref * D_.asDiagonal() * R_ref.transpose() * targetVel_; is always equal to 0 since D_ is a null matrix (dx,dy,dz) = (0,0,0)

The geometric controller with mavros/PX4 implement the control laws from Faessler et al. paper but without the rotor drag components because it is easier to integrate with Mavros/px4?

As it is clear in the paper, the drag coefficents are usually not a constant . It depends on both the trajectory and the vehicle parameters - revealing that the nature of the drag is indeed not linear as modeled in the paper. Therefore I set the drag coeffients all to zero as default so that people can adjust it to their needs.

Indeed other implementation such as Lee et al requires the knowledge of J (inertia matrix), mass ...

This is because in Lee(2010), the controller is based on force / moments. This is not a good idea on implementing on real systems since you cannot control force/moments reliably. Therefore this package uses bodyrate setpoints + thrust as a setpoint and only take the attitude error function from the paper in the implementation:

mavros_controllers/geometric_controller/src/geometric_controller.cpp

Lines 442 to 466 in b955fd8

	Eigen::Vector4d geometricCtrl::geometric_attcontroller(const Eigen::Vector4d &ref_att, const Eigen::Vector3d &ref_acc,
	Eigen::Vector4d &curr_att) {
	// Geometric attitude controller
	// Attitude error is defined as in Lee, Taeyoung, Melvin Leok, and N. Harris McClamroch. "Geometric tracking control
	// of a quadrotor UAV on SE (3)." 49th IEEE conference on decision and control (CDC). IEEE, 2010.
	// The original paper inputs moment commands, but for offboard control angular rate commands are sent
	Eigen::Vector4d ratecmd;
	Eigen::Matrix3d rotmat; // Rotation matrix of current atttitude
	Eigen::Matrix3d rotmat_d; // Rotation matrix of desired attitude
	Eigen::Vector3d zb;
	Eigen::Vector3d error_att;
	rotmat = quat2RotMatrix(curr_att);
	rotmat_d = quat2RotMatrix(ref_att);
	error_att = 0.5 * matrix_hat_inv(rotmat_d.transpose() * rotmat - rotmat.transpose() * rotmat);
	ratecmd.head(3) = (2.0 / attctrl_tau_) * error_att;
	rotmat = quat2RotMatrix(mavAtt_);
	zb = rotmat.col(2);
	ratecmd(3) =
	std::max(0.0, std::min(1.0, norm_thrust_const_ * ref_acc.dot(zb) + norm_thrust_offset_)); // Calculate thrust
	return ratecmd;
	}

In Eigen::Vector4d geometricCtrl::attcontroller(Eigen::Vector4d &ref_att, Eigen::Vector3d &ref_acc, Eigen::Vector4d &curr_att) I do not see from where the following equation comes from:
ratecmd(3) = std::max(0.0, std::min(1.0, norm_thrust_const_ * ref_acc.dot(zb)))
what is the role of the norm_thrust_const_ here?

You cannot control the thrust of the vehicle in offboard mode of PX4. The thrust commands that we can send are normalized thrust setpoints, which range from [-1, +1] while 0 thrust means zero thrust.
However, all the controllers assume that thrust input T is in force units. Therefore we need to scale the command thrust (N) to normalized thrust inputs, which norm_thrust_const stands for

On another thread, regarding a real implementation on a drone, you mentioned three steps:
a. You need to make sure the rate controller of the px4 side is tuned properly so that it is capable of tracking the angular rate commands you are providing.
b. You need to make sure the normalized thrust inputs on the geometric_controller side is tuned properly
c. Only when a,b is done you need to tune the K_pos and K_vel terms

Can you tell me what do you mean in point (b) ?

It is not possible for PX4 to know how much thrust max thrust commands would produce, since this depends on what motors/voltage/props the vehicle is using for its propulsion. Therefore PX4 considers thrust inputs as normalized thrust inputs. Therefore the norm_thrust_const is different for the real vehicle and the iris vehicle in Gazebo SITL. This is one of the things that are not on a feedback loop and you need to tune reasonably, espeically because none of the control loops in this package has a integrator (on purpose)

Hope this helps

[FaboNo]

@Jaeyoung-Lim great explanations! Yes it helps a lot to clarify the points which were unclear to me... and it confirms that implementing a geometric controller on a real drone is not an easy task!

[Jaeyoung-Lim]

@FaboNo In the end, it is not that complicated. It is just that it is not easy to control force with normal ESCs since there is no feedback therefore it is better to avoid using force / moment based controls in the first place.

[Jaeyoung-Lim]

@mzahana Are you still using this package and have plans to move forward with the yaw setpoints?

[mzahana]

@Jaeyoung-Lim Sorry for getting disconnected from this issue, I am short on time at the time being. I use this package on demand, i.e. depending on the project(s) I work on. At the time being, I am not using it. However, I still believe that the yaw setpoint should be in the flat target callback, and msg. I have had the intention to submit a PR for this, but not soon as I am swamped with other tasks with higher priorities. So, maybe next month, unless someone else is generous and does it earlier.

On a side note, I was also interested to add integral action in the controller to correct for steady-state error as the current controller acts like a PD and won't respond to steady-state errors. I saw one paper from Martin Saska's group did that, and since then I had the intention to do it, but again, short on time! This would be a separate issue/PR though.

Thanks for the follow up.

[958117216]

@mzahana man，can you share the paper from Martin Saska's group ? I will appreciate it.

[mzahana]

@958117216
The paper is called Autonomous Landing on a Moving Vehicle with an Unmanned Aerial Vehicle", see section 9.

Link to the paper

[brunopinto900]

I am adding yaw field on the FlatTarget.msg. But i am having a compilation error:

Errors << geometric_controller:make /home/bruno/catkin_ws/logs/geometric_controller/build.make.004.log
/home/bruno/catkin_ws/src/mavros_controllers/geometric_controller/src/geometric_controller.cpp: In member function ‘void geometricCtrl::flattargetCallback(const FlatTarget&)’:
/home/bruno/catkin_ws/src/mavros_controllers/geometric_controller/src/geometric_controller.cpp:150:35: error: ‘const FlatTarget’ {aka ‘const struct controller_msgs::FlatTarget_<std::allocator >’} has no member named ‘yaw’
150 | std_msgs::Float64 yaw_msg = msg.yaw;
| ^~~

---

This is the message definition:

geometry_msgs/Vector3 position
geometry_msgs/Vector3 velocity
geometry_msgs/Vector3 acceleration
geometry_msgs/Vector3 jerk
geometry_msgs/Vector3 snap
std_msgs/Float64 yaw

This is how i am using it:

void geometricCtrl::flattargetCallback(const controller_msgs::FlatTarget &msg) {
reference_request_last_ = reference_request_now_;

targetPos_prev_ = targetPos_;
targetVel_prev_ = targetVel_;

reference_request_now_ = ros::Time::now();
reference_request_dt_ = (reference_request_now_ - reference_request_last_).toSec();

targetPos_ = toEigen(msg.position);
targetVel_ = toEigen(msg.velocity);
std_msgs::Float64 yaw_msg = msg.yaw;
mavYaw_ = double(yaw_msg.data);

Makes no sense

[Jaeyoung-Lim]

@brunopinto900 Your message definition is not updated. Do it after 'catkin clean'

[brunopinto900]

I tried that before opening the issue xD, didn't work either.

[Jaeyoung-Lim]

@brunopinto900 Then there is something wrong with your workapace path

[brunopinto900]

Had to execute catkin clean on other workspace, that was somehow interfering with the mavros_controller workspace. Everything is working right now.

[brunopinto900]

To test the yaw field in the FlatTarget.msg, i requested the quadrotor to fly a circle while pointing (yaw) to the center of rotation. Here is the results:

RVIZ.mp4

GAZEBO.mp4

I added in the shapeTrajectory.cpp the following code:

float shapetrajectory::getYaw(double time) {

float yaw;

switch (type_) {
case TRAJ_ZERO:

  yaw = 0;
  break;

case TRAJ_CIRCLE:
  yaw = (traj_omega_ * time) - M_PI; // always pointing to the center of the circle
  yaw = atan2f( sin(yaw),cos(yaw) ); // wrap angle around [-pi,pi]
  break;

}

return yaw;
}

What do you think?

[Jaeyoung-Lim]

@brunopinto900 Looks good! but please make a pull request, so that we can discuss the details. I can review the code before we merge it in.