About ForceVisualPlugin

[mengchaoheng]

The reason why the position and scaling of the force are incorrect is that the plugin is located within the visual label of the link tag, for example, in plane.sdf:

<link name="right_elevon">
      <inertial>
        <mass>0.00000001</mass>
        <inertia>
          <ixx>0.000001</ixx>
          <ixy>0.0</ixy>
          <iyy>0.000001</iyy>
          <ixz>0.0</ixz>
          <iyz>0.0</iyz>
          <izz>0.000001</izz>
        </inertia>
        <pose>0 -0.3 0 0.00 0 0.0</pose>
      </inertial>
      <visual name='right_elevon_visual'>
        <pose>0.07 0.0 -0.08 0.00 0 0.0</pose>
        <geometry>
          <mesh>
            <scale>0.1 0.1 0.1</scale>
            <uri>model://plane/meshes/right_aileron.dae</uri>
          </mesh>
        </geometry>
        <material>
          <script>
            <name>Gazebo/Blue</name>
            <uri>__default__</uri>
          </script>
        </material>
        <plugin name="right_elevon_lift_visual" filename="libForceVisual.so">
          <topic_name>lift_force/right_elevon</topic_name>
          <color>Gazebo/Red</color>
        </plugin>
      </visual>
    </link>

The final drawn force's origin is the point specified by the tag in , and scaled by the value specified in . In reality, our force's point of application is at the cp of the link_name coordinate system specified by the libLiftDragPlugin plugin. As follows:

<plugin name="right_wing" filename="libLiftDragPlugin.so">
      <a0>0.05984281113</a0>
      <cla>4.752798721</cla>
      <cda>0.6417112299</cda>
      <cma>0.0</cma>
      <alpha_stall>0.3391428111</alpha_stall>
      <cla_stall>-3.85</cla_stall>
      <cda_stall>-0.9233984055</cda_stall>
      <cma_stall>0</cma_stall>
      <cp>0.0 -0.45 0.05</cp>
      <area>0.6</area>
      <air_density>1.2041</air_density>
      <forward>1 0 0</forward>
      <upward>0 0 1</upward>
      <link_name>base_link</link_name>
      <topic_name>lift_force/right_elevon</topic_name> <!--Uncomment to draw the force-->
      <control_joint_name>
        right_elevon_joint
      </control_joint_name>
      <control_joint_rad_to_cl>-0.3</control_joint_rad_to_cl>
      <robotNamespace></robotNamespace>
      <windSubTopic>world_wind</windSubTopic>
    </plugin>

Therefore, to fix this problem, we need to modify the code to:

1. .sdf file:

<link name="left_elevon">
      <visual name='left_elevon_visual'>
        <plugin name="left_elevon_lift_visual" filename="libForceVisual.so">
          <topic_name>lift_force/left_elevon</topic_name>
          <link_name>base_link</link_name>
          <color>Gazebo/Red</color>
          <scale>0.1</scale>
        </plugin>
      </visual>
    </link>

2. Tools/simulation/gazebo-classic/sitl_gazebo-classic/src/liftdrag_plugin/liftdrag_plugin.cpp:

#include "force_visual/force_visual.h"

using namespace gazebo;

GZ_REGISTER_VISUAL_PLUGIN(ForceVisualPlugin)

ForceVisualPlugin::ForceVisualPlugin() {}

ForceVisualPlugin::~ForceVisualPlugin() {}

void ForceVisualPlugin::Load(rendering::VisualPtr _visual,
                             sdf::ElementPtr _sdf)
{
  GZ_ASSERT(_visual, "ForceVisualPlugin _visual pointer is NULL");
  GZ_ASSERT(_sdf, "ForceVisualPlugin _sdf pointer is NULL");

  this->visual = _visual;
  this->sdf = _sdf;

  gzdbg << "Loading ForceVisual plugin" << std::endl;
}

void ForceVisualPlugin::Init()
{
  this->node.reset(new gazebo::transport::Node());
  this->node->Init();
  this->subs.reset();

  if (this->sdf->HasElement("topic_name")) {
      const auto lift_force_topic = this->sdf->Get<std::string>("topic_name");
      this->subs = this->node->Subscribe("~/" + lift_force_topic, &ForceVisualPlugin::OnUpdate, this);
      gzdbg << "Subscribing on ~/" << lift_force_topic << std::endl;
  }

  if (this->sdf->HasElement("scale")) {
    try { this->forceScale = this->sdf->Get<double>("scale"); }
    catch (...) { this->forceScale = 1.0; }
  }
  // Read link_name (used to attach the line to a specified link's Visual)
  if (this->sdf->HasElement("link_name")) {
    this->frameLinkName = this->sdf->Get<std::string>("link_name");
  }


  this->host.reset();
  {
    auto scene = this->visual->GetScene();
    std::string visName = this->visual->Name(); // 形如 "model::link::visual"
    std::string modelName;
    auto pos = visName.find("::");
    if (pos != std::string::npos) {
      modelName = visName.substr(0, pos);
    }

    if (!this->frameLinkName.empty()) {
      if (!modelName.empty()) {
        auto v = scene->GetVisual(modelName + std::string("::") + this->frameLinkName);
        if (v) this->host = v;
      }
      if (!this->host) {
        auto v = scene->GetVisual(this->frameLinkName);
        if (v) this->host = v;
      }
    }

    if (!this->host) {
      this->host = this->visual;
    }
  }

  auto forceVector = this->host->CreateDynamicLine(rendering::RENDERING_LINE_LIST);

  this->forceVector.reset(forceVector);
  this->forceVector->setVisibilityFlags(GZ_VISIBILITY_GUI);

  if (this->sdf->HasElement("color")) {
      this->forceVector->setMaterial(this->sdf->Get<std::string>("color"));
  } else {
      this->forceVector->setMaterial("Gazebo/Blue");
  }

  if (this->sdf->HasElement("material")) {
    this->forceVector->setMaterial(this->sdf->Get<std::string>("material"));
  }

  for(int k = 0; k < 6; ++k) { // -> needs three lines, so 6 points
    this->forceVector->AddPoint(ignition::math::Vector3d::Zero);
  }

  this->forceVector->Update();
}

void ForceVisualPlugin::OnUpdate(ConstForcePtr& force_msg)
{
  ignition::math::Vector3d force;
  auto force_vector_msg = force_msg->force();
  force.Set(force_vector_msg.x(), force_vector_msg.y(), force_vector_msg.z());

  ignition::math::Vector3d center;
  auto force_center_msg = force_msg->center();
  center.Set(force_center_msg.x(), force_center_msg.y(), force_center_msg.z());

  // Note that if the visual is scaled, the center needs to be scaled, too,
  // such that the force appears on the visual surface control and not on
  // the actual joint (which will look weird).
  // I am not sure what happens if both the visual and the joint are scaled,
  // but I don't see a reason for doing that, like, ever.
  // No scaling correction. Because the force is applied to the "link_name" cp point, and is not affected by the current visual tag.
  // Visualize scaling of force length (<scale>)
  force *= this->forceScale;

  this->UpdateVector(center, force);
}

void ForceVisualPlugin::UpdateVector(const ignition::math::Vector3d& center, const ignition::math::Vector3d& force)
{
  const float arrow_scale = 0.1;

  ignition::math::Vector3d begin = center;
  ignition::math::Vector3d end = center + force;

  this->forceVector->SetPoint(0, begin);
  this->forceVector->SetPoint(1, end);
  this->forceVector->SetPoint(2, end);
  this->forceVector->SetPoint(3, end - arrow_scale * ignition::math::Matrix3d(1, 0, 0, 0, 0.9848, -0.1736, 0,  0.1736, 0.9848) * (end - begin));
  this->forceVector->SetPoint(4, end);
  this->forceVector->SetPoint(5, end - arrow_scale * ignition::math::Matrix3d(1, 0, 0, 0, 0.9848,  0.1736, 0, -0.1736, 0.9848) * (end - begin));
}

3. Tools/simulation/gazebo-classic/sitl_gazebo-classic/include/force_visual/force_visual.h

#ifndef _GAZEBO_FORCE_VISUAL_PLUGIN_HH_
#define _GAZEBO_FORCE_VISUAL_PLUGIN_HH_

#include <gazebo/common/Plugin.hh>
#include <gazebo/rendering/rendering.hh>
#include <gazebo/rendering/Visual.hh>
#include <gazebo/transport/TransportTypes.hh>

#include "Force.pb.h"

namespace gazebo
{
  typedef const boost::shared_ptr<const physics_msgs::msgs::Force> ConstForcePtr;

  /// \brief A visual plugin that draws a force published on a topic
  class GAZEBO_VISIBLE ForceVisualPlugin : public VisualPlugin
  {
    /// \brief Constructor.
    public: ForceVisualPlugin();

    /// \brief Destructor.
    public: ~ForceVisualPlugin();

    // Documentation Inherited.
    public: virtual void Load(rendering::VisualPtr _visual, sdf::ElementPtr _sdf);

    // Documentation Inherited.
    public: virtual void Init();

    /// \brief Pointer to visual containing plugin.
    protected: rendering::VisualPtr visual;

    /// \brief Pointer to element containing sdf.
    protected: sdf::ElementPtr sdf;

    private: void OnUpdate(ConstForcePtr& force_msg);
    private: void UpdateVector(const ignition::math::Vector3d& center, const ignition::math::Vector3d& force);

    private: transport::SubscriberPtr subs;
    private: transport::NodePtr node;
    private: rendering::DynamicLinesPtr forceVector;

    private: std::string frameLinkName;
    private: rendering::VisualPtr host;
    private: double forceScale = 1.0;
  };
}

#endif

[mengchaoheng]

@Jaeyoung-Lim Here answers the following question:

PX4-SITL_gazebo-classic/src/force_visual/force_visual.cpp

Lines 66 to 70 in 6697ab1

	// Note that if the visual is scaled, the center needs to be scaled, too,
	// such that the force appears on the visual surface control and not on
	// the actual joint (which will look weird).
	// I am not sure what happens if both the visual and the joint are scaled,
	// but I don't see a reason for doing that, like, ever.

[Jaeyoung-Lim]

@mengchaoheng Please consider making a PR

[mengchaoheng]

@Jaeyoung-Lim #1073