Skip to content

Commit

Permalink
Created working ur10e_control package to control the UR10e & gripper …
Browse files Browse the repository at this point in the history
…w/ MoveIt! in Gazebo
  • Loading branch information
willnatsan committed Jun 5, 2024
1 parent 8135e46 commit 1840263
Showing 25 changed files with 724 additions and 0 deletions.
44 changes: 44 additions & 0 deletions ur10e_control/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -0,0 +1,44 @@
cmake_minimum_required(VERSION 3.8)
project(ur10e_control)

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(moveit_ros_planning_interface REQUIRED)
find_package(rclcpp REQUIRED)

add_executable(csv_control src/csv_control.cpp)
target_include_directories(csv_control PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>)
target_compile_features(csv_control PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17
ament_target_dependencies(
csv_control
"moveit_ros_planning_interface"
"rclcpp"
)

install (
DIRECTORY launch
DESTINATION share/${PROJECT_NAME}
)

install(TARGETS csv_control
DESTINATION lib/${PROJECT_NAME})

if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
# the following line skips the linter which checks for copyrights
# comment the line when a copyright and license is added to all source files
set(ament_cmake_copyright_FOUND TRUE)
# the following line skips cpplint (only works in a git repo)
# comment the line when this package is in a git repo and when
# a copyright and license is added to all source files
set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
endif()

ament_package()
134 changes: 134 additions & 0 deletions ur10e_control/launch/moveit_control.launch.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,134 @@
import xacro
from os.path import join
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription, RegisterEventHandler
from launch.event_handlers import OnProcessExit
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.actions import Node
from ament_index_python import get_package_share_directory
from moveit_configs_utils import MoveItConfigsBuilder

def generate_launch_description():
# MoveIt! Configuration
moveit_config = (
MoveItConfigsBuilder("ur10e")
.robot_description(file_path="config/ur10e_robotiq.urdf")
.robot_description_semantic(file_path="config/ur10e_robotiq.srdf")
.robot_description_kinematics(file_path="config/kinematics.yaml")
.planning_scene_monitor(publish_robot_description=True, publish_robot_description_semantic=True)
.trajectory_execution(file_path="config/moveit_controllers.yaml")
.planning_pipelines(pipelines=["ompl"])
.to_moveit_configs()
)
moveit_config_dict = moveit_config.to_dict()
moveit_config_dict.update({"use_sim_time": True})

move_group_node = Node(
package="moveit_ros_move_group",
executable="move_group",
output="screen",
parameters=[moveit_config_dict],
arguments=["--ros-args", "--log-level", "info"],
)


# Configure URDF file
urdf_pkg_share = get_package_share_directory('ur10e_description')
urdf_path_local = 'urdf/ur10e_robotiq_2f_85.urdf.xacro'
urdf_path_global = join(urdf_pkg_share, urdf_path_local)
robot_description_raw = xacro.process_file(urdf_path_global).toxml()

# Launch Gazebo
gz_pkg_share = get_package_share_directory('ros_gz_sim')
launch_gazebo = IncludeLaunchDescription(
PythonLaunchDescriptionSource([join(gz_pkg_share, 'launch', 'gz_sim.launch.py')])
)



# Configure nodes to launch
static_tf_node = Node(
package="tf2_ros",
executable="static_transform_publisher",
name="static_transform_publisher",
output="log",
arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "world", "ur10e_base_link"],
)

rsp_node = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
parameters=[
{'robot_description': robot_description_raw,
'use_sim_time': True}
],
output='both',
)

rviz_config_file = join(urdf_pkg_share, 'rviz', 'view_robot.rviz')

rviz_node = Node(
package='rviz2',
executable='rviz2',
output='log',
name='rviz2',
arguments=['-d', rviz_config_file],
parameters=[{'use_sim_time': True}],
)

robot_name = 'ur10e'
create_node = Node(
package='ros_gz_sim',
executable='create',
arguments=[
'-topic', 'robot_description',
'-name', robot_name,
'-world', 'default'
],
parameters=[{'use_sim_time': True}],
output='screen',
)

# Spawn controller nodes

joint_state_broadcaster_spawner = Node(
package="controller_manager",
executable="spawner",
arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"],
)

joint_trajectory_controller_spawner = Node(
package="controller_manager",
executable="spawner",
arguments=["joint_trajectory_controller", "--controller-manager", "/controller_manager"],
)

robotiq_gripper_controller_spawner = Node(
package="controller_manager",
executable="spawner",
arguments=["robotiq_gripper_controller", "--controller-manager", "/controller_manager"],
)

delay_joint_state_broadcaster_spawner = RegisterEventHandler(
event_handler=OnProcessExit(
target_action=create_node,
on_exit=[joint_state_broadcaster_spawner],
)
)

delay_joint_trajectory_controller_spawner = RegisterEventHandler(
event_handler=OnProcessExit(
target_action=joint_state_broadcaster_spawner,
on_exit=[joint_trajectory_controller_spawner, robotiq_gripper_controller_spawner],
)
)

return LaunchDescription([
move_group_node,
rsp_node,
rviz_node,
launch_gazebo,
create_node,
delay_joint_state_broadcaster_spawner,
delay_joint_trajectory_controller_spawner,
])
21 changes: 21 additions & 0 deletions ur10e_control/package.xml
Original file line number Diff line number Diff line change
@@ -0,0 +1,21 @@
<?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>ur10e_control</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="[email protected]">willnatsan</maintainer>
<license>TODO: License declaration</license>

<buildtool_depend>ament_cmake</buildtool_depend>

<depend>moveit_ros_planning_interface</depend>
<depend>rclcpp</depend>

<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>

<export>
<build_type>ament_cmake</build_type>
</export>
</package>
10 changes: 10 additions & 0 deletions ur10e_control/src/csv_control.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,10 @@
#include <cstdio>

int main(int argc, char ** argv)
{
(void) argc;
(void) argv;

printf("hello world ur10e_control package\n");
return 0;
}
25 changes: 25 additions & 0 deletions ur10e_moveit_config/.setup_assistant
Original file line number Diff line number Diff line change
@@ -0,0 +1,25 @@
moveit_setup_assistant_config:
urdf:
package: ur10e_description
relative_path: urdf/ur10e_robotiq_2f_85.urdf
srdf:
relative_path: config/ur10e_robotiq.srdf
package_settings:
author_name: willnatsan
author_email: [email protected]
generated_timestamp: 1717569747
control_xacro:
command:
- position
state:
- position
- velocity
modified_urdf:
xacros:
- control_xacro
control_xacro:
command:
- position
state:
- position
- velocity
11 changes: 11 additions & 0 deletions ur10e_moveit_config/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -0,0 +1,11 @@
cmake_minimum_required(VERSION 3.22)
project(ur10e_moveit_config)

find_package(ament_cmake REQUIRED)

ament_package()

install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}
PATTERN "setup_assistant.launch" EXCLUDE)
install(DIRECTORY config DESTINATION share/${PROJECT_NAME})
install(FILES .setup_assistant DESTINATION share/${PROJECT_NAME})
10 changes: 10 additions & 0 deletions ur10e_moveit_config/config/initial_positions.yaml
Original file line number Diff line number Diff line change
@@ -0,0 +1,10 @@
# Default initial positions for ur10e_robotiq's ros2_control fake system

initial_positions:
robotiq_85_left_knuckle_joint: 0
ur10e_elbow_joint: 0
ur10e_shoulder_lift_joint: 0
ur10e_shoulder_pan_joint: 0
ur10e_wrist_1_joint: 0
ur10e_wrist_2_joint: 0
ur10e_wrist_3_joint: 0
45 changes: 45 additions & 0 deletions ur10e_moveit_config/config/joint_limits.yaml
Original file line number Diff line number Diff line change
@@ -0,0 +1,45 @@
# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed

# For beginners, we downscale velocity and acceleration limits.
# You can always specify higher scaling factors (<= 1.0) in your motion requests. # Increase the values below to 1.0 to always move at maximum speed.
default_velocity_scaling_factor: 0.1
default_acceleration_scaling_factor: 0.1

# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration]
# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits]
joint_limits:
robotiq_85_left_knuckle_joint:
has_velocity_limits: true
max_velocity: 0.5
has_acceleration_limits: false
max_acceleration: 0
ur10e_elbow_joint:
has_velocity_limits: true
max_velocity: 3.1415926535897931
has_acceleration_limits: false
max_acceleration: 0
ur10e_shoulder_lift_joint:
has_velocity_limits: true
max_velocity: 2.0943951023931953
has_acceleration_limits: false
max_acceleration: 0
ur10e_shoulder_pan_joint:
has_velocity_limits: true
max_velocity: 2.0943951023931953
has_acceleration_limits: false
max_acceleration: 0
ur10e_wrist_1_joint:
has_velocity_limits: true
max_velocity: 3.1415926535897931
has_acceleration_limits: false
max_acceleration: 0
ur10e_wrist_2_joint:
has_velocity_limits: true
max_velocity: 3.1415926535897931
has_acceleration_limits: false
max_acceleration: 0
ur10e_wrist_3_joint:
has_velocity_limits: true
max_velocity: 3.1415926535897931
has_acceleration_limits: false
max_acceleration: 0
4 changes: 4 additions & 0 deletions ur10e_moveit_config/config/kinematics.yaml
Original file line number Diff line number Diff line change
@@ -0,0 +1,4 @@
ur10e_arm:
kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
kinematics_solver_search_resolution: 0.0050000000000000001
kinematics_solver_timeout: 0.0050000000000000001
51 changes: 51 additions & 0 deletions ur10e_moveit_config/config/moveit.rviz
Original file line number Diff line number Diff line change
@@ -0,0 +1,51 @@
Panels:
- Class: rviz_common/Displays
Name: Displays
Property Tree Widget:
Expanded:
- /MotionPlanning1
- Class: rviz_common/Help
Name: Help
- Class: rviz_common/Views
Name: Views
Visualization Manager:
Displays:
- Class: rviz_default_plugins/Grid
Name: Grid
Value: true
- Class: moveit_rviz_plugin/MotionPlanning
Name: MotionPlanning
Planned Path:
Loop Animation: true
State Display Time: 0.05 s
Trajectory Topic: display_planned_path
Planning Scene Topic: monitored_planning_scene
Robot Description: robot_description
Scene Geometry:
Scene Alpha: 1
Scene Robot:
Robot Alpha: 0.5
Value: true
Global Options:
Fixed Frame: world
Tools:
- Class: rviz_default_plugins/Interact
- Class: rviz_default_plugins/MoveCamera
- Class: rviz_default_plugins/Select
Value: true
Views:
Current:
Class: rviz_default_plugins/Orbit
Distance: 2.0
Focal Point:
X: -0.1
Y: 0.25
Z: 0.30
Name: Current View
Pitch: 0.5
Target Frame: world
Yaw: -0.623
Window Geometry:
Height: 975
QMainWindow State: 000000ff00000000fd0000000100000000000002b400000375fc0200000005fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100fffffffb000000100044006900730070006c006100790073010000003d00000123000000c900fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670100000166000001910000018800fffffffb0000000800480065006c0070000000029a0000006e0000006e00fffffffb0000000a0056006900650077007301000002fd000000b5000000a400ffffff000001f60000037500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Width: 1200
28 changes: 28 additions & 0 deletions ur10e_moveit_config/config/moveit_controllers.yaml
Original file line number Diff line number Diff line change
@@ -0,0 +1,28 @@
# MoveIt uses this configuration for controller management

moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager

moveit_simple_controller_manager:
controller_names:
- joint_trajectory_controller
- robotiq_gripper_controller

joint_trajectory_controller:
type: FollowJointTrajectory
action_ns: follow_joint_trajectory
default: true
joints:
- ur10e_shoulder_pan_joint
- ur10e_shoulder_lift_joint
- ur10e_elbow_joint
- ur10e_wrist_1_joint
- ur10e_wrist_2_joint
- ur10e_wrist_3_joint
action_ns: follow_joint_trajectory
default: true
robotiq_gripper_controller:
type: GripperCommand
joints:
- robotiq_85_left_knuckle_joint
action_ns: gripper_cmd
default: true
Loading

0 comments on commit 1840263

Please sign in to comment.