nic/drawing-robot-ros2
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Add custom xarm packages

Browse Source
This commit is contained in:
Nicolas Hiillos
2023-03-21 13:33:51 +02:00
parent 1763aee9ca
commit e42e0fea90
176 changed files with 13024 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

76
src/custom_xarm_gazebo/CMakeLists.txt Normal file
View File

@@ -0,0 +1,76 @@
cmake_minimum_required(VERSION 3.5)
project(custom_xarm_gazebo)
# Default to C99
if(NOT CMAKE_C_STANDARD)
set(CMAKE_C_STANDARD 99)
endif()
# Default to C++14
if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 14)
endif()
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(control_toolbox REQUIRED)
find_package(controller_manager REQUIRED)
find_package(gazebo_ros REQUIRED)
# uncomment the following section in order to fill in
# further dependencies manually.
# find_package(<dependency> REQUIRED)
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
# the following line skips the linter which checks for copyrights
# uncomment the line when a copyright and license is not present in all source files
#set(ament_cmake_copyright_FOUND TRUE)
# the following line skips cpplint (only works in a git repo)
# uncomment the line when this package is not in a git repo
#set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
endif()
include_directories(
include
${ament_INCLUDE_DIRS}
${GAZEBO_INCLUDE_DIRS}
)
link_directories(${GAZEBO_LIBRARY_DIRS})
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}")
add_library(gazebo_mimic_joint_plugin SHARED src/mimic_joint_plugin.cpp)
ament_target_dependencies(gazebo_mimic_joint_plugin
control_toolbox
controller_manager
gazebo_ros
)
target_link_libraries(gazebo_mimic_joint_plugin ${ament_LIBRARIES} ${GAZEBO_LIBRARIES})
install(
DIRECTORY include/
DESTINATION include
)
install(TARGETS
gazebo_mimic_joint_plugin
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION bin
)
install(DIRECTORY
worlds
launch
DESTINATION share/${PROJECT_NAME}/
)
ament_package()

79
src/custom_xarm_gazebo/include/xarm_gazebo/mimic_joint_plugin.h Normal file
View File

@@ -0,0 +1,79 @@
/*
Copyright (c) 2014, Konstantinos Chatzilygeroudis
All rights reserved.
Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer
in the documentation and/or other materials provided with the distribution.
3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived
from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING,
BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
Refer to: https://github.com/roboticsgroup/roboticsgroup_upatras_gazebo_plugins
Modified for ROS2 compatibility and UFACTORY xArm gripper simulation
Modified by: Vinman <vinman.cub@gmail.com>
============================================================================*/
#ifndef XARM_GAZEBO_MIMIC_JOINT_PLUGIN_H
#define XARM_GAZEBO_MIMIC_JOINT_PLUGIN_H
#include <rclcpp/rclcpp.hpp>
#include "controller_manager/controller_manager.hpp"
// ros_control
#include <control_toolbox/pid.hpp>
// Gazebo includes
#include <gazebo/common/Plugin.hh>
#include <gazebo/common/common.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/physics/Model.hh>
#include <gazebo_ros/node.hpp>
namespace gazebo {
class GazeboMimicJointPlugin : public ModelPlugin {
public:
GazeboMimicJointPlugin();
virtual ~GazeboMimicJointPlugin() override;
// Overloaded Gazebo entry point
virtual void Load(gazebo::physics::ModelPtr parent, sdf::ElementPtr sdf) override;
private:
void OnUpdate();
// Node Handles
gazebo_ros::Node::SharedPtr model_nh_;
// Pointer to the update event connection
gazebo::event::ConnectionPtr update_connection_;
// PID controller if needed
std::shared_ptr<control_toolbox::Pid> pid_;
// Pointers to the joints
physics::JointPtr joint_, mimic_joint_;
// Pointer to the model
physics::ModelPtr model_;
// Parameters
std::string joint_name_, mimic_joint_name_;
double multiplier_, offset_, sensitiveness_, max_effort_;
bool has_pid_;
std::shared_ptr<rclcpp::Rate> loop_rate_;
};
}
#endif

258
src/custom_xarm_gazebo/launch/_dual_robot_beside_table_gazebo.launch.py Normal file
View File

@@ -0,0 +1,258 @@
#!/usr/bin/env python3
# Software License Agreement (BSD License)
#
# Copyright (c) 2021, UFACTORY, Inc.
# All rights reserved.
#
# Author: Vinman <vinman.wen@ufactory.cc> <vinman.cub@gmail.com>
import os
from ament_index_python import get_package_share_directory
from launch.launch_description_sources import load_python_launch_file_as_module
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument, RegisterEventHandler
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
from launch.event_handlers import OnProcessExit
from launch.actions import OpaqueFunction
def launch_setup(context, *args, **kwargs):
prefix_1 = LaunchConfiguration('prefix_1', default='L_')
prefix_2 = LaunchConfiguration('prefix_2', default='R_')
dof = LaunchConfiguration('dof', default=7)
dof_1 = LaunchConfiguration('dof_1', default=dof)
dof_2 = LaunchConfiguration('dof_2', default=dof)
robot_type = LaunchConfiguration('robot_type', default='xarm')
robot_type_1 = LaunchConfiguration('robot_type_1', default=robot_type)
robot_type_2 = LaunchConfiguration('robot_type_2', default=robot_type)
add_gripper = LaunchConfiguration('add_gripper', default=False)
add_gripper_1 = LaunchConfiguration('add_gripper_1', default=add_gripper)
add_gripper_2 = LaunchConfiguration('add_gripper_2', default=add_gripper)
add_vacuum_gripper = LaunchConfiguration('add_vacuum_gripper', default=False)
add_vacuum_gripper_1 = LaunchConfiguration('add_vacuum_gripper_1', default=add_vacuum_gripper)
add_vacuum_gripper_2 = LaunchConfiguration('add_vacuum_gripper_2', default=add_vacuum_gripper)
hw_ns = LaunchConfiguration('hw_ns', default='xarm')
limited = LaunchConfiguration('limited', default=False)
effort_control = LaunchConfiguration('effort_control', default=False)
velocity_control = LaunchConfiguration('velocity_control', default=False)
ros2_control_plugin = LaunchConfiguration('ros2_control_plugin', default='gazebo_ros2_control/GazeboSystem')
add_realsense_d435i = LaunchConfiguration('add_realsense_d435i', default=False)
add_realsense_d435i_1 = LaunchConfiguration('add_realsense_d435i_1', default=add_realsense_d435i)
add_realsense_d435i_2 = LaunchConfiguration('add_realsense_d435i_2', default=add_realsense_d435i)
add_other_geometry = LaunchConfiguration('add_other_geometry', default=False)
add_other_geometry_1 = LaunchConfiguration('add_other_geometry_1', default=add_other_geometry)
add_other_geometry_2 = LaunchConfiguration('add_other_geometry_2', default=add_other_geometry)
geometry_type = LaunchConfiguration('geometry_type', default='box')
geometry_type_1 = LaunchConfiguration('geometry_type_1', default=geometry_type)
geometry_type_2 = LaunchConfiguration('geometry_type_2', default=geometry_type)
geometry_mass = LaunchConfiguration('geometry_mass', default=0.1)
geometry_mass_1 = LaunchConfiguration('geometry_mass_1', default=geometry_mass)
geometry_mass_2 = LaunchConfiguration('geometry_mass_2', default=geometry_mass)
geometry_height = LaunchConfiguration('geometry_height', default=0.1)
geometry_height_1 = LaunchConfiguration('geometry_height_1', default=geometry_height)
geometry_height_2 = LaunchConfiguration('geometry_height_2', default=geometry_height)
geometry_radius = LaunchConfiguration('geometry_radius', default=0.1)
geometry_radius_1 = LaunchConfiguration('geometry_radius_1', default=geometry_radius)
geometry_radius_2 = LaunchConfiguration('geometry_radius_2', default=geometry_radius)
geometry_length = LaunchConfiguration('geometry_length', default=0.1)
geometry_length_1 = LaunchConfiguration('geometry_length_1', default=geometry_length)
geometry_length_2 = LaunchConfiguration('geometry_length_2', default=geometry_length)
geometry_width = LaunchConfiguration('geometry_width', default=0.1)
geometry_width_1 = LaunchConfiguration('geometry_width_1', default=geometry_width)
geometry_width_2 = LaunchConfiguration('geometry_width_2', default=geometry_width)
geometry_mesh_filename = LaunchConfiguration('geometry_mesh_filename', default='')
geometry_mesh_filename_1 = LaunchConfiguration('geometry_mesh_filename_1', default=geometry_mesh_filename)
geometry_mesh_filename_2 = LaunchConfiguration('geometry_mesh_filename_2', default=geometry_mesh_filename)
geometry_mesh_origin_xyz = LaunchConfiguration('geometry_mesh_origin_xyz', default='"0 0 0"')
geometry_mesh_origin_xyz_1 = LaunchConfiguration('geometry_mesh_origin_xyz_1', default=geometry_mesh_origin_xyz)
geometry_mesh_origin_xyz_2 = LaunchConfiguration('geometry_mesh_origin_xyz_2', default=geometry_mesh_origin_xyz)
geometry_mesh_origin_rpy = LaunchConfiguration('geometry_mesh_origin_rpy', default='"0 0 0"')
geometry_mesh_origin_rpy_1 = LaunchConfiguration('geometry_mesh_origin_rpy_1', default=geometry_mesh_origin_rpy)
geometry_mesh_origin_rpy_2 = LaunchConfiguration('geometry_mesh_origin_rpy_2', default=geometry_mesh_origin_rpy)
geometry_mesh_tcp_xyz = LaunchConfiguration('geometry_mesh_tcp_xyz', default='"0 0 0"')
geometry_mesh_tcp_xyz_1 = LaunchConfiguration('geometry_mesh_tcp_xyz_1', default=geometry_mesh_tcp_xyz)
geometry_mesh_tcp_xyz_2 = LaunchConfiguration('geometry_mesh_tcp_xyz_2', default=geometry_mesh_tcp_xyz)
geometry_mesh_tcp_rpy = LaunchConfiguration('geometry_mesh_tcp_rpy', default='"0 0 0"')
geometry_mesh_tcp_rpy_1 = LaunchConfiguration('geometry_mesh_tcp_rpy_1', default=geometry_mesh_tcp_rpy)
geometry_mesh_tcp_rpy_2 = LaunchConfiguration('geometry_mesh_tcp_rpy_2', default=geometry_mesh_tcp_rpy)
load_controller = LaunchConfiguration('load_controller', default=False)
ros_namespace = LaunchConfiguration('ros_namespace', default='').perform(context)
# ros2 control params
# xarm_controller/launch/lib/robot_controller_lib.py
mod = load_python_launch_file_as_module(os.path.join(get_package_share_directory('xarm_controller'), 'launch', 'lib', 'robot_controller_lib.py'))
generate_dual_ros2_control_params_temp_file = getattr(mod, 'generate_dual_ros2_control_params_temp_file')
ros2_control_params = generate_dual_ros2_control_params_temp_file(
os.path.join(get_package_share_directory('xarm_controller'), 'config', '{}{}_controllers.yaml'.format(robot_type_1.perform(context), dof_1.perform(context))),
os.path.join(get_package_share_directory('xarm_controller'), 'config', '{}{}_controllers.yaml'.format(robot_type_2.perform(context), dof_2.perform(context))),
prefix_1=prefix_1.perform(context),
prefix_2=prefix_2.perform(context),
add_gripper_1=add_gripper_1.perform(context) in ('True', 'true'),
add_gripper_2=add_gripper_2.perform(context) in ('True', 'true'),
ros_namespace=ros_namespace,
update_rate=1000,
)
# robot_description
# xarm_description/launch/lib/robot_description_lib.py
mod = load_python_launch_file_as_module(os.path.join(get_package_share_directory('xarm_description'), 'launch', 'lib', 'robot_description_lib.py'))
get_xacro_file_content = getattr(mod, 'get_xacro_file_content')
robot_description = {
'robot_description': get_xacro_file_content(
xacro_file=PathJoinSubstitution([FindPackageShare('xarm_description'), 'urdf', 'dual_xarm_device.urdf.xacro']),
arguments={
'prefix_1': prefix_1,
'prefix_2': prefix_2,
'dof_1': dof_1,
'dof_2': dof_2,
'robot_type_1': robot_type_1,
'robot_type_2': robot_type_2,
'add_gripper_1': add_gripper_1,
'add_gripper_2': add_gripper_2,
'add_vacuum_gripper_1': add_vacuum_gripper_1,
'add_vacuum_gripper_2': add_vacuum_gripper_2,
'hw_ns': hw_ns.perform(context).strip('/'),
'limited': limited,
'effort_control': effort_control,
'velocity_control': velocity_control,
'ros2_control_plugin': ros2_control_plugin,
'ros2_control_params': ros2_control_params,
'add_realsense_d435i_1': add_realsense_d435i_1,
'add_realsense_d435i_2': add_realsense_d435i_2,
'add_other_geometry_1': add_other_geometry_1,
'add_other_geometry_2': add_other_geometry_2,
'geometry_type_1': geometry_type_1,
'geometry_type_2': geometry_type_2,
'geometry_mass_1': geometry_mass_1,
'geometry_mass_2': geometry_mass_2,
'geometry_height_1': geometry_height_1,
'geometry_height_2': geometry_height_2,
'geometry_radius_1': geometry_radius_1,
'geometry_radius_2': geometry_radius_2,
'geometry_length_1': geometry_length_1,
'geometry_length_2': geometry_length_2,
'geometry_width_1': geometry_width_1,
'geometry_width_2': geometry_width_2,
'geometry_mesh_filename_1': geometry_mesh_filename_1,
'geometry_mesh_filename_2': geometry_mesh_filename_2,
'geometry_mesh_origin_xyz_1': geometry_mesh_origin_xyz_1,
'geometry_mesh_origin_xyz_2': geometry_mesh_origin_xyz_2,
'geometry_mesh_origin_rpy_1': geometry_mesh_origin_rpy_1,
'geometry_mesh_origin_rpy_2': geometry_mesh_origin_rpy_2,
'geometry_mesh_tcp_xyz_1': geometry_mesh_tcp_xyz_1,
'geometry_mesh_tcp_xyz_2': geometry_mesh_tcp_xyz_2,
'geometry_mesh_tcp_rpy_1': geometry_mesh_tcp_rpy_1,
'geometry_mesh_tcp_rpy_2': geometry_mesh_tcp_rpy_2,
}
),
}
# robot state publisher node
robot_state_publisher_node = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
output='screen',
parameters=[{'use_sim_time': True}, robot_description],
remappings=[
('/tf', 'tf'),
('/tf_static', 'tf_static'),
]
)
# gazebo launch
# gazebo_ros/launch/gazebo.launch.py
xarm_gazebo_world = PathJoinSubstitution([FindPackageShare('xarm_gazebo'), 'worlds', 'table.world'])
gazebo_launch = IncludeLaunchDescription(
PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('gazebo_ros'), 'launch', 'gazebo.launch.py'])),
launch_arguments={
'world': xarm_gazebo_world,
'server_required': 'true',
'gui_required': 'true',
# 'pause': 'true'
}.items(),
)
# gazebo spawn entity node
gazebo_spawn_entity_node = Node(
package="gazebo_ros",
executable="spawn_entity.py",
output='screen',
arguments=[
'-topic', 'robot_description',
'-entity', 'dual_xarm',
'-x', '0.5',
'-y', '-0.5',
'-z', '1.021',
'-Y', '1.571',
],
parameters=[{'use_sim_time': True}],
)
# Load controllers
controllers = [
'joint_state_broadcaster',
'{}{}{}_traj_controller'.format(prefix_1.perform(context), robot_type_1.perform(context), dof_1.perform(context)),
'{}{}{}_traj_controller'.format(prefix_2.perform(context), robot_type_2.perform(context), dof_2.perform(context)),
]
# check robot_type is xarm
if robot_type_1.perform(context) == 'xarm' and add_gripper_1.perform(context) in ('True', 'true'):
controllers.append('{}{}_gripper_traj_controller'.format(prefix_1.perform(context), robot_type_1.perform(context)))
# check robot_type is xarm
if robot_type_2.perform(context) == 'xarm' and add_gripper_2.perform(context) in ('True', 'true'):
controllers.append('{}{}_gripper_traj_controller'.format(prefix_2.perform(context), robot_type_2.perform(context)))
load_controllers = []
if load_controller.perform(context) in ('True', 'true'):
for controller in controllers:
load_controllers.append(Node(
package='controller_manager',
executable='spawner',
output='screen',
arguments=[
controller,
'--controller-manager', '{}/controller_manager'.format(ros_namespace)
],
parameters=[{'use_sim_time': True}],
))
if len(load_controllers) > 0:
return [
RegisterEventHandler(
event_handler=OnProcessExit(
target_action=gazebo_spawn_entity_node,
on_exit=load_controllers,
)
),
gazebo_launch,
robot_state_publisher_node,
gazebo_spawn_entity_node,
]
else:
return [
gazebo_launch,
robot_state_publisher_node,
gazebo_spawn_entity_node,
]
def generate_launch_description():
return LaunchDescription([
OpaqueFunction(function=launch_setup)
])

184
src/custom_xarm_gazebo/launch/_robot_beside_table_gazebo.launch.py Normal file
View File

@@ -0,0 +1,184 @@
#!/usr/bin/env python3
# Software License Agreement (BSD License)
#
# Copyright (c) 2021, UFACTORY, Inc.
# All rights reserved.
#
# Author: Vinman <vinman.wen@ufactory.cc> <vinman.cub@gmail.com>
import os
from ament_index_python import get_package_share_directory
from launch.launch_description_sources import load_python_launch_file_as_module
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument, RegisterEventHandler
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
from launch.event_handlers import OnProcessExit
from launch.actions import OpaqueFunction
def launch_setup(context, *args, **kwargs):
prefix = LaunchConfiguration('prefix', default='')
hw_ns = LaunchConfiguration('hw_ns', default='xarm')
limited = LaunchConfiguration('limited', default=False)
effort_control = LaunchConfiguration('effort_control', default=False)
velocity_control = LaunchConfiguration('velocity_control', default=False)
add_gripper = LaunchConfiguration('add_gripper', default=False)
add_vacuum_gripper = LaunchConfiguration('add_vacuum_gripper', default=False)
dof = LaunchConfiguration('dof', default=7)
robot_type = LaunchConfiguration('robot_type', default='xarm')
ros2_control_plugin = LaunchConfiguration('ros2_control_plugin', default='gazebo_ros2_control/GazeboSystem')
add_realsense_d435i = LaunchConfiguration('add_realsense_d435i', default=False)
add_other_geometry = LaunchConfiguration('add_other_geometry', default=False)
geometry_type = LaunchConfiguration('geometry_type', default='box')
geometry_mass = LaunchConfiguration('geometry_mass', default=0.1)
geometry_height = LaunchConfiguration('geometry_height', default=0.1)
geometry_radius = LaunchConfiguration('geometry_radius', default=0.1)
geometry_length = LaunchConfiguration('geometry_length', default=0.1)
geometry_width = LaunchConfiguration('geometry_width', default=0.1)
geometry_mesh_filename = LaunchConfiguration('geometry_mesh_filename', default='')
geometry_mesh_origin_xyz = LaunchConfiguration('geometry_mesh_origin_xyz', default='"0 0 0"')
geometry_mesh_origin_rpy = LaunchConfiguration('geometry_mesh_origin_rpy', default='"0 0 0"')
geometry_mesh_tcp_xyz = LaunchConfiguration('geometry_mesh_tcp_xyz', default='"0 0 0"')
geometry_mesh_tcp_rpy = LaunchConfiguration('geometry_mesh_tcp_rpy', default='"0 0 0"')
load_controller = LaunchConfiguration('load_controller', default=False)
ros_namespace = LaunchConfiguration('ros_namespace', default='').perform(context)
# ros2 control params
# xarm_controller/launch/lib/robot_controller_lib.py
mod = load_python_launch_file_as_module(os.path.join(get_package_share_directory('xarm_controller'), 'launch', 'lib', 'robot_controller_lib.py'))
generate_ros2_control_params_temp_file = getattr(mod, 'generate_ros2_control_params_temp_file')
ros2_control_params = generate_ros2_control_params_temp_file(
os.path.join(get_package_share_directory('xarm_controller'), 'config', '{}{}_controllers.yaml'.format(robot_type.perform(context), dof.perform(context))),
prefix=prefix.perform(context),
add_gripper=add_gripper.perform(context) in ('True', 'true'),
ros_namespace=LaunchConfiguration('ros_namespace', default='').perform(context),
update_rate=1000,
)
# robot_description
# xarm_description/launch/lib/robot_description_lib.py
mod = load_python_launch_file_as_module(os.path.join(get_package_share_directory('xarm_description'), 'launch', 'lib', 'robot_description_lib.py'))
get_xacro_file_content = getattr(mod, 'get_xacro_file_content')
robot_description = {
'robot_description': get_xacro_file_content(
xacro_file=PathJoinSubstitution([FindPackageShare('xarm_description'), 'urdf', 'xarm_device.urdf.xacro']),
arguments={
'prefix': prefix,
'dof': dof,
'robot_type': robot_type,
'add_gripper': add_gripper,
'add_vacuum_gripper': add_vacuum_gripper,
'hw_ns': hw_ns.perform(context).strip('/'),
'limited': limited,
'effort_control': effort_control,
'velocity_control': velocity_control,
'ros2_control_plugin': ros2_control_plugin,
'ros2_control_params': ros2_control_params,
'add_realsense_d435i': add_realsense_d435i,
'add_other_geometry': add_other_geometry,
'geometry_type': geometry_type,
'geometry_mass': geometry_mass,
'geometry_height': geometry_height,
'geometry_radius': geometry_radius,
'geometry_length': geometry_length,
'geometry_width': geometry_width,
'geometry_mesh_filename': geometry_mesh_filename,
'geometry_mesh_origin_xyz': geometry_mesh_origin_xyz,
'geometry_mesh_origin_rpy': geometry_mesh_origin_rpy,
'geometry_mesh_tcp_xyz': geometry_mesh_tcp_xyz,
'geometry_mesh_tcp_rpy': geometry_mesh_tcp_rpy,
}
),
}
# robot state publisher node
robot_state_publisher_node = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
output='screen',
parameters=[{'use_sim_time': True}, robot_description],
remappings=[
('/tf', 'tf'),
('/tf_static', 'tf_static'),
]
)
# gazebo launch
# gazebo_ros/launch/gazebo.launch.py
xarm_gazebo_world = PathJoinSubstitution([FindPackageShare('xarm_gazebo'), 'worlds', 'table.world'])
gazebo_launch = IncludeLaunchDescription(
PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('gazebo_ros'), 'launch', 'gazebo.launch.py'])),
launch_arguments={
'world': xarm_gazebo_world,
'server_required': 'true',
'gui_required': 'true',
}.items(),
)
# gazebo spawn entity node
gazebo_spawn_entity_node = Node(
package="gazebo_ros",
executable="spawn_entity.py",
output='screen',
arguments=[
'-topic', 'robot_description',
'-entity', '{}{}'.format(robot_type.perform(context), dof.perform(context)),
'-x', '-0.2',
'-y', '-0.5',
'-z', '1.021',
'-Y', '1.571',
],
parameters=[{'use_sim_time': True}],
)
# Load controllers
controllers = [
'joint_state_broadcaster',
'{}{}{}_traj_controller'.format(prefix.perform(context), robot_type.perform(context), dof.perform(context)),
]
if robot_type.perform(context) == 'xarm' and add_gripper.perform(context) in ('True', 'true'):
controllers.append('{}{}_gripper_traj_controller'.format(prefix.perform(context), robot_type.perform(context)))
load_controllers = []
if load_controller.perform(context) in ('True', 'true'):
for controller in controllers:
load_controllers.append(Node(
package='controller_manager',
executable='spawner',
output='screen',
arguments=[
controller,
'--controller-manager', '{}/controller_manager'.format(ros_namespace)
],
parameters=[{'use_sim_time': True}],
))
if len(load_controllers) > 0:
return [
RegisterEventHandler(
event_handler=OnProcessExit(
target_action=gazebo_spawn_entity_node,
on_exit=load_controllers,
)
),
gazebo_launch,
robot_state_publisher_node,
gazebo_spawn_entity_node,
]
else:
return [
gazebo_launch,
robot_state_publisher_node,
gazebo_spawn_entity_node,
]
def generate_launch_description():
return LaunchDescription([
OpaqueFunction(function=launch_setup)
])

70
src/custom_xarm_gazebo/launch/lite6_beside_table_gazebo.launch.py Normal file
View File

@@ -0,0 +1,70 @@
#!/usr/bin/env python3
# Software License Agreement (BSD License)
#
# Copyright (c) 2021, UFACTORY, Inc.
# All rights reserved.
#
# Author: Vinman <vinman.wen@ufactory.cc> <vinman.cub@gmail.com>
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir
def generate_launch_description():
prefix = LaunchConfiguration('prefix', default='')
hw_ns = LaunchConfiguration('hw_ns', default='xarm')
limited = LaunchConfiguration('limited', default=False)
effort_control = LaunchConfiguration('effort_control', default=False)
velocity_control = LaunchConfiguration('velocity_control', default=False)
add_gripper = LaunchConfiguration('add_gripper', default=False)
add_vacuum_gripper = LaunchConfiguration('add_vacuum_gripper', default=False)
add_realsense_d435i = LaunchConfiguration('add_realsense_d435i', default=False)
add_other_geometry = LaunchConfiguration('add_other_geometry', default=False)
geometry_type = LaunchConfiguration('geometry_type', default='box')
geometry_mass = LaunchConfiguration('geometry_mass', default=0.1)
geometry_height = LaunchConfiguration('geometry_height', default=0.1)
geometry_radius = LaunchConfiguration('geometry_radius', default=0.1)
geometry_length = LaunchConfiguration('geometry_length', default=0.1)
geometry_width = LaunchConfiguration('geometry_width', default=0.1)
geometry_mesh_filename = LaunchConfiguration('geometry_mesh_filename', default='')
geometry_mesh_origin_xyz = LaunchConfiguration('geometry_mesh_origin_xyz', default='"0 0 0"')
geometry_mesh_origin_rpy = LaunchConfiguration('geometry_mesh_origin_rpy', default='"0 0 0"')
geometry_mesh_tcp_xyz = LaunchConfiguration('geometry_mesh_tcp_xyz', default='"0 0 0"')
geometry_mesh_tcp_rpy = LaunchConfiguration('geometry_mesh_tcp_rpy', default='"0 0 0"')
# robot gazebo launch
# xarm_gazebo/launch/_robot_beside_table_gazebo.launch.py
robot_gazobo_launch = IncludeLaunchDescription(
PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/_robot_beside_table_gazebo.launch.py']),
launch_arguments={
'prefix': prefix,
'hw_ns': hw_ns,
'limited': limited,
'effort_control': effort_control,
'velocity_control': velocity_control,
'add_gripper': add_gripper,
'add_vacuum_gripper': add_vacuum_gripper,
'dof': '6',
'robot_type': 'lite',
'add_realsense_d435i': add_realsense_d435i,
'add_other_geometry': add_other_geometry,
'geometry_type': geometry_type,
'geometry_mass': geometry_mass,
'geometry_height': geometry_height,
'geometry_radius': geometry_radius,
'geometry_length': geometry_length,
'geometry_width': geometry_width,
'geometry_mesh_filename': geometry_mesh_filename,
'geometry_mesh_origin_xyz': geometry_mesh_origin_xyz,
'geometry_mesh_origin_rpy': geometry_mesh_origin_rpy,
'geometry_mesh_tcp_xyz': geometry_mesh_tcp_xyz,
'geometry_mesh_tcp_rpy': geometry_mesh_tcp_rpy,
}.items(),
)
return LaunchDescription([
robot_gazobo_launch
])

70
src/custom_xarm_gazebo/launch/xarm5_beside_table_gazebo.launch.py Normal file
View File

@@ -0,0 +1,70 @@
#!/usr/bin/env python3
# Software License Agreement (BSD License)
#
# Copyright (c) 2021, UFACTORY, Inc.
# All rights reserved.
#
# Author: Vinman <vinman.wen@ufactory.cc> <vinman.cub@gmail.com>
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir
def generate_launch_description():
prefix = LaunchConfiguration('prefix', default='')
hw_ns = LaunchConfiguration('hw_ns', default='xarm')
limited = LaunchConfiguration('limited', default=False)
effort_control = LaunchConfiguration('effort_control', default=False)
velocity_control = LaunchConfiguration('velocity_control', default=False)
add_gripper = LaunchConfiguration('add_gripper', default=False)
add_vacuum_gripper = LaunchConfiguration('add_vacuum_gripper', default=False)
add_realsense_d435i = LaunchConfiguration('add_realsense_d435i', default=False)
add_other_geometry = LaunchConfiguration('add_other_geometry', default=False)
geometry_type = LaunchConfiguration('geometry_type', default='box')
geometry_mass = LaunchConfiguration('geometry_mass', default=0.1)
geometry_height = LaunchConfiguration('geometry_height', default=0.1)
geometry_radius = LaunchConfiguration('geometry_radius', default=0.1)
geometry_length = LaunchConfiguration('geometry_length', default=0.1)
geometry_width = LaunchConfiguration('geometry_width', default=0.1)
geometry_mesh_filename = LaunchConfiguration('geometry_mesh_filename', default='')
geometry_mesh_origin_xyz = LaunchConfiguration('geometry_mesh_origin_xyz', default='"0 0 0"')
geometry_mesh_origin_rpy = LaunchConfiguration('geometry_mesh_origin_rpy', default='"0 0 0"')
geometry_mesh_tcp_xyz = LaunchConfiguration('geometry_mesh_tcp_xyz', default='"0 0 0"')
geometry_mesh_tcp_rpy = LaunchConfiguration('geometry_mesh_tcp_rpy', default='"0 0 0"')
# robot gazebo launch
# xarm_gazebo/launch/_robot_beside_table_gazebo.launch.py
robot_gazobo_launch = IncludeLaunchDescription(
PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/_robot_beside_table_gazebo.launch.py']),
launch_arguments={
'prefix': prefix,
'hw_ns': hw_ns,
'limited': limited,
'effort_control': effort_control,
'velocity_control': velocity_control,
'add_gripper': add_gripper,
'add_vacuum_gripper': add_vacuum_gripper,
'dof': '5',
'robot_type': 'xarm',
'add_realsense_d435i': add_realsense_d435i,
'add_other_geometry': add_other_geometry,
'geometry_type': geometry_type,
'geometry_mass': geometry_mass,
'geometry_height': geometry_height,
'geometry_radius': geometry_radius,
'geometry_length': geometry_length,
'geometry_width': geometry_width,
'geometry_mesh_filename': geometry_mesh_filename,
'geometry_mesh_origin_xyz': geometry_mesh_origin_xyz,
'geometry_mesh_origin_rpy': geometry_mesh_origin_rpy,
'geometry_mesh_tcp_xyz': geometry_mesh_tcp_xyz,
'geometry_mesh_tcp_rpy': geometry_mesh_tcp_rpy,
}.items(),
)
return LaunchDescription([
robot_gazobo_launch
])

70
src/custom_xarm_gazebo/launch/xarm6_beside_table_gazebo.launch.py Normal file
View File

@@ -0,0 +1,70 @@
#!/usr/bin/env python3
# Software License Agreement (BSD License)
#
# Copyright (c) 2021, UFACTORY, Inc.
# All rights reserved.
#
# Author: Vinman <vinman.wen@ufactory.cc> <vinman.cub@gmail.com>
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir
def generate_launch_description():
prefix = LaunchConfiguration('prefix', default='')
hw_ns = LaunchConfiguration('hw_ns', default='xarm')
limited = LaunchConfiguration('limited', default=False)
effort_control = LaunchConfiguration('effort_control', default=False)
velocity_control = LaunchConfiguration('velocity_control', default=False)
add_gripper = LaunchConfiguration('add_gripper', default=False)
add_vacuum_gripper = LaunchConfiguration('add_vacuum_gripper', default=False)
add_realsense_d435i = LaunchConfiguration('add_realsense_d435i', default=False)
add_other_geometry = LaunchConfiguration('add_other_geometry', default=False)
geometry_type = LaunchConfiguration('geometry_type', default='box')
geometry_mass = LaunchConfiguration('geometry_mass', default=0.1)
geometry_height = LaunchConfiguration('geometry_height', default=0.1)
geometry_radius = LaunchConfiguration('geometry_radius', default=0.1)
geometry_length = LaunchConfiguration('geometry_length', default=0.1)
geometry_width = LaunchConfiguration('geometry_width', default=0.1)
geometry_mesh_filename = LaunchConfiguration('geometry_mesh_filename', default='')
geometry_mesh_origin_xyz = LaunchConfiguration('geometry_mesh_origin_xyz', default='"0 0 0"')
geometry_mesh_origin_rpy = LaunchConfiguration('geometry_mesh_origin_rpy', default='"0 0 0"')
geometry_mesh_tcp_xyz = LaunchConfiguration('geometry_mesh_tcp_xyz', default='"0 0 0"')
geometry_mesh_tcp_rpy = LaunchConfiguration('geometry_mesh_tcp_rpy', default='"0 0 0"')
# robot gazebo launch
# xarm_gazebo/launch/_robot_beside_table_gazebo.launch.py
robot_gazobo_launch = IncludeLaunchDescription(
PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/_robot_beside_table_gazebo.launch.py']),
launch_arguments={
'prefix': prefix,
'hw_ns': hw_ns,
'limited': limited,
'effort_control': effort_control,
'velocity_control': velocity_control,
'add_gripper': add_gripper,
'add_vacuum_gripper': add_vacuum_gripper,
'dof': '6',
'robot_type': 'xarm',
'add_realsense_d435i': add_realsense_d435i,
'add_other_geometry': add_other_geometry,
'geometry_type': geometry_type,
'geometry_mass': geometry_mass,
'geometry_height': geometry_height,
'geometry_radius': geometry_radius,
'geometry_length': geometry_length,
'geometry_width': geometry_width,
'geometry_mesh_filename': geometry_mesh_filename,
'geometry_mesh_origin_xyz': geometry_mesh_origin_xyz,
'geometry_mesh_origin_rpy': geometry_mesh_origin_rpy,
'geometry_mesh_tcp_xyz': geometry_mesh_tcp_xyz,
'geometry_mesh_tcp_rpy': geometry_mesh_tcp_rpy,
}.items(),
)
return LaunchDescription([
robot_gazobo_launch
])

70
src/custom_xarm_gazebo/launch/xarm7_beside_table_gazebo.launch.py Normal file
View File

@@ -0,0 +1,70 @@
#!/usr/bin/env python3
# Software License Agreement (BSD License)
#
# Copyright (c) 2021, UFACTORY, Inc.
# All rights reserved.
#
# Author: Vinman <vinman.wen@ufactory.cc> <vinman.cub@gmail.com>
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, ThisLaunchFileDir
def generate_launch_description():
prefix = LaunchConfiguration('prefix', default='')
hw_ns = LaunchConfiguration('hw_ns', default='xarm')
limited = LaunchConfiguration('limited', default=False)
effort_control = LaunchConfiguration('effort_control', default=False)
velocity_control = LaunchConfiguration('velocity_control', default=False)
add_gripper = LaunchConfiguration('add_gripper', default=False)
add_vacuum_gripper = LaunchConfiguration('add_vacuum_gripper', default=False)
add_realsense_d435i = LaunchConfiguration('add_realsense_d435i', default=False)
add_other_geometry = LaunchConfiguration('add_other_geometry', default=False)
geometry_type = LaunchConfiguration('geometry_type', default='box')
geometry_mass = LaunchConfiguration('geometry_mass', default=0.1)
geometry_height = LaunchConfiguration('geometry_height', default=0.1)
geometry_radius = LaunchConfiguration('geometry_radius', default=0.1)
geometry_length = LaunchConfiguration('geometry_length', default=0.1)
geometry_width = LaunchConfiguration('geometry_width', default=0.1)
geometry_mesh_filename = LaunchConfiguration('geometry_mesh_filename', default='')
geometry_mesh_origin_xyz = LaunchConfiguration('geometry_mesh_origin_xyz', default='"0 0 0"')
geometry_mesh_origin_rpy = LaunchConfiguration('geometry_mesh_origin_rpy', default='"0 0 0"')
geometry_mesh_tcp_xyz = LaunchConfiguration('geometry_mesh_tcp_xyz', default='"0 0 0"')
geometry_mesh_tcp_rpy = LaunchConfiguration('geometry_mesh_tcp_rpy', default='"0 0 0"')
# robot gazebo launch
# xarm_gazebo/launch/_robot_beside_table_gazebo.launch.py
robot_gazobo_launch = IncludeLaunchDescription(
PythonLaunchDescriptionSource([ThisLaunchFileDir(), '/_robot_beside_table_gazebo.launch.py']),
launch_arguments={
'prefix': prefix,
'hw_ns': hw_ns,
'limited': limited,
'effort_control': effort_control,
'velocity_control': velocity_control,
'add_gripper': add_gripper,
'add_vacuum_gripper': add_vacuum_gripper,
'dof': '7',
'robot_type': 'xarm',
'add_realsense_d435i': add_realsense_d435i,
'add_other_geometry': add_other_geometry,
'geometry_type': geometry_type,
'geometry_mass': geometry_mass,
'geometry_height': geometry_height,
'geometry_radius': geometry_radius,
'geometry_length': geometry_length,
'geometry_width': geometry_width,
'geometry_mesh_filename': geometry_mesh_filename,
'geometry_mesh_origin_xyz': geometry_mesh_origin_xyz,
'geometry_mesh_origin_rpy': geometry_mesh_origin_rpy,
'geometry_mesh_tcp_xyz': geometry_mesh_tcp_xyz,
'geometry_mesh_tcp_rpy': geometry_mesh_tcp_rpy,
}.items(),
)
return LaunchDescription([
robot_gazobo_launch
])

31
src/custom_xarm_gazebo/package.xml Normal file
View File

@@ -0,0 +1,31 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>custom_xarm_gazebo</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="a@a.com"></maintainer>
<license>TODO: License declaration</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>rclcpp</depend>
<depend>control_toolbox</depend>
<depend>controller_manager</depend>
<depend>gazebo_ros</depend>
<depend>xarm_description</depend>
<depend>xarm_controller</depend>
<exec_depend>launch</exec_depend>
<exec_depend>launch_ros</exec_depend>
<exec_depend>gazebo_plugins</exec_depend>
<exec_depend>gazebo_ros2_control</exec_depend>
<exec_depend>robot_state_publisher</exec_depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

166
src/custom_xarm_gazebo/src/mimic_joint_plugin.cpp Normal file
View File

@@ -0,0 +1,166 @@
/*
Copyright (c) 2014, Konstantinos Chatzilygeroudis
All rights reserved.
Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer
in the documentation and/or other materials provided with the distribution.
3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived
from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING,
BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
Refer to: https://github.com/roboticsgroup/roboticsgroup_upatras_gazebo_plugins
Modified for ROS2 compatibility and UFACTORY xArm gripper simulation
Modified by: Vinman <vinman.cub@gmail.com>
============================================================================*/
#include "xarm_gazebo/mimic_joint_plugin.h"
namespace gazebo {
GazeboMimicJointPlugin::GazeboMimicJointPlugin()
{
joint_.reset();
mimic_joint_.reset();
}
GazeboMimicJointPlugin::~GazeboMimicJointPlugin()
{
update_connection_.reset();
}
void GazeboMimicJointPlugin::Load(gazebo::physics::ModelPtr parent, sdf::ElementPtr sdf)
{
model_ = parent;
model_nh_ = gazebo_ros::Node::Get(sdf);
RCLCPP_INFO(
model_nh_->get_logger(), "Starting GazeboMimicJointPlugin in namespace: %s, name: %s",
model_nh_->get_namespace(), model_nh_->get_name());
// Error message if the model couldn't be found
if (!model_) {
RCLCPP_ERROR_STREAM(model_nh_->get_logger(), "parent model is NULL");
return;
}
// Check that ROS has been initialized
if (!rclcpp::ok()) {
RCLCPP_FATAL_STREAM(
model_nh_->get_logger(),
"A ROS node for Gazebo has not been initialized, unable to load plugin. " <<
"Load the Gazebo system plugin 'libgazebo_ros2_mimic_joint_plugin.so' in the gazebo_ros package)");
return;
}
// Check for joint element
if (!sdf->HasElement("joint")) {
RCLCPP_ERROR(model_nh_->get_logger(), "No joint element present. GazeboMimicJointPlugin could not be loaded.");
return;
}
joint_name_ = sdf->GetElement("joint")->Get<std::string>();
// Check for mimicJoint element
if (!sdf->HasElement("mimicJoint")) {
RCLCPP_ERROR(model_nh_->get_logger(), "No mimicJoint element present. GazeboMimicJointPlugin could not be loaded.");
return;
}
mimic_joint_name_ = sdf->GetElement("mimicJoint")->Get<std::string>();
// Check if PID controller wanted
has_pid_ = sdf->HasElement("hasPID");
if (has_pid_) {
const std::string prefix = "gains." + joint_name_;
const auto k_p = model_nh_->declare_parameter<double>(prefix + ".p", 10.0);
const auto k_i = model_nh_->declare_parameter<double>(prefix + ".i", 0.1);
const auto k_d = model_nh_->declare_parameter<double>(prefix + ".d", 0.0);
const auto i_clamp = model_nh_->declare_parameter<double>(prefix + ".i_clamp", 0.2);
// Initialize PID
pid_ = std::make_shared<control_toolbox::Pid>(k_p, k_i, k_d, i_clamp, -i_clamp);
}
// Check for multiplier element
multiplier_ = 1.0;
if (sdf->HasElement("multiplier"))
multiplier_ = sdf->GetElement("multiplier")->Get<double>();
// Check for offset element
offset_ = 0.0;
if (sdf->HasElement("offset"))
offset_ = sdf->GetElement("offset")->Get<double>();
// Check for sensitiveness element
sensitiveness_ = 0.0;
if (sdf->HasElement("sensitiveness"))
sensitiveness_ = sdf->GetElement("sensitiveness")->Get<double>();
// Get pointers to joints
joint_ = model_->GetJoint(joint_name_);
if (!joint_) {
RCLCPP_ERROR(model_nh_->get_logger(), "No joint named \"%s\". GazeboMimicJointPlugin could not be loaded.", joint_name_.c_str());
return;
}
mimic_joint_ = model_->GetJoint(mimic_joint_name_);
if (!mimic_joint_) {
RCLCPP_ERROR(model_nh_->get_logger(), "No (mimic) joint named \"%s\". GazeboMimicJointPlugin could not be loaded.", mimic_joint_name_.c_str());
return;
}
// Check for max effort
if (sdf->HasElement("maxEffort")) {
max_effort_ = sdf->GetElement("maxEffort")->Get<double>();
}
else {
max_effort_ = mimic_joint_->GetEffortLimit(0);
}
// Set max effort
if (!has_pid_) {
mimic_joint_->SetParam("fmax", 0, max_effort_);
}
loop_rate_ = rclcpp::Rate::make_shared(1.0 / model_->GetWorld()->Physics()->GetMaxStepSize());
// Listen to the update event. This event is broadcast every
// simulation iteration.
update_connection_ = event::Events::ConnectWorldUpdateBegin(
boost::bind(&GazeboMimicJointPlugin::OnUpdate, this));
// Output some confirmation
RCLCPP_INFO_STREAM(model_nh_->get_logger(), "GazeboMimicJointPlugin loaded! Joint: \"" << joint_name_ << "\", Mimic joint: \"" << mimic_joint_name_ << "\""
<< ", Multiplier: " << multiplier_ << ", Offset: " << offset_
<< ", MaxEffort: " << max_effort_ << ", Sensitiveness: " << sensitiveness_);
}
void GazeboMimicJointPlugin::OnUpdate()
{
// Set mimic joint's angle based on joint's angle
double angle = joint_->Position(0) * multiplier_ + offset_;
double a = mimic_joint_->Position(0);
if (fabs(angle - a) >= sensitiveness_) {
if (has_pid_) {
double error = angle - a;
double effort = ignition::math::clamp(pid_->computeCommand(error, loop_rate_->period().count()), -max_effort_, max_effort_);
mimic_joint_->SetForce(0, effort);
}
else {
mimic_joint_->SetPosition(0, angle, true);
}
}
}
GZ_REGISTER_MODEL_PLUGIN(GazeboMimicJointPlugin)
} // namespace gazebo

25
src/custom_xarm_gazebo/worlds/table.world Executable file
View File

@@ -0,0 +1,25 @@
<?xml version="1.0" ?>
<sdf version="1.4">
<world name="default">
<include>
<uri>model://ground_plane</uri>
</include>
<include>
<uri>model://sun</uri>
</include>
<include>
<uri>model://table</uri>
<name>table</name>
<pose>0.0 -0.84 0 0 0 0</pose>
</include>
<physics type="ode">
<max_step_size>0.001</max_step_size>
<real_time_factor>1</real_time_factor>
<real_time_update_rate>1000</real_time_update_rate>
<!-- (2022-12-20) humble: change gravity from [0 0 -9.81] to [0 0 0] -->
<gravity>0 0 0</gravity>
</physics>
</world>
</sdf>