Skip to content

Commit fc7dc84

Browse files
Jepear19Jepear19
committed
Add gz_mimic_joint plugin
1 parent a7e51ba commit fc7dc84

5 files changed

Lines changed: 225 additions & 0 deletions

File tree

‎Industrial/gz_mimic_joint/CMakeLists.txt‎

Lines changed: 69 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,69 @@
1+
cmake_minimum_required(VERSION 3.8)
2+
project(gz_mimic_joint)
3+
4+
if(NOT CMAKE_CXX_STANDARD)
5+
set(CMAKE_CXX_STANDARD 17)
6+
endif()
7+
8+
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
9+
add_compile_options(-Wall -Wextra -Wpedantic)
10+
endif()
11+
12+
############################
13+
# Dependencies
14+
############################
15+
16+
find_package(ament_cmake REQUIRED)
17+
find_package(rclcpp REQUIRED)
18+
19+
find_package(gz-sim8 REQUIRED)
20+
find_package(gz-plugin2 REQUIRED)
21+
22+
23+
############################
24+
# Plugin library
25+
############################
26+
27+
add_library(gz_mimic_joint SHARED
28+
src/gz_mimic_joint.cpp
29+
)
30+
31+
target_link_libraries(gz_mimic_joint
32+
gz-sim8::gz-sim8
33+
gz-plugin2::gz-plugin2
34+
)
35+
36+
ament_target_dependencies(gz_mimic_joint
37+
rclcpp
38+
)
39+
40+
############################
41+
# Install plugin
42+
############################
43+
44+
install(
45+
TARGETS gz_mimic_joint
46+
LIBRARY DESTINATION lib
47+
)
48+
49+
############################
50+
# Environment hook (.dsv)
51+
############################
52+
53+
install(
54+
FILES env-hooks/gz_mimic_joint.dsv
55+
DESTINATION share/${PROJECT_NAME}/environment
56+
)
57+
58+
############################
59+
# Export
60+
############################
61+
62+
ament_export_libraries(gz_mimic_joint)
63+
64+
ament_export_dependencies(
65+
gz-sim8
66+
gz-plugin2
67+
)
68+
69+
ament_package()

‎Industrial/gz_mimic_joint/env-hooks/gz_mimic_joint.dsv‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1 @@
1+
prepend-non-duplicate;GZ_SIM_SYSTEM_PLUGIN_PATH;lib

‎Industrial/gz_mimic_joint/package.xml‎

Lines changed: 21 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,21 @@
1+
<?xml version="1.0"?>
2+
<package format="3">
3+
4+
<name>gz_mimic_joint</name>
5+
<version>0.0.1</version>
6+
7+
<description>Gazebo Harmonic Mimic Joint Plugin</description>
8+
9+
<maintainer email="dev@todo.com">dev</maintainer>
10+
<license>Apache-2.0</license>
11+
12+
<buildtool_depend>ament_cmake</buildtool_depend>
13+
14+
<depend>gz-sim8</depend>
15+
<depend>gz-plugin2</depend>
16+
17+
<export>
18+
<build_type>ament_cmake</build_type>
19+
</export>
20+
21+
</package>

‎Industrial/gz_mimic_joint/src/gz_mimic_joint.cpp‎

Lines changed: 125 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,125 @@
1+
#include <gz/sim/System.hh>
2+
#include <gz/sim/EntityComponentManager.hh>
3+
#include <gz/sim/components/Name.hh>
4+
#include <gz/sim/components/JointPosition.hh>
5+
6+
#include <gz/plugin/Register.hh>
7+
8+
#include <iostream>
9+
10+
using namespace gz;
11+
using namespace sim;
12+
13+
namespace gz_mimic_joint
14+
{
15+
16+
class MimicJoint :
17+
public System,
18+
public ISystemConfigure,
19+
public ISystemPreUpdate
20+
{
21+
22+
public:
23+
24+
void Configure(
25+
const Entity &_entity,
26+
const std::shared_ptr<const sdf::Element> &_sdf,
27+
EntityComponentManager &_ecm,
28+
EventManager &) override
29+
{
30+
std::cout << "\n[MimicJoint] Configure() START\n";
31+
32+
worldEntity = _entity;
33+
34+
if (_sdf->HasElement("parent_joint"))
35+
parent_joint_name = _sdf->Get<std::string>("parent_joint");
36+
37+
if (_sdf->HasElement("mimic_joint"))
38+
mimic_joint_name = _sdf->Get<std::string>("mimic_joint");
39+
40+
std::cout << "[MimicJoint] parent_joint: " << parent_joint_name << std::endl;
41+
std::cout << "[MimicJoint] mimic_joint : " << mimic_joint_name << std::endl;
42+
43+
std::cout << "[MimicJoint] Configure() END\n\n";
44+
}
45+
46+
void PreUpdate(
47+
const UpdateInfo &,
48+
EntityComponentManager &_ecm) override
49+
{
50+
// Buscar joints solo una vez
51+
if (parentJoint == kNullEntity)
52+
{
53+
parentJoint = FindJoint(_ecm, parent_joint_name);
54+
55+
if (parentJoint != kNullEntity)
56+
std::cout << "[MimicJoint] Parent joint found\n";
57+
}
58+
59+
if (mimicJoint == kNullEntity)
60+
{
61+
mimicJoint = FindJoint(_ecm, mimic_joint_name);
62+
63+
if (mimicJoint != kNullEntity)
64+
std::cout << "[MimicJoint] Mimic joint found\n";
65+
}
66+
67+
if (parentJoint != kNullEntity && mimicJoint != kNullEntity)
68+
{
69+
auto parentPos = _ecm.Component<components::JointPosition>(parentJoint);
70+
71+
if (parentPos && !parentPos->Data().empty())
72+
{
73+
_ecm.SetComponentData(
74+
mimicJoint,
75+
components::JointPosition({parentPos->Data()[0]})
76+
);
77+
}
78+
}
79+
}
80+
81+
private:
82+
83+
Entity FindJoint(EntityComponentManager &_ecm, const std::string &jointName)
84+
{
85+
Entity result{kNullEntity};
86+
87+
_ecm.Each<components::Name>(
88+
[&](const Entity &_entity, const components::Name *_name)
89+
{
90+
if (_name->Data() == jointName)
91+
{
92+
result = _entity;
93+
return false;
94+
}
95+
return true;
96+
});
97+
98+
if (result == kNullEntity)
99+
{
100+
std::cout << "[MimicJoint] Joint NOT found: " << jointName << std::endl;
101+
}
102+
103+
return result;
104+
}
105+
106+
private:
107+
108+
Entity worldEntity{kNullEntity};
109+
110+
std::string parent_joint_name;
111+
std::string mimic_joint_name;
112+
113+
Entity parentJoint{kNullEntity};
114+
Entity mimicJoint{kNullEntity};
115+
116+
};
117+
118+
}
119+
120+
GZ_ADD_PLUGIN(
121+
gz_mimic_joint::MimicJoint,
122+
gz::sim::System,
123+
gz::sim::ISystemConfigure,
124+
gz::sim::ISystemPreUpdate
125+
)

‎Industrial/ros2_SimRealRobotControl_gz/packages/ur5/ros2srrc_ur5_gazebo/urdf/ur5_robotiq_2f85.urdf.xacro‎

Lines changed: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -88,6 +88,15 @@
8888
</plugin>
8989
</gazebo>
9090

91+
<gazebo>
92+
<plugin filename="libgz_mimic_joint.so" name="gz_mimic_joint">
93+
94+
<parent_joint>robotiq_85_left_knuckle_joint</parent_joint>
95+
<mimic_joint>robotiq_85_right_knuckle_joint</mimic_joint>
96+
97+
</plugin>
98+
</gazebo>
99+
91100
</xacro:unless>
92101

93102
<!-- World: LINK -->

0 commit comments

Comments
 (0)