Skip to content

Commit 4b3c44b

Browse files
livanov93livanov93
committed
Introduce MimicJointSystem plugin.
1 parent 01e6f68 commit 4b3c44b

File tree

5 files changed

+333
-2
lines changed

5 files changed

+333
-2
lines changed

‎ign_ros2_control/CMakeLists.txt‎

Lines changed: 30 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -44,10 +44,40 @@ elseif("$ENV{IGNITION_VERSION}" STREQUAL "fortress")
4444
set(IGN_GAZEBO_VER ${ignition-gazebo6_VERSION_MAJOR})
4545
message(STATUS "Compiling against Ignition Fortress")
4646

47+
#################################################
48+
find_package(ignition-cmake2 REQUIRED)
49+
find_package(ignition-plugin1 REQUIRED COMPONENTS register)
50+
set(IGN_PLUGIN_VER ${ignition-plugin1_VERSION_MAJOR})
51+
add_library(mimic-joint-system SHARED src/MimicJointSystem.cc)
52+
target_link_libraries(mimic-joint-system
53+
PRIVATE ignition-plugin${IGN_PLUGIN_VER}::ignition-plugin${IGN_PLUGIN_VER}
54+
PRIVATE ignition-gazebo6::ignition-gazebo6)
55+
# target_link_libraries(mimic-joint-system
56+
# ignition-gazebo${IGN_GAZEBO_VER}::core
57+
# ignition-plugin${IGN_PLUGIN_VER}::register
58+
# )
59+
install(TARGETS mimic-joint-system
60+
DESTINATION lib)
61+
4762
else()
4863
find_package(ignition-gazebo6 REQUIRED)
4964
set(IGN_GAZEBO_VER ${ignition-gazebo6_VERSION_MAJOR})
5065
message(STATUS "Compiling against Ignition Fortress")
66+
67+
#################################################
68+
find_package(ignition-cmake2 REQUIRED)
69+
find_package(ignition-plugin1 REQUIRED COMPONENTS register)
70+
set(IGN_PLUGIN_VER ${ignition-plugin1_VERSION_MAJOR})
71+
add_library(mimic-joint-system SHARED src/MimicJointSystem.cc)
72+
target_link_libraries(mimic-joint-system
73+
PRIVATE ignition-plugin${IGN_PLUGIN_VER}::ignition-plugin${IGN_PLUGIN_VER}
74+
PRIVATE ignition-gazebo6::ignition-gazebo6)
75+
# target_link_libraries(mimic-joint-system
76+
# ignition-gazebo${IGN_GAZEBO_VER}::core
77+
# ignition-plugin${IGN_PLUGIN_VER}::register
78+
# )
79+
install(TARGETS mimic-joint-system
80+
DESTINATION lib)
5181
endif()
5282

5383
find_package(ignition-plugin1 REQUIRED)

‎ign_ros2_control/include/ign_ros2_control/MimicJointSystem.hh‎

Lines changed: 82 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,82 @@
1+
/*
2+
* Copyright (C) 2018 Open Source Robotics Foundation
3+
*
4+
* Licensed under the Apache License, Version 2.0 (the "License");
5+
* you may not use this file except in compliance with the License.
6+
* You may obtain a copy of the License at
7+
*
8+
* http://www.apache.org/licenses/LICENSE-2.0
9+
*
10+
* Unless required by applicable law or agreed to in writing, software
11+
* distributed under the License is distributed on an "AS IS" BASIS,
12+
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13+
* See the License for the specific language governing permissions and
14+
* limitations under the License.
15+
*
16+
*/
17+
#ifndef SYSTEM_PLUGIN_SAMPLESYSTEM_HH_
18+
#define SYSTEM_PLUGIN_SAMPLESYSTEM_HH_
19+
20+
//! [header]
21+
#include <memory>
22+
23+
#include <ignition/gazebo/System.hh>
24+
25+
//! <plugin filename="mimic-joint-system" name="ign_ros2_control::MimicJointSystem">
26+
//! </plugin>
27+
namespace ign_ros2_control
28+
{
29+
class MimicJointSystemPrivate;
30+
31+
class MimicJointSystem:
32+
// This class is a system.
33+
public ignition::gazebo::System,
34+
public ignition::gazebo::ISystemConfigure,
35+
// This class also implements the ISystemPreUpdate, ISystemUpdate,
36+
// and ISystemPostUpdate interfaces.
37+
public ignition::gazebo::ISystemPreUpdate,
38+
public ignition::gazebo::ISystemUpdate,
39+
public ignition::gazebo::ISystemPostUpdate
40+
{
41+
public: MimicJointSystem();
42+
43+
public: ~MimicJointSystem() override;
44+
45+
// Documentation inherited
46+
public: void Configure(
47+
const ignition::gazebo::Entity & _entity,
48+
const std::shared_ptr<const sdf::Element> & _sdf,
49+
ignition::gazebo::EntityComponentManager & _ecm,
50+
ignition::gazebo::EventManager & _eventMgr) override;
51+
52+
public: void PreUpdate(const ignition::gazebo::UpdateInfo &_info,
53+
ignition::gazebo::EntityComponentManager &_ecm) override;
54+
55+
public: void Update(const ignition::gazebo::UpdateInfo &_info,
56+
ignition::gazebo::EntityComponentManager &_ecm) override;
57+
58+
public: void PostUpdate(const ignition::gazebo::UpdateInfo &_info,
59+
const ignition::gazebo::EntityComponentManager &_ecm) override;
60+
61+
private:
62+
// /// \brief Entity ID for sensor within Gazebo.
63+
//
64+
// ignition::gazebo::Entity entity_;
65+
//
66+
// /// \brief ECM pointer
67+
// ignition::gazebo::EntityComponentManager * ecm{nullptr};
68+
69+
// double multiplier_ = 1.0;
70+
// double offset = 0.0;
71+
//
72+
// std::string joint_name_;
73+
// std::string mimic_joint_name_;
74+
75+
/// \brief Private data pointer
76+
private: std::unique_ptr<MimicJointSystemPrivate> dataPtr;
77+
78+
};
79+
}
80+
//! [header]
81+
82+
#endif

‎ign_ros2_control/picknik_system.sdf‎

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,7 @@
1+
<?xml version="1.0" ?>
2+
<sdf version="1.6">
3+
<world name="default">
4+
<plugin filename="mimic-joint-system" name="ign_ros2_control::MimicJointSystem">
5+
</plugin>>
6+
</world>
7+
</sdf>

‎ign_ros2_control/src/MimicJointSystem.cc‎

Lines changed: 205 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,205 @@
1+
#include "ign_ros2_control/MimicJointSystem.hh"
2+
3+
#include "ignition/gazebo/Model.hh"
4+
#include "ignition/gazebo/components/JointForceCmd.hh"
5+
#include "ignition/gazebo/components/JointVelocityCmd.hh"
6+
#include "ignition/gazebo/components/JointPosition.hh"
7+
8+
#include <ignition/math/PID.hh>
9+
10+
11+
//! [registerMimicJointSystem]
12+
#include <ignition/plugin/Register.hh>
13+
14+
IGNITION_ADD_PLUGIN(
15+
ign_ros2_control::MimicJointSystem,
16+
ignition::gazebo::System,
17+
ign_ros2_control::MimicJointSystem::ISystemConfigure,
18+
ign_ros2_control::MimicJointSystem::ISystemPreUpdate,
19+
ign_ros2_control::MimicJointSystem::ISystemUpdate,
20+
ign_ros2_control::MimicJointSystem::ISystemPostUpdate)
21+
//! [registerMimicJointSystem]
22+
23+
using namespace ign_ros2_control;
24+
25+
class ign_ros2_control::MimicJointSystemPrivate{
26+
27+
/// \brief Joint Entity
28+
public: ignition::gazebo::Entity jointEntity{ignition::gazebo::kNullEntity};
29+
30+
/// \brief ECM pointer
31+
ignition::gazebo::EntityComponentManager * ecm{nullptr};
32+
33+
/// \brief Joint name
34+
public: std::string jointName;
35+
36+
public: std::string mimicJointName;
37+
38+
/// \brief Commanded joint position
39+
public: double jointPosCmd{0.0};
40+
41+
public: double multiplier{1.0};
42+
43+
public: double offset{0.0};
44+
45+
/// \brief Model interface
46+
public: ignition::gazebo::Model model{ignition::gazebo::kNullEntity};
47+
48+
/// \brief Position PID controller.
49+
public: ignition::math::PID posPid;
50+
51+
/// \brief Joint index to be used.
52+
public: unsigned int jointIndex = 0u;
53+
54+
/// \brief Operation modes
55+
enum OperationMode
56+
{
57+
/// \brief Use PID to achieve positional control
58+
PID,
59+
/// \brief Bypass PID completely. This means the joint will move to that
60+
/// position bypassing the physics engine.
61+
ABS
62+
};
63+
64+
/// \brief Joint position mode
65+
public: OperationMode mode = OperationMode::PID;
66+
67+
};
68+
69+
MimicJointSystem::MimicJointSystem(): dataPtr(std::make_unique<ign_ros2_control::MimicJointSystemPrivate>())
70+
{
71+
}
72+
73+
MimicJointSystem::~MimicJointSystem()
74+
{
75+
this->dataPtr.reset();
76+
}
77+
78+
void MimicJointSystem::Configure(const ignition::gazebo::Entity &_entity,
79+
const std::shared_ptr<const sdf::Element> &_sdf,
80+
ignition::gazebo::EntityComponentManager &_ecm,
81+
ignition::gazebo::EventManager &_eventMgr) {
82+
83+
// Make sure the controller is attached to a valid model
84+
const auto model = ignition::gazebo::Model(_entity);
85+
if (!model.Valid(_ecm)) {
86+
87+
ignerr << "MimicJointSystem Failed to initialize because ["<<model.Name(_ecm) <<"] (Entity="<<_entity <<")] is not a model.. ";
88+
return;
89+
}
90+
91+
this->dataPtr->ecm = &_ecm;
92+
93+
// Get params from SDF
94+
this->dataPtr->jointName = _sdf->Get<std::string>("joint_name");
95+
96+
if (this->dataPtr->jointName.empty())
97+
{
98+
ignerr << "MimicJointSystem found an empty joint_name parameter. "
99+
<< "Failed to initialize.";
100+
return;
101+
}
102+
103+
this->dataPtr->mimicJointName = _sdf->Get<std::string>("mimic_joint_name");
104+
if (this->dataPtr->mimicJointName.empty())
105+
{
106+
ignerr << "MimicJointSystem found an empty mimic_joint_name parameter. "
107+
<< "Failed to initialize.";
108+
return;
109+
}
110+
111+
if (_sdf->HasElement("multiplier"))
112+
{
113+
this->dataPtr->multiplier = _sdf->Get<unsigned int>("multiplier");
114+
}
115+
116+
if (_sdf->HasElement("offset"))
117+
{
118+
this->dataPtr->offset = _sdf->Get<unsigned int>("offset");
119+
}
120+
121+
if (_sdf->HasElement("joint_index"))
122+
{
123+
this->dataPtr->jointIndex = _sdf->Get<unsigned int>("joint_index");
124+
}
125+
126+
// PID parameters
127+
double p = 1;
128+
double i = 0.1;
129+
double d = 0.01;
130+
double iMax = 1;
131+
double iMin = -1;
132+
double cmdMax = 1000;
133+
double cmdMin = -1000;
134+
double cmdOffset = 0;
135+
136+
if (_sdf->HasElement("p_gain"))
137+
{
138+
p = _sdf->Get<double>("p_gain");
139+
}
140+
if (_sdf->HasElement("i_gain"))
141+
{
142+
i = _sdf->Get<double>("i_gain");
143+
}
144+
if (_sdf->HasElement("d_gain"))
145+
{
146+
d = _sdf->Get<double>("d_gain");
147+
}
148+
if (_sdf->HasElement("i_max"))
149+
{
150+
iMax = _sdf->Get<double>("i_max");
151+
}
152+
if (_sdf->HasElement("i_min"))
153+
{
154+
iMin = _sdf->Get<double>("i_min");
155+
}
156+
if (_sdf->HasElement("cmd_max"))
157+
{
158+
cmdMax = _sdf->Get<double>("cmd_max");
159+
}
160+
if (_sdf->HasElement("cmd_min"))
161+
{
162+
cmdMin = _sdf->Get<double>("cmd_min");
163+
}
164+
if (_sdf->HasElement("cmd_offset"))
165+
{
166+
cmdOffset = _sdf->Get<double>("cmd_offset");
167+
}
168+
if (_sdf->HasElement("use_velocity_commands"))
169+
{
170+
auto useVelocityCommands = _sdf->Get<bool>("use_velocity_commands");
171+
if (useVelocityCommands)
172+
{
173+
this->dataPtr->mode =
174+
MimicJointSystemPrivate::OperationMode::ABS;
175+
}
176+
}
177+
178+
this->dataPtr->posPid.Init(p, i, d, iMax, iMin, cmdMax, cmdMin, cmdOffset);
179+
180+
181+
if (_sdf->HasElement("initial_position"))
182+
{
183+
this->dataPtr->jointPosCmd = _sdf->Get<double>("initial_position");
184+
}
185+
186+
187+
}
188+
189+
void MimicJointSystem::PreUpdate(const ignition::gazebo::UpdateInfo &_info,
190+
ignition::gazebo::EntityComponentManager &_ecm)
191+
{
192+
// ignmsg << "MimicJointSystem::PreUpdate" << std::endl;
193+
}
194+
195+
void MimicJointSystem::Update(const ignition::gazebo::UpdateInfo &_info,
196+
ignition::gazebo::EntityComponentManager &_ecm)
197+
{
198+
// ignmsg << "MimicJointSystem::Update" << std::endl;
199+
}
200+
201+
void MimicJointSystem::PostUpdate(const ignition::gazebo::UpdateInfo &_info,
202+
const ignition::gazebo::EntityComponentManager &_ecm)
203+
{
204+
// ignmsg << "MimicJointSystem::PostUpdate" << std::endl;
205+
}

‎ign_ros2_control/src/ign_system.cpp‎

Lines changed: 9 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -95,6 +95,7 @@ struct MimicJoint
9595
std::size_t joint_index;
9696
std::size_t mimicked_joint_index;
9797
double multiplier = 1.0;
98+
double mimic_ff_force_scaling = 0.01;
9899
std::vector<std::string> interfaces_to_mimic;
99100
};
100101

@@ -402,6 +403,12 @@ bool IgnitionSystem::initSim(
402403
mimic_joint.multiplier = 1.0;
403404
}
404405

406+
mimic_joint.mimic_ff_force_scaling =
407+
(hardware_info.joints[j].parameters.find("mimic_ff_force_scaling") ==
408+
hardware_info.joints[j].parameters.end() ) ? mimic_joint.mimic_ff_force_scaling : stod(
409+
hardware_info.joints[j].parameters.at(
410+
"mimic_ff_force_scaling"));
411+
405412
// check joint info of mimicked joint
406413
auto & joint_info_mimicked = hardware_info.joints[mimic_joint.mimicked_joint_index];
407414
const auto state_mimicked_interface = std::find_if(
@@ -959,7 +966,7 @@ hardware_interface::return_type IgnitionSystem::write(
959966
jointAxis->Data().MaxVelocity());
960967

961968
this->dataPtr->joints_[mimic_joint.joint_index].pid_vel.SetCmdOffset(
962-
mimic_joint.multiplier *
969+
mimic_joint.mimic_ff_force_scaling * mimic_joint.multiplier *
963970
this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_position_to_force_cmd);
964971
// calculate target force/torque - output of inner pid
965972
double target_force = this->dataPtr->joints_[mimic_joint.joint_index].pid_vel.Update(
@@ -995,7 +1002,7 @@ hardware_interface::return_type IgnitionSystem::write(
9951002
jointAxis->Data().MaxVelocity());
9961003

9971004
this->dataPtr->joints_[mimic_joint.joint_index].pid_vel.SetCmdOffset(
998-
mimic_joint.multiplier *
1005+
mimic_joint.mimic_ff_force_scaling * mimic_joint.multiplier *
9991006
this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_velocity_to_force_cmd);
10001007

10011008
// calculate target force/torque - output of inner pid

0 commit comments

Comments
 (0)