Skip to content
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.

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

PID effort control of joints (+mimic) #122

Open
wants to merge 25 commits into
base: humble
Choose a base branch
from
Open
Show file tree
Hide file tree
Changes from 13 commits
Commits
Show all changes
25 commits
Select commit Hold shift + click to select a range
bb54c12
Introduce PID for position control.
livanov93 Feb 22, 2023
0104d7a
Add generate parameter library.
livanov93 Feb 23, 2023
c018f7c
Refresh params every loop step.
livanov93 Feb 24, 2023
cb75106
Reorganize calcs.
livanov93 Feb 24, 2023
6d5eda3
Add pid params via hardware parameters.
livanov93 Feb 24, 2023
87496ff
Add pid sim params into demos urdfs.
livanov93 Mar 2, 2023
c11b195
Testing clamping.
livanov93 Mar 2, 2023
c3bd601
Change pid params. Cascade control. Comment mimic stuff for now.
livanov93 Mar 3, 2023
29e9943
Debug output.
livanov93 Mar 8, 2023
25187ea
Adapt pid params for examples.
livanov93 Mar 9, 2023
01e6f68
Move all control to force - based output.
livanov93 Mar 9, 2023
ea36469
Introduce MimicJointSystem plugin.
livanov93 Mar 15, 2023
65b7388
Add mimic joint vel pid only support.
livanov93 Mar 17, 2023
1a89e06
Round commanded force.
livanov93 Mar 20, 2023
9bbb874
Remove suffix for state interfaces as it confuses robot_state_publish…
livanov93 Mar 24, 2023
e40142c
Add cascade control parameter. Code refactoring.
livanov93 Mar 24, 2023
6c671e4
Identation, alphabetizing, usage description comment, empty destructo…
livanov93 Mar 24, 2023
3b47f98
Merge branch 'humble' into pid-with-force-output
livanov93 Mar 24, 2023
1831835
Fix error.
livanov93 Mar 24, 2023
e8ac35e
Namespace fix.
livanov93 Mar 24, 2023
64f6b67
Adapt pid values for simple position example.
livanov93 Mar 24, 2023
6c5ecef
Suggestions and fixes for humble CI. write method return value handling.
livanov93 Mar 27, 2023
a100537
Satisfy ament tools.
livanov93 Mar 27, 2023
496dde5
Fix linters by bypassing for specific lines.
livanov93 Mar 27, 2023
460d47c
Remove suggestion comment.
livanov93 Mar 31, 2023
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
40 changes: 38 additions & 2 deletions ign_ros2_control/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,11 @@ find_package(hardware_interface REQUIRED)
find_package(pluginlib REQUIRED)
find_package(rclcpp REQUIRED)
find_package(yaml_cpp_vendor REQUIRED)
find_package(generate_parameter_library REQUIRED)
livanov93 marked this conversation as resolved.
Show resolved Hide resolved

generate_parameter_library(ign_ros2_control_parameters
src/ign_ros2_control_parameters.yaml
)

if("$ENV{IGNITION_VERSION}" STREQUAL "citadel")
find_package(ignition-gazebo3 REQUIRED)
Expand All @@ -39,10 +44,31 @@ elseif("$ENV{IGNITION_VERSION}" STREQUAL "fortress")
set(IGN_GAZEBO_VER ${ignition-gazebo6_VERSION_MAJOR})
message(STATUS "Compiling against Ignition Fortress")

find_package(ignition-cmake2 REQUIRED)
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

duplicated code in the if and else

find_package(ignition-plugin1 REQUIRED COMPONENTS register)
livanov93 marked this conversation as resolved.
Show resolved Hide resolved
set(IGN_PLUGIN_VER ${ignition-plugin1_VERSION_MAJOR})
add_library(mimic-joint-system SHARED src/MimicJointSystem.cc)
target_link_libraries(mimic-joint-system
PRIVATE ignition-plugin${IGN_PLUGIN_VER}::ignition-plugin${IGN_PLUGIN_VER}
PRIVATE ignition-gazebo6::ignition-gazebo6)
livanov93 marked this conversation as resolved.
Show resolved Hide resolved
install(TARGETS mimic-joint-system
DESTINATION lib)

else()
find_package(ignition-gazebo6 REQUIRED)
set(IGN_GAZEBO_VER ${ignition-gazebo6_VERSION_MAJOR})
message(STATUS "Compiling against Ignition Fortress")

find_package(ignition-cmake2 REQUIRED)
find_package(ignition-plugin1 REQUIRED COMPONENTS register)
set(IGN_PLUGIN_VER ${ignition-plugin1_VERSION_MAJOR})
add_library(mimic-joint-system SHARED src/MimicJointSystem.cc)
target_link_libraries(mimic-joint-system
ignition-gazebo${IGN_GAZEBO_VER}::core
ignition-plugin${IGN_PLUGIN_VER}::register
)
install(TARGETS mimic-joint-system
DESTINATION lib)
endif()

find_package(ignition-plugin1 REQUIRED)
Expand All @@ -58,6 +84,7 @@ target_link_libraries(${PROJECT_NAME}-system
ignition-gazebo${IGN_GAZEBO_VER}::core
ignition-plugin${IGN_PLUGIN_VER}::register
)
target_link_libraries(${PROJECT_NAME}-system ign_ros2_control_parameters)
ament_target_dependencies(${PROJECT_NAME}-system
ament_index_cpp
controller_manager
Expand All @@ -73,6 +100,8 @@ ament_target_dependencies(${PROJECT_NAME}-system
add_library(ign_hardware_plugins SHARED
src/ign_system.cpp
)
target_include_directories(ign_hardware_plugins PRIVATE include)
target_link_libraries(ign_hardware_plugins ign_ros2_control_parameters)
ament_target_dependencies(ign_hardware_plugins
rclcpp_lifecycle
hardware_interface
Expand All @@ -82,9 +111,14 @@ target_link_libraries(ign_hardware_plugins
ignition-gazebo${IGN_GAZEBO_VER}::core
)

install(DIRECTORY include/
DESTINATION include
)

## Install
install(TARGETS
ign_hardware_plugins
ign_hardware_plugins ign_ros2_control_parameters
EXPORT export_ign_ros2_control
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION bin
Expand All @@ -98,7 +132,9 @@ endif()

ament_export_include_directories(include)
ament_export_libraries(${PROJECT_NAME} ign_hardware_plugins)

ament_export_targets(
export_ign_ros2_control HAS_LIBRARY_TARGET
)
# Install directories
install(TARGETS ${PROJECT_NAME}-system
DESTINATION lib
Expand Down
94 changes: 94 additions & 0 deletions ign_ros2_control/include/ign_ros2_control/MimicJointSystem.hh
Original file line number Diff line number Diff line change
@@ -0,0 +1,94 @@
/*
* Copyright 2023 Open Source Robotics Foundation, Inc.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
*/

//----------------------------------------------------------------------
/*!\file
*
* \author Lovro Ivanov [email protected]
* \date 2023-03-15
*
*/
//----------------------------------------------------------------------

#ifndef IGNITION_GAZEBO_SYSTEMS_MIMICJOINTSYSTEM_HH_
#define IGNITION_GAZEBO_SYSTEMS_MIMICJOINTSYSTEM_HH_

//! [header]
#include <memory>

#include <ignition/gazebo/System.hh>

//! <plugin filename="mimic-joint-system" name="ign_ros2_control::MimicJointSystem">
livanov93 marked this conversation as resolved.
Show resolved Hide resolved
//! <joint_name>joint_name</joint_name>
//! <mimic_joint_name>mimic_joint_name</mimic_joint_name>
//! <multiplier>1.0</multiplier>
//! <offset>0.0</offset>
//! <joint_index>0</joint_index>
//! <mimic_joint_index>0</mimic_joint_index>
//! <p_gain>100.0</p_gain>
//! <i_gain>0.1</i_gain>
//! <d_gain>0.0</d_gain>
//! <i_max>5.0</i_max>
//! <i_min>-5.0</i_min>
//! <cmd_max>500.0</cmd_max>
//! <cmd_min>-500.0</cmd_min>
//! <cmd_offset>0.0</cmd_offset>
//! <dead_zone>0.001</dead_zone>
//! <use_velocity_commands>false</use_velocity_commands>
//! </plugin>

namespace ign_ros2_control
{
class MimicJointSystemPrivate;

class MimicJointSystem:
// This class is a system.
public ignition::gazebo::System,
public ignition::gazebo::ISystemConfigure,
// This class also implements the ISystemPreUpdate, ISystemUpdate,
livanov93 marked this conversation as resolved.
Show resolved Hide resolved
// and ISystemPostUpdate interfaces.
public ignition::gazebo::ISystemPreUpdate,
public ignition::gazebo::ISystemUpdate,
public ignition::gazebo::ISystemPostUpdate
{
public: MimicJointSystem();

public: ~MimicJointSystem() override=default;
livanov93 marked this conversation as resolved.
Show resolved Hide resolved

// Documentation inherited
public: void Configure(const ignition::gazebo::Entity &_entity,
const std::shared_ptr<const sdf::Element> &_sdf,
livanov93 marked this conversation as resolved.
Show resolved Hide resolved
ignition::gazebo::EntityComponentManager &_ecm,
ignition::gazebo::EventManager &_eventMgr) override;

public: void PreUpdate(const ignition::gazebo::UpdateInfo &_info,
ignition::gazebo::EntityComponentManager &_ecm) override;

public: void Update(const ignition::gazebo::UpdateInfo &_info,
ignition::gazebo::EntityComponentManager &_ecm) override;

public: void PostUpdate(const ignition::gazebo::UpdateInfo &_info,
const ignition::gazebo::EntityComponentManager &_ecm) override;

private:
/// \brief Private data pointer
private: std::unique_ptr<MimicJointSystemPrivate> dataPtr;
};
}
//! [header]

#endif
13 changes: 13 additions & 0 deletions ign_ros2_control/include/ign_ros2_control/ign_system.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -25,6 +25,10 @@
#include "rclcpp_lifecycle/state.hpp"
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"

#include "ign_ros2_control_parameters.hpp"
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change
#include "ign_ros2_control_parameters.hpp"
// auto-generated by generate_parameter_library
#include "ign_ros2_control_parameters.hpp"

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

TODO still (nitpick)


#include "rclcpp/executors/single_threaded_executor.hpp"
livanov93 marked this conversation as resolved.
Show resolved Hide resolved

namespace ign_ros2_control
{
using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
Expand Down Expand Up @@ -88,6 +92,15 @@ class IgnitionSystem : public IgnitionSystemInterface

/// \brief Private data class
std::unique_ptr<IgnitionSystemPrivate> dataPtr;

// Parameters from ROS for ign_ros2_control
std::shared_ptr<ParamListener> param_listener_;
Params params_;

rclcpp::Node::SharedPtr param_node_;
std::thread spin_thread_;
std::atomic<bool> stop_spin_ = false;
rclcpp::executors::SingleThreadedExecutor::SharedPtr exec_;
};

} // namespace ign_ros2_control
Expand Down
1 change: 1 addition & 0 deletions ign_ros2_control/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,7 @@
<depend>rclcpp_lifecycle</depend>
<depend>hardware_interface</depend>
<depend>controller_manager</depend>
<depend>generate_parameter_library</depend>

<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
Expand Down
Loading