Skip to content

Fix visualization plugin #1073

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 3 commits into
base: main
Choose a base branch
from
Open

Fix visualization plugin #1073

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
0874dac
The force visualization plugin is affected
mengchaoheng Aug 11, 2025
71f9940
Use the point cp of the force relative to the base link.
mengchaoheng Aug 20, 2025
1d7560d
fix point of application of forces
mengchaoheng Oct 11, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 4 additions & 0 deletions include/force_visual/force_visual.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -39,6 +39,10 @@ namespace gazebo
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;
};
}

Expand Down
36 changes: 24 additions & 12 deletions models/plane/plane.sdf.jinja
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -192,7 +192,9 @@
</material>
<plugin name="left_elevon_lift_visual" filename="libForceVisual.so">
<topic_name>lift_force/left_elevon</topic_name>
<color>Gazebo/Green</color>
<color>Gazebo/Red</color>
<link_name>base_link</link_name> <!--This is the link that is actually subjected to force, depending on the libLiftDragPlugin settings. -->
<scale>0.1</scale> <!-- Scale of the force visual, Adjust the appropriate value according to the model. -->
</plugin>
</visual>
</link>
Expand Down Expand Up @@ -225,7 +227,9 @@
</material>
<plugin name="right_elevon_lift_visual" filename="libForceVisual.so">
<topic_name>lift_force/right_elevon</topic_name>
<color>Gazebo/Green</color>
<color>Gazebo/Red</color>
<link_name>base_link</link_name> <!--This is the link that is actually subjected to force, depending on the libLiftDragPlugin settings. -->
<scale>0.1</scale> <!-- Scale of the force visual, Adjust the appropriate value according to the model. -->
</plugin>
</visual>
</link>
Expand Down Expand Up @@ -258,7 +262,9 @@
</material>
<plugin name="left_flap_lift_visual" filename="libForceVisual.so">
<topic_name>lift_force/left_flap</topic_name>
<color>Gazebo/Green</color>
<color>Gazebo/Red</color>
<link_name>base_link</link_name> <!--This is the link that is actually subjected to force, depending on the libLiftDragPlugin settings. -->
<scale>0.1</scale> <!-- Scale of the force visual, Adjust the appropriate value according to the model. -->
</plugin>
</visual>
</link>
Expand Down Expand Up @@ -291,7 +297,9 @@
</material>
<plugin name="right_elevon_lift_visual" filename="libForceVisual.so">
<topic_name>lift_force/right_flap</topic_name>
<color>Gazebo/Green</color>
<color>Gazebo/Red</color>
<link_name>base_link</link_name> <!--This is the link that is actually subjected to force, depending on the libLiftDragPlugin settings. -->
<scale>0.1</scale> <!-- Scale of the force visual, Adjust the appropriate value according to the model. -->
</plugin>
</visual>
</link>
Expand Down Expand Up @@ -324,7 +332,9 @@
</material>
<plugin name="elevator_lift_visual" filename="libForceVisual.so">
<topic_name>lift_force/elevator</topic_name>
<color>Gazebo/Green</color>
<color>Gazebo/Red</color>
<link_name>base_link</link_name> <!--This is the link that is actually subjected to force, depending on the libLiftDragPlugin settings. -->
<scale>0.1</scale> <!-- Scale of the force visual, Adjust the appropriate value according to the model. -->
</plugin>
</visual>
</link>
Expand Down Expand Up @@ -357,7 +367,9 @@
</material>
<plugin name="rudder_lift_visual" filename="libForceVisual.so">
<topic_name>lift_force/rudder</topic_name>
<color>Gazebo/Green</color>
<color>Gazebo/Red</color>
<link_name>base_link</link_name> <!--This is the link that is actually subjected to force, depending on the libLiftDragPlugin settings. -->
<scale>0.1</scale> <!-- Scale of the force visual, Adjust the appropriate value according to the model. -->
</plugin>
</visual>
</link>
Expand Down Expand Up @@ -511,7 +523,7 @@
<forward>1 0 0</forward>
<upward>0 0 1</upward>
<link_name>base_link</link_name>
<topic_name>lift_force/left_elevon</topic_name> <!--Uncomment to draw the force-->
<topic_name>lift_force/left_elevon</topic_name> <!--The origin of force will be the value set by the <cp> tag.-->
<control_joint_name>
left_elevon_joint
</control_joint_name>
Expand All @@ -534,7 +546,7 @@
<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-->
<topic_name>lift_force/right_elevon</topic_name> <!--The origin of force will be the value set by the <cp> tag.-->
<control_joint_name>
right_elevon_joint
</control_joint_name>
Expand All @@ -557,7 +569,7 @@
<forward>1 0 0</forward>
<upward>0 0 1</upward>
<link_name>base_link</link_name>
<topic_name>lift_force/left_flap</topic_name> <!--Uncomment to draw the force-->
<topic_name>lift_force/left_flap</topic_name> <!--The origin of force will be the value set by the <cp> tag.-->
<control_joint_name>
left_flap_joint
</control_joint_name>
Expand All @@ -580,7 +592,7 @@
<forward>1 0 0</forward>
<upward>0 0 1</upward>
<link_name>base_link</link_name>
<topic_name>lift_force/right_flap</topic_name> <!--Uncomment to draw the force-->
<topic_name>lift_force/right_flap</topic_name> <!--The origin of force will be the value set by the <cp> tag.-->
<control_joint_name>
right_flap_joint
</control_joint_name>
Expand All @@ -603,7 +615,7 @@
<forward>1 0 0</forward>
<upward>0 0 1</upward>
<link_name>base_link</link_name>
<topic_name>lift_force/elevator</topic_name> <!--Uncomment to draw the force-->
<topic_name>lift_force/elevator</topic_name> <!--The origin of force will be the value set by the <cp> tag.-->
<control_joint_name>
elevator_joint
</control_joint_name>
Expand All @@ -626,7 +638,7 @@
<forward>1 0 0</forward>
<upward>0 1 0</upward>
<link_name>base_link</link_name>
<topic_name>lift_force/rudder</topic_name> <!--Uncomment to draw the force-->
<topic_name>lift_force/rudder</topic_name> <!--The origin of force will be the value set by the <cp> tag.-->
<control_joint_name>
rudder_joint
</control_joint_name>
Expand Down
49 changes: 41 additions & 8 deletions src/force_visual/force_visual.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -32,7 +32,44 @@ void ForceVisualPlugin::Init()
gzdbg << "Subscribing on ~/" << lift_force_topic << std::endl;
}

auto forceVector = this->visual->CreateDynamicLine(rendering::RENDERING_LINE_LIST);
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();
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);

Expand Down Expand Up @@ -63,13 +100,9 @@ void ForceVisualPlugin::OnUpdate(ConstForcePtr& force_msg)
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.
const auto scale = this->visual->Scale();
center = center / scale;
// The force is applied to the cp point of "link_name", and is not affected by the current visual tag.
// Visualize scaling of force length (<scale>)
force *= this->forceScale;

this->UpdateVector(center, force);
}
Expand Down
2 changes: 1 addition & 1 deletion src/liftdrag_plugin/liftdrag_plugin.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -435,7 +435,7 @@ void LiftDragPlugin::OnUpdate()
this->link->AddForceAtRelativePosition(force, this->cp);
this->link->AddTorque(moment);

auto relative_center = this->link->RelativePose().Pos() + this->cp;
auto relative_center = this->cp;

// Publish force and center of pressure for potential visual plugin.
// - dt is used to control the rate at which the force is published
Expand Down