Quadrotor positive Yaw rotation direction is inverted

[fjanguita]

Environment

• OS Version: Ubuntu 22.04
• Binary Gazebo Harmonic 8.6.0

Description

• Expected behavior: Quadrotor model turns counter-clockwise (positive) when counter-clockwise rotors have higher motor angular velocity than clockwise rotors.
• Actual behavior: Quadrotor turns clockwise (negative) in the described situation.

Steps to reproduce

1. Set 'quadrotor_base.sdf' front-right and back-left rotors to turn counter-clockwise, and front-left and back-right motors to turn clockwise using a MulticopterMotorModel plugin for each rotor. This is 'quadrotor_base.sdf' default configuration.

        <plugin
            filename="ignition-gazebo-multicopter-motor-model-system"
            name="ignition::gazebo::systems::MulticopterMotorModel">
            <robotNamespace>model/{{ namespace }}</robotNamespace>
            <jointName>rotor_0_joint</jointName>
            <linkName>rotor_0</linkName>
            <turningDirection>ccw</turningDirection>
            <timeConstantUp>0.0125</timeConstantUp>
            <timeConstantDown>0.025</timeConstantDown>
            <maxRotVelocity>800.0</maxRotVelocity>
            <motorConstant>8.54858e-06</motorConstant>
            <momentConstant>0.016</momentConstant>
            <commandSubTopic>command/motor_speed</commandSubTopic>
            <motorNumber>0</motorNumber>
            <rotorDragCoefficient>8.06428e-05</rotorDragCoefficient>
            <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
            <motorSpeedPubTopic>motor_speed/0</motorSpeedPubTopic>
            <rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
            <motorType>velocity</motorType>
        </plugin>
        <plugin
            filename="ignition-gazebo-multicopter-motor-model-system"
            name="ignition::gazebo::systems::MulticopterMotorModel">
            <robotNamespace>model/{{ namespace }}</robotNamespace>
            <jointName>rotor_1_joint</jointName>
            <linkName>rotor_1</linkName>
            <turningDirection>ccw</turningDirection>
            <timeConstantUp>0.0125</timeConstantUp>
            <timeConstantDown>0.025</timeConstantDown>
            <maxRotVelocity>800.0</maxRotVelocity>
            <motorConstant>8.54858e-06</motorConstant>
            <momentConstant>0.016</momentConstant>
            <commandSubTopic>command/motor_speed</commandSubTopic>
            <motorNumber>1</motorNumber>
            <rotorDragCoefficient>8.06428e-05</rotorDragCoefficient>
            <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
            <motorSpeedPubTopic>motor_speed/1</motorSpeedPubTopic>
            <rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
            <motorType>velocity</motorType>
        </plugin>
        <plugin
            filename="ignition-gazebo-multicopter-motor-model-system"
            name="ignition::gazebo::systems::MulticopterMotorModel">
            <robotNamespace>model/{{ namespace }}</robotNamespace>
            <jointName>rotor_2_joint</jointName>
            <linkName>rotor_2</linkName>
            <turningDirection>cw</turningDirection>
            <timeConstantUp>0.0125</timeConstantUp>
            <timeConstantDown>0.025</timeConstantDown>
            <maxRotVelocity>800.0</maxRotVelocity>
            <motorConstant>8.54858e-06</motorConstant>
            <momentConstant>0.016</momentConstant>
            <commandSubTopic>command/motor_speed</commandSubTopic>
            <motorNumber>2</motorNumber>
            <rotorDragCoefficient>8.06428e-05</rotorDragCoefficient>
            <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
            <motorSpeedPubTopic>motor_speed/2</motorSpeedPubTopic>
            <rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
            <motorType>velocity</motorType>
        </plugin>

2. Set the MulticopterVelocityControl plugin 'rotorConfiguration' rotors to match the turning direction of the motors (1 is for ccw and -1 is for cw). This is also the default configuration for the plugin in the 'quadrotor_base.sdf' model from Gazebo.

      <plugin
        filename="libMulticopterVelocityControl.so"
        name="gz::sim::systems::MulticopterVelocityControlAlt">
        <robotNamespace>model/{{ namespace }}</robotNamespace>
        <commandSubTopic>cmd_vel</commandSubTopic>
        <enableSubTopic>velocity_controller/enable</enableSubTopic>
        <comLinkName>base_link</comLinkName>
        <velocityGain>3.3 3.3 3.5</velocityGain>
        <attitudeGain>5.0 5.0 0.9</attitudeGain>
        <angularRateGain>1.5 1.5 0.09</angularRateGain>
        <maximumLinearAcceleration>2 2 2</maximumLinearAcceleration>
        <maximumLinearVelocity>5 5 5</maximumLinearVelocity>
        <maximumAngularVelocity>3 3 3</maximumAngularVelocity>
        <linearVelocityNoiseMean>0.0 0.0 0.0</linearVelocityNoiseMean>
        <linearVelocityNoiseStdDev>0.0 0.0 0.0</linearVelocityNoiseStdDev>
        <angularVelocityNoiseMean>0 0 0</angularVelocityNoiseMean>
        <angularVelocityNoiseStdDev>0.0 0.0 0.0</angularVelocityNoiseStdDev>        
        <rotorConfiguration>
          <rotor>
            <jointName>rotor_0_joint</jointName>
            <forceConstant>8.54858e-06</forceConstant>
            <momentConstant>0.016</momentConstant>
            // TURNING DIRECTION
            <direction>1</direction>
          </rotor>
          <rotor>
            <jointName>rotor_1_joint</jointName>
            <forceConstant>8.54858e-06</forceConstant>
            <momentConstant>0.016</momentConstant>
            // TURNING DIRECTION
           <direction>1</direction>
          </rotor>
          <rotor>
            <jointName>rotor_2_joint</jointName>
            <forceConstant>8.54858e-06</forceConstant>
            <momentConstant>0.016</momentConstant>
            // TURNING DIRECTION
            <direction>-1</direction>
          </rotor>
          <rotor>
            <jointName>rotor_3_joint</jointName>
            <forceConstant>8.54858e-06</forceConstant>
            <momentConstant>0.016</momentConstant>
           // TURNING DIRECTION
            <direction>-1</direction>
          </rotor>
        </rotorConfiguration>
      </plugin>

3. Hardcode higher angular velocities to rotors 0 and 1 than to rotors 2 and 3 and the quadrotor will turn clockwise instead of counter-clockwise

void MulticopterVelocityControl::PublishRotorVelocities(
  EntityComponentManager & _ecm,
  const Eigen::VectorXd & vels)
{
 // HARDCODED MOTOR ANGULAR VELOCITIES
  Eigen::Vector4d _vels = Eigen::Vector4d(670.0, 670.0, 660.0, 660.0);

  if (_vels.size() != this->rotorVelocitiesMsg.velocity_size()) {
    this->rotorVelocitiesMsg.mutable_velocity()->Resize(_vels.size(), 0);
  }

  std::cout << "Computed rotor angular velocities: " << std::endl
            << "  [" << _vels[0] << "," << _vels[1] << ","
            << _vels[2] << "," << _vels[3] << "]" << std::endl;

  for (int i = 0; i < this->rotorVelocities.size(); ++i) {
    this->rotorVelocitiesMsg.set_velocity(i, _vels(i));
  }
  // Publish the message by setting the Actuators component on the model entity.
  // This assumes that the MulticopterMotorModel system is attached to this
  // model
  auto actuatorMsgComp =
    _ecm.Component<components::Actuators>(this->model.Entity());

  if (actuatorMsgComp) {
    auto compFunc = [](const msgs::Actuators & _a, const msgs::Actuators & _b)
      {
        return std::equal(
          _a.velocity().begin(), _a.velocity().end(),
          _b.velocity().begin());
      };
    auto state = actuatorMsgComp->SetData(this->rotorVelocitiesMsg, compFunc) ?
      ComponentState::PeriodicChange :
      ComponentState::NoChange;
    _ecm.SetChanged(this->model.Entity(), components::Actuators::typeId, state);
  } else {
    _ecm.CreateComponent(
      this->model.Entity(),
      components::Actuators(this->rotorVelocitiesMsg));
  }
}

Output

Quadrotor has negative angular velocity for the Z axis (should be positive)

twist {
  linear {
    x: -1.8380353695818558e-15
    y: -7.4028219961261316e-16
    z: -0.49010591265427061
  }
  angular {
    x: -5.4013549831217308e-17
    y: 4.54517670116565e-16
    z: -0.21990879322402179
  }
}