Yaw in flat target.msg #189

brunopinto900 · 2021-07-20T15:32:59Z

Problem Description:

This PR adds yaw field to the FlatTarget.msg and any necessary modifications. The circle trajectory now sets yaw to point to the center of the circle at all time.

Additional Context:

This PR is related with #153.

To discuss:

The polynomial trajectory getYaw returns 0

float polynomialtrajectory::getYaw(double time) {
  float yaw = 0.0;
  return yaw;
}

Yaw is a float (Float32) not a double (Float64)

…ssage

…nter of the circle

Jaeyoung-Lim

Thanks for the contribution!

All looks good except some minor points

I think we need to add the case where yaw can be ignored. Could you add this case to the message definition and handle it in the callback?
The polynomial trajectory is now set to yaw=0. I think this is a unnecessary restriction and basically disables the case for velocity yaw
How does velocity_yaw option work with this case?

As far as I am concerned this does not add the reduced attitude controller for yaw control. From the discussion we had in #186 Are you planning to add this?

Jaeyoung-Lim · 2021-07-20T18:36:03Z

.gitignore

@@ -5,3 +5,6 @@

 #Artifacts from vscode
 .vscode/*
+
+#Rviz config
+geometric_controller/launch/config_file.rviz


Why would you want to ignore the rviz config?

Because i am always configuring rviz. So need always to commit. I can remove this condition on .gitignore if you like.

Please remove it from the gitignore. You just need to not commit the rviz changes you did when you commit

Jaeyoung-Lim · 2021-07-20T18:37:28Z

geometric_controller/src/geometric_controller.cpp

@@ -147,6 +147,7 @@ void geometricCtrl::flattargetCallback(const controller_msgs::FlatTarget &msg) {

  targetPos_ = toEigen(msg.position);
  targetVel_ = toEigen(msg.velocity);
+  mavYaw_ = double(msg.yaw.data);


This means that we always need to define yaw, which I think is not what we want. Can you add a case where we can specify ignoring yaw option and handle the case here?

You are right. We can solve this by using a mask (IGNORE_YAW) in the message definition. What do you think?

That sounds great

Jaeyoung-Lim · 2021-07-20T18:38:32Z

trajectory_publisher/src/polynomialtrajectory.cpp

@@ -128,6 +128,10 @@ Eigen::VectorXd polynomialtrajectory::getCoefficients(int dim) {
  }
 }

+float polynomialtrajectory::getYaw(double time) {
+  float yaw = 0.0;


Where does yaw=0.0 come from? This needs to configurable, and probably belongs to the ignore yaw case

brunopinto900 · 2021-07-20T20:08:26Z

I think we need to add the case where yaw can be ignored. Could you add this case to the message definition and handle it in the callback?

What do you mean? Using the already defined mask? something like IGNORE_MASK = some number?

The polynomial trajectory is now set to yaw=0. I think this is a unnecessary restriction and basically disables the case for velocity yaw

How does velocity_yaw option work with this case?

Great question, i still have to think more about this.

As far as I am concerned this does not add the reduced attitude controller for yaw control. From the discussion we had in #186 Are you planning to add this?

Yes, it doesn't add, i am currently working on it.

Jaeyoung-Lim · 2021-07-21T08:21:31Z

What do you mean? Using the already defined mask? something like IGNORE_MASK = some number?

Yes, preferably a bitmask

As far as I am concerned this does not add the reduced attitude controller for yaw control. From the discussion we had in #186 Are you planning to add this?
Yes, it doesn't add, i am currently working on it.

Awesome!

Jaeyoung-Lim · 2021-07-28T07:47:59Z

@brunopinto900 Any updates?

Jaeyoung-Lim · 2021-08-02T05:56:29Z

@brunopinto900 Have you made any progress here?

brunopinto900 · 2021-08-02T08:47:12Z

Hey, not yet. I am going on a vacation and have to finnish work stuff.

Jaeyoung-Lim · 2021-08-16T15:16:45Z

@brunopinto900 Any updates regarding this work?

brunopinto900 · 2021-08-16T15:22:28Z

Hello,

Not yet. Since i am going a vacation, i am finnishing work stuff. By the way i sent you an e-mail.

Jaeyoung-Lim · 2021-08-16T15:32:23Z

@brunopinto900 Since this PR has been open for almost a month, could you provide an approximate ETA on when this will be addressed?

brunopinto900 · 2021-08-16T15:33:56Z

Since its a small thing, i can finnish tomorrow.

Jaeyoung-Lim · 2021-08-16T15:34:40Z

@brunopinto900 Great, thanks

brunopinto900 · 2021-08-16T19:32:31Z

I think we need to add the case where yaw can be ignored. Could you add this case to the message definition and handle it in the callback? This means that we always need to define yaw, which I think is not what we want. Can you add a case where we can specify ignoring yaw option and handle the case here?

Jaeyoung-Lim · 2021-08-16T19:37:01Z

@brunopinto900 You can define a ignore flag as in https://github.com/mavlink/mavros/blob/29db67693a3fecadfec973a6879930645c2ee9d1/mavros_msgs/msg/AttitudeTarget.msg#L9

brunopinto900 · 2021-08-16T19:37:35Z

I think we need to add the case where yaw can be ignored. Could you add this case to the message definition and handle it in the callback?

I added "std::msgs/bool IGNORE_YAW" in the message definition? Much simpler than a bit mask. What do you think? Or want to have as the AttitudeTarget.msg?

The polynomial trajectory is now set to yaw=0. I think this is a unnecessary restriction and basically disables the case for velocity yaw

The polynomial trajectory now sets yaw to the direction of drone [ atan2f(velocity(1),velocity(0)); ]. But already added logic to ignore yaw.

Please remove it from the gitignore. You just need to not commit the rviz changes you did when you commit

Removed rviz from .gitignore

I have two questions. Shouldn't mavYaw_ be targetYaw_? Because its the target orientation and not the current drone yaw

In this function:

Eigen::Vector3d geometricCtrl::controlPosition(const Eigen::Vector3d &target_pos, const Eigen::Vector3d &target_vel,
const Eigen::Vector3d &target_acc) {
/// Compute BodyRate commands using differential flatness
/// Controller based on Faessler 2017
const Eigen::Vector3d a_ref = target_acc;
if (velocity_yaw_) {
mavYaw_ = getVelocityYaw(mavVel_);
}

mavYaw_ is being computed from the current velocity, shouldn't be from target_vel? Although in practice, the drone's velocity will closely match the target velocity.

brunopinto900 · 2021-08-16T20:06:40Z

Using the bit mask to ignore yaw. Its a cleaner interface.

… practice to name private variables

…ecause its the typical approach

brunopinto900 · 2021-09-06T10:17:35Z

Hello @Jaeyoung-Lim Did you the chance to review the pull request?

Jaeyoung-Lim

Sorry for the late review.

I think the yaw ignore implementation looks good on the geometric_controller side, but on the trajectory_publisher it seems like the yaw is assumed to be in the direction of velocity. I don't think this is a generally applicable assumption to make and usually not the behavior we want.

Jaeyoung-Lim · 2021-10-15T09:52:44Z

geometric_controller/src/geometric_controller.cpp

@@ -147,7 +147,11 @@ void geometricCtrl::flattargetCallback(const controller_msgs::FlatTarget &msg) {

  targetPos_ = toEigen(msg.position);
  targetVel_ = toEigen(msg.velocity);
-
+  if( (msg.type_mask & 8) == 8) // Ignore yaw


Suggested change

if( (msg.type_mask & 8) == 8) // Ignore yaw

if( (msg.type_mask == controller_msgs::IGNORE_YAW) // Ignore yaw

Jaeyoung-Lim · 2021-10-15T09:53:26Z

geometric_controller/src/geometric_controller.cpp

@@ -147,7 +147,11 @@ void geometricCtrl::flattargetCallback(const controller_msgs::FlatTarget &msg) {

  targetPos_ = toEigen(msg.position);
  targetVel_ = toEigen(msg.velocity);
-
+  if( (msg.type_mask & 8) == 8) // Ignore yaw
+     mavYaw_ = 0.0;


If you are ignoring yaw, you should not set the yaw to zero. Otherwise this is resetting the yaw setpoint to zero

Jaeyoung-Lim · 2021-10-15T09:55:11Z

trajectory_publisher/src/trajectoryPublisher.cpp

-  msg.acceleration.z = a_targ(2);
+
+  if(ignore_yaw_) {
+    msg.type_mask |= 8;


Suggested change

msg.type_mask |= 8;

msg.type_mask |=controller_msgs::IGNORE_YAW

Jaeyoung-Lim · 2021-10-15T09:56:42Z

trajectory_publisher/src/trajectoryPublisher.cpp

@@ -70,6 +70,7 @@ trajectoryPublisher::trajectoryPublisher(const ros::NodeHandle& nh, const ros::N
  nh_private_.param<int>("trajectory_type", trajectory_type_, 0);
  nh_private_.param<int>("number_of_primitives", num_primitives_, 7);
  nh_private_.param<int>("reference_type", pubreference_type_, 2);
+  nh_private_.param<bool>("ignore_yaw", ignore_yaw_, false);


I think the flag is a bit inaccurate. The trajectory publisher not ignoring yaw but more whether you want to publish yaw.

Jaeyoung-Lim · 2021-10-15T09:57:22Z

trajectory_publisher/src/polynomialtrajectory.cpp

+  Eigen::Vector3d velocity = getVelocity(time); // Yaw is set in the direction of the vehicle
+  float yaw = atan2f(velocity(1),velocity(0));
+  return yaw;
+}


I don't think yaw should be always aligned with the velocity for polynomial trajectories.

Jaeyoung-Lim · 2022-01-24T09:53:53Z

@brunopinto900 I am closing this PR as stale. Please reopen if you are willing to continue 😄

brunopinto900 added 7 commits July 20, 2021 16:17

Edit: Added yaw field [Float32] in the FlatTarget.msg

b2a825f

Edit: Ignore Rviz config file

87d7992

Edit: Added #include <std_msgs/Float32.h> for std_msgs/Float32 yaw me…

9f1a129

…ssage

Edit: Added update on the yaw field in the FlatTargetCallback function

7d1fe3e

New: Added support to include yaw information in the circle trajectory

2ec42fb

New: The circle trajectory now sets the yaw to always point on the ce…

0813481

…nter of the circle

Edit: Added #include <std_msgs/Float32.h> to publish the yaw field

ffbad4c

Jaeyoung-Lim requested changes Jul 20, 2021

View reviewed changes

Edit: Removed rviz from gitignore

70cb74f

brunopinto900 closed this Aug 16, 2021

brunopinto900 reopened this Aug 16, 2021

brunopinto900 added 5 commits August 16, 2021 21:24

Edit: Added mask to ignore yaw

354bcbd

Edit: The controller ignores yaw, if required

84356c2

Edit: p_targ, v_targ and a_targ renamed with a '_' suffix. A standard…

11dcc55

… practice to name private variables

Edit: getYaw function now sets yaw as the direction of the vehicle, b…

a7c6f16

…ecause its the typical approach

Edit: Added logic to handle ignore_yaw cases

822dc51

Jaeyoung-Lim requested changes Oct 15, 2021

View reviewed changes

Jaeyoung-Lim closed this Jan 24, 2022

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Yaw in flat target.msg #189

Yaw in flat target.msg #189

brunopinto900 commented Jul 20, 2021 •

edited

Loading

Jaeyoung-Lim left a comment •

edited

Loading

Jaeyoung-Lim Jul 20, 2021

brunopinto900 Jul 20, 2021

Jaeyoung-Lim Jul 20, 2021

Jaeyoung-Lim Jul 20, 2021

brunopinto900 Jul 20, 2021

Jaeyoung-Lim Jul 20, 2021

Jaeyoung-Lim Jul 20, 2021

brunopinto900 commented Jul 20, 2021

Jaeyoung-Lim commented Jul 21, 2021

Jaeyoung-Lim commented Jul 28, 2021

Jaeyoung-Lim commented Aug 2, 2021

brunopinto900 commented Aug 2, 2021

Jaeyoung-Lim commented Aug 16, 2021

brunopinto900 commented Aug 16, 2021 •

edited

Loading

Jaeyoung-Lim commented Aug 16, 2021 •

edited

Loading

brunopinto900 commented Aug 16, 2021 •

edited

Loading

Jaeyoung-Lim commented Aug 16, 2021

brunopinto900 commented Aug 16, 2021

Jaeyoung-Lim commented Aug 16, 2021

brunopinto900 commented Aug 16, 2021 •

edited

Loading

brunopinto900 commented Aug 16, 2021 •

edited

Loading

brunopinto900 commented Sep 6, 2021

Jaeyoung-Lim left a comment

Jaeyoung-Lim Oct 15, 2021

Jaeyoung-Lim Oct 15, 2021

Jaeyoung-Lim Oct 15, 2021

Jaeyoung-Lim Oct 15, 2021

Jaeyoung-Lim Oct 15, 2021

Jaeyoung-Lim commented Jan 24, 2022

	if( (msg.type_mask & 8) == 8) // Ignore yaw
	if( (msg.type_mask == controller_msgs::IGNORE_YAW) // Ignore yaw

	msg.type_mask \|= 8;
	msg.type_mask \|=controller_msgs::IGNORE_YAW

Yaw in flat target.msg #189

Yaw in flat target.msg #189

Conversation

brunopinto900 commented Jul 20, 2021 • edited Loading

Jaeyoung-Lim left a comment • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

brunopinto900 commented Jul 20, 2021

Jaeyoung-Lim commented Jul 21, 2021

Jaeyoung-Lim commented Jul 28, 2021

Jaeyoung-Lim commented Aug 2, 2021

brunopinto900 commented Aug 2, 2021

Jaeyoung-Lim commented Aug 16, 2021

brunopinto900 commented Aug 16, 2021 • edited Loading

Jaeyoung-Lim commented Aug 16, 2021 • edited Loading

brunopinto900 commented Aug 16, 2021 • edited Loading

Jaeyoung-Lim commented Aug 16, 2021

brunopinto900 commented Aug 16, 2021

Jaeyoung-Lim commented Aug 16, 2021

brunopinto900 commented Aug 16, 2021 • edited Loading

brunopinto900 commented Aug 16, 2021 • edited Loading

brunopinto900 commented Sep 6, 2021

Jaeyoung-Lim left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Jaeyoung-Lim commented Jan 24, 2022

brunopinto900 commented Jul 20, 2021 •

edited

Loading

Jaeyoung-Lim left a comment •

edited

Loading

brunopinto900 commented Aug 16, 2021 •

edited

Loading

Jaeyoung-Lim commented Aug 16, 2021 •

edited

Loading

brunopinto900 commented Aug 16, 2021 •

edited

Loading

brunopinto900 commented Aug 16, 2021 •

edited

Loading

brunopinto900 commented Aug 16, 2021 •

edited

Loading