diff --git a/ur5_moveit_config/launch/moveit_planning_execution.launch b/ur5_moveit_config/launch/moveit_planning_execution.launch
index 7ae8caed1..a8ba0a3a2 100644
--- a/ur5_moveit_config/launch/moveit_planning_execution.launch
+++ b/ur5_moveit_config/launch/moveit_planning_execution.launch
@@ -1,9 +1,10 @@
+
-
+
diff --git a/ur5_moveit_tutorials/CMakeLists.txt b/ur5_moveit_tutorials/CMakeLists.txt
new file mode 100644
index 000000000..2e199af33
--- /dev/null
+++ b/ur5_moveit_tutorials/CMakeLists.txt
@@ -0,0 +1,16 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(ur5_moveit_tutorials)
+
+find_package(catkin REQUIRED)
+
+catkin_package(
+)
+
+install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+install(DIRECTORY config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+
+install(PROGRAMS
+ scripts/joint_group_pos_ctrl_wrapper.py
+ scripts/keyboard_teleop.py
+ DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
\ No newline at end of file
diff --git a/ur5_moveit_tutorials/config/joint_group_pos_servo_params.yaml b/ur5_moveit_tutorials/config/joint_group_pos_servo_params.yaml
new file mode 100644
index 000000000..dac5a5c24
--- /dev/null
+++ b/ur5_moveit_tutorials/config/joint_group_pos_servo_params.yaml
@@ -0,0 +1,5 @@
+command_out_type: std_msgs/Float64MultiArray
+
+publish_joint_positions: true
+publish_joint_velocities: false
+command_out_topic: /joint_group_pos_controller/command # Publish outgoing commands here
\ No newline at end of file
diff --git a/ur5_moveit_tutorials/config/joint_group_vel_servo_params.yaml b/ur5_moveit_tutorials/config/joint_group_vel_servo_params.yaml
new file mode 100644
index 000000000..ef655e2fc
--- /dev/null
+++ b/ur5_moveit_tutorials/config/joint_group_vel_servo_params.yaml
@@ -0,0 +1,5 @@
+command_out_type: std_msgs/Float64MultiArray
+
+publish_joint_positions: false
+publish_joint_velocities: true
+command_out_topic: /joint_group_vel_controller/command # Publish outgoing commands here
\ No newline at end of file
diff --git a/ur5_moveit_tutorials/config/joy_teleop_config.yaml b/ur5_moveit_tutorials/config/joy_teleop_config.yaml
new file mode 100644
index 000000000..803739b4c
--- /dev/null
+++ b/ur5_moveit_tutorials/config/joy_teleop_config.yaml
@@ -0,0 +1,36 @@
+teleop:
+ cartesian_twist_command:
+ type: topic
+ message_type: geometry_msgs/TwistStamped
+ topic_name: /servo_server/delta_twist_cmds
+ axis_mappings:
+ -
+ axis: 0
+ target: twist.linear.x
+ scale: 1
+ offset: 0
+ -
+ axis: 1
+ target: twist.linear.y
+ scale: 1
+ offset: 0
+ -
+ axis: 7
+ target: twist.linear.z
+ scale: 1
+ offset: 0
+ -
+ axis: 3
+ target: twist.angular.x
+ scale: 1
+ offset: 0
+ -
+ axis: 4
+ target: twist.angular.y
+ scale: 1
+ offset: 0
+ -
+ axis: 6
+ target: twist.angular.z
+ scale: 1
+ offset: 0
diff --git a/ur5_moveit_tutorials/config/moveit_servo.rviz b/ur5_moveit_tutorials/config/moveit_servo.rviz
new file mode 100644
index 000000000..1e2b7228c
--- /dev/null
+++ b/ur5_moveit_tutorials/config/moveit_servo.rviz
@@ -0,0 +1,267 @@
+Panels:
+ - Class: rviz/Displays
+ Help Height: 0
+ Name: Displays
+ Property Tree Widget:
+ Expanded:
+ - /TF1
+ - /TF1/Frames1
+ - /TwistStamped1
+ - /RobotModel1
+ Splitter Ratio: 0.5
+ Tree Height: 865
+ - Class: rviz/Selection
+ Name: Selection
+ - Class: rviz/Tool Properties
+ Expanded:
+ - /2D Pose Estimate1
+ - /2D Nav Goal1
+ - /Publish Point1
+ Name: Tool Properties
+ Splitter Ratio: 0.5886790156364441
+ - Class: rviz/Views
+ Expanded:
+ - /Current View1
+ Name: Views
+ Splitter Ratio: 0.5
+ - Class: rviz/Time
+ Experimental: false
+ Name: Time
+ SyncMode: 0
+ SyncSource: ""
+Preferences:
+ PromptSaveOnExit: true
+Toolbars:
+ toolButtonStyle: 2
+Visualization Manager:
+ Class: ""
+ Displays:
+ - Alpha: 0.5
+ Cell Size: 1
+ Class: rviz/Grid
+ Color: 160; 160; 164
+ Enabled: true
+ Line Style:
+ Line Width: 0.029999999329447746
+ Value: Lines
+ Name: Grid
+ Normal Cell Count: 0
+ Offset:
+ X: 0
+ Y: 0
+ Z: 0
+ Plane: XY
+ Plane Cell Count: 10
+ Reference Frame:
+ Value: true
+ - Class: rviz/TF
+ Enabled: true
+ Frame Timeout: 15
+ Frames:
+ All Enabled: false
+ base:
+ Value: false
+ base_link:
+ Value: true
+ base_link_inertia:
+ Value: false
+ flange:
+ Value: false
+ forearm_link:
+ Value: false
+ shoulder_link:
+ Value: false
+ tool0:
+ Value: true
+ tool0_controller:
+ Value: false
+ upper_arm_link:
+ Value: false
+ world:
+ Value: false
+ wrist_1_link:
+ Value: false
+ wrist_2_link:
+ Value: false
+ wrist_3_link:
+ Value: false
+ Marker Alpha: 1
+ Marker Scale: 0.5
+ Name: TF
+ Show Arrows: false
+ Show Axes: true
+ Show Names: true
+ Tree:
+ world:
+ base_link:
+ base:
+ {}
+ base_link_inertia:
+ shoulder_link:
+ upper_arm_link:
+ forearm_link:
+ wrist_1_link:
+ wrist_2_link:
+ wrist_3_link:
+ flange:
+ tool0:
+ tool0_controller:
+ {}
+ Update Interval: 0
+ Value: true
+ - Alpha: 1
+ Angular Arrow Scale: 1
+ Angular Color: 204; 204; 51
+ Arrow Width: 0.10000000149011612
+ Class: rviz/TwistStamped
+ Enabled: true
+ Hide Small Values: false
+ History Length: 10
+ Linear Arrow Scale: 2
+ Linear Color: 173; 127; 168
+ Name: TwistStamped
+ Queue Size: 10
+ Topic: /servo_server/delta_twist_cmds
+ Unreliable: false
+ Value: true
+ - Alpha: 0.30000001192092896
+ Class: rviz/RobotModel
+ Collision Enabled: false
+ Enabled: true
+ Links:
+ All Links Enabled: true
+ Expand Joint Details: false
+ Expand Link Details: false
+ Expand Tree: false
+ Link Tree Style: Links in Alphabetic Order
+ base:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ base_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ base_link_inertia:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ flange:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ forearm_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ shoulder_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ tool0:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ tool0_controller:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ upper_arm_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ world:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ wrist_1_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ wrist_2_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ wrist_3_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ Name: RobotModel
+ Robot Description: robot_description
+ TF Prefix: ""
+ Update Interval: 0
+ Value: true
+ Visual Enabled: true
+ Enabled: true
+ Global Options:
+ Background Color: 48; 48; 48
+ Default Light: true
+ Fixed Frame: world
+ Frame Rate: 30
+ Name: root
+ Tools:
+ - Class: rviz/Interact
+ Hide Inactive Objects: true
+ - Class: rviz/MoveCamera
+ - Class: rviz/Select
+ - Class: rviz/FocusCamera
+ - Class: rviz/Measure
+ - Class: rviz/SetInitialPose
+ Theta std deviation: 0.2617993950843811
+ Topic: /initialpose
+ X std deviation: 0.5
+ Y std deviation: 0.5
+ - Class: rviz/SetGoal
+ Topic: /move_base_simple/goal
+ - Class: rviz/PublishPoint
+ Single click: true
+ Topic: /clicked_point
+ Value: true
+ Views:
+ Current:
+ Class: rviz/Orbit
+ Distance: 1.2807179689407349
+ Enable Stereo Rendering:
+ Stereo Eye Separation: 0.05999999865889549
+ Stereo Focal Distance: 1
+ Swap Stereo Eyes: false
+ Value: false
+ Field of View: 0.7853981852531433
+ Focal Point:
+ X: -0.29767945408821106
+ Y: 0.5415677428245544
+ Z: 0.4893118143081665
+ Focal Shape Fixed Size: true
+ Focal Shape Size: 0.05000000074505806
+ Invert Z Axis: false
+ Name: Current View
+ Near Clip Distance: 0.009999999776482582
+ Pitch: 0.6603972911834717
+ Target Frame:
+ Yaw: 2.030400276184082
+ Saved: ~
+Window Geometry:
+ Displays:
+ collapsed: false
+ Height: 1016
+ Hide Left Dock: false
+ Hide Right Dock: true
+ QMainWindow State: 000000ff00000000fd00000004000000000000027e0000039efc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000039e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e006701000001fc000001cb0000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006700000001e1000001fa0000000000000000000000010000010f000002affc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002af000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650000000000000004b0000004f300fffffffb0000000800540069006d00650100000000000004500000000000000000000004b40000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+ Selection:
+ collapsed: false
+ Time:
+ collapsed: false
+ Tool Properties:
+ collapsed: false
+ Views:
+ collapsed: true
+ Width: 1848
+ X: 72
+ Y: 27
diff --git a/ur5_moveit_tutorials/config/pos_joint_traj_servo_params.yaml b/ur5_moveit_tutorials/config/pos_joint_traj_servo_params.yaml
new file mode 100644
index 000000000..91269e658
--- /dev/null
+++ b/ur5_moveit_tutorials/config/pos_joint_traj_servo_params.yaml
@@ -0,0 +1,5 @@
+command_out_type: trajectory_msgs/JointTrajectory
+
+publish_joint_positions: true
+publish_joint_velocities: false
+command_out_topic: /pos_joint_traj_controller/command
\ No newline at end of file
diff --git a/ur5_moveit_tutorials/config/servo_params_common.yaml b/ur5_moveit_tutorials/config/servo_params_common.yaml
new file mode 100644
index 000000000..828babdf1
--- /dev/null
+++ b/ur5_moveit_tutorials/config/servo_params_common.yaml
@@ -0,0 +1,59 @@
+###############################################
+# Modify all parameters related to servoing here
+###############################################
+use_gazebo: true # Whether the robot is started in a Gazebo simulation environment
+
+## Properties of incoming commands
+command_in_type: "speed_units" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s
+scale:
+ # Scale parameters are only used if command_in_type=="unitless"
+ linear: 0.6 # Max linear velocity. Meters per publish_period. Unit is [m/s]. Only used for Cartesian commands.
+ rotational: 0.3 # Max angular velocity. Rads per publish_period. Unit is [rad/s]. Only used for Cartesian commands.
+ # Max joint angular/linear velocity. Rads or Meters per publish period. Only used for joint commands on joint_command_in_topic.
+ joint: 0.01
+low_pass_filter_coeff: 2. # Larger --> trust the filtered data more, trust the measurements less.
+
+## Properties of outgoing commands
+publish_period: 0.008 # 1/Nominal publish rate [seconds]
+low_latency_mode: false # Set this to true to publish as soon as an incoming Twist command is received (publish_period is ignored)
+
+publish_joint_accelerations: false
+
+## MoveIt properties
+move_group_name: manipulator # Often 'manipulator' or 'arm'
+planning_frame: world # The MoveIt planning frame. Often 'base_link' or 'world'
+
+## Other frames
+ee_frame_name: tool0 # The name of the end effector link, used to return the EE pose
+robot_link_command_frame: world # commands must be given in the frame of a robot link. Usually either the base or end effector
+
+## Stopping behaviour
+incoming_command_timeout: 0.5 # Stop servoing if X seconds elapse without a new command
+# If 0, republish commands forever even if the robot is stationary. Otherwise, specify num. to publish.
+# Important because ROS may drop some messages and we need the robot to halt reliably.
+num_outgoing_halt_msgs_to_publish: 4
+
+## Configure handling of singularities and joint limits
+lower_singularity_threshold: 17 # Start decelerating when the condition number hits this (close to singularity)
+hard_stop_singularity_threshold: 30 # Stop when the condition number hits this
+joint_limit_margin: 0.1 # added as a buffer to joint limits [radians]. If moving quickly, make this larger.
+
+## Topic names
+cartesian_command_in_topic: delta_twist_cmds # Topic for incoming Cartesian twist commands
+joint_command_in_topic: delta_joint_cmds # Topic for incoming joint angle commands
+joint_topic: joint_states
+status_topic: status # Publish status to this topic
+
+## Collision checking for the entire robot body
+check_collisions: true # Check collisions?
+collision_check_rate: 50 # [Hz] Collision-checking can easily bog down a CPU if done too often.
+# Two collision check algorithms are available:
+# "threshold_distance" begins slowing down when nearer than a specified distance. Good if you want to tune collision thresholds manually.
+# "stop_distance" stops if a collision is nearer than the worst-case stopping distance and the distance is decreasing. Requires joint acceleration limits
+collision_check_type: threshold_distance
+# Parameters for "threshold_distance"-type collision checking
+self_collision_proximity_threshold: 0.01 # Start decelerating when a self-collision is this far [m]
+scene_collision_proximity_threshold: 0.05 # Start decelerating when a scene collision is this far [m]
+# Parameters for "stop_distance"-type collision checking
+collision_distance_safety_factor: 1000 # Must be >= 1. A large safety factor is recommended to account for latency
+min_allowable_collision_distance: 0.01 # Stop if a collision is closer than this [m]
diff --git a/ur5_moveit_tutorials/launch/ur5_moveit_servo_common.launch b/ur5_moveit_tutorials/launch/ur5_moveit_servo_common.launch
new file mode 100644
index 000000000..a5c602f31
--- /dev/null
+++ b/ur5_moveit_tutorials/launch/ur5_moveit_servo_common.launch
@@ -0,0 +1,30 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/ur5_moveit_tutorials/launch/ur5_moveit_servo_w_joint_group_pos_controller.launch b/ur5_moveit_tutorials/launch/ur5_moveit_servo_w_joint_group_pos_controller.launch
new file mode 100644
index 000000000..0ac670e48
--- /dev/null
+++ b/ur5_moveit_tutorials/launch/ur5_moveit_servo_w_joint_group_pos_controller.launch
@@ -0,0 +1,7 @@
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/ur5_moveit_tutorials/launch/ur5_moveit_servo_w_joint_group_vel_controller.launch b/ur5_moveit_tutorials/launch/ur5_moveit_servo_w_joint_group_vel_controller.launch
new file mode 100644
index 000000000..10c97baa0
--- /dev/null
+++ b/ur5_moveit_tutorials/launch/ur5_moveit_servo_w_joint_group_vel_controller.launch
@@ -0,0 +1,8 @@
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/ur5_moveit_tutorials/launch/ur5_moveit_servo_w_joy.launch b/ur5_moveit_tutorials/launch/ur5_moveit_servo_w_joy.launch
new file mode 100644
index 000000000..faf84d665
--- /dev/null
+++ b/ur5_moveit_tutorials/launch/ur5_moveit_servo_w_joy.launch
@@ -0,0 +1,8 @@
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/ur5_moveit_tutorials/launch/ur5_moveit_servo_w_traj_controller.launch b/ur5_moveit_tutorials/launch/ur5_moveit_servo_w_traj_controller.launch
new file mode 100644
index 000000000..a0703bf75
--- /dev/null
+++ b/ur5_moveit_tutorials/launch/ur5_moveit_servo_w_traj_controller.launch
@@ -0,0 +1,7 @@
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/ur5_moveit_tutorials/package.xml b/ur5_moveit_tutorials/package.xml
new file mode 100644
index 000000000..e61c4efc0
--- /dev/null
+++ b/ur5_moveit_tutorials/package.xml
@@ -0,0 +1,23 @@
+
+
+ ur5_moveit_tutorials
+ 1.2.7
+ The package that contains MoveIt tutorials with UR5
+
+ Tahsincan Köse
+ G.A. vd. Hoorn
+ Miguel Prada Sarasola
+ Nadia Hammoudeh Garcia
+
+ BSD
+
+ catkin
+ ur_gazebo
+ ur5_moveit_config
+ moveit_commander
+ moveit_servo
+ joy
+
+
+
+
diff --git a/ur5_moveit_tutorials/scripts/keyboard_teleop.py b/ur5_moveit_tutorials/scripts/keyboard_teleop.py
new file mode 100755
index 000000000..3c90bcc15
--- /dev/null
+++ b/ur5_moveit_tutorials/scripts/keyboard_teleop.py
@@ -0,0 +1,126 @@
+#!/usr/bin/env python
+
+import rospy
+from geometry_msgs.msg import TwistStamped
+import sys, select, termios, tty
+import threading
+import moveit_commander
+
+GROUP_NAME = "manipulator"
+
+def get_end_effector_name():
+ moveit_commander.roscpp_initialize(sys.argv)
+ group = moveit_commander.MoveGroupCommander(GROUP_NAME)
+ end_effector_link = group.get_end_effector_link()
+ rospy.loginfo("End effector link for group '%s': %s", GROUP_NAME, end_effector_link)
+
+ return end_effector_link
+
+class TeleopTwistKeyboard:
+ def __init__(self):
+ self.end_effector_link = get_end_effector_name()
+ # Key mappings for linear and angular velocities
+ self.key_bindings = {
+ 'w': (1, 0, 0, 0, 0, 0), # Move forward in x
+ 's': (-1, 0, 0, 0, 0, 0), # Move backward in x
+ 'a': (0, 1, 0, 0, 0, 0), # Move left in y
+ 'd': (0, -1, 0, 0, 0, 0), # Move right in y
+ 'r': (0, 0, 1, 0, 0, 0), # Move up in z
+ 'f': (0, 0, -1, 0, 0, 0), # Move down in z
+ 'i': (0, 0, 0, 1, 0, 0), # Roll +
+ 'k': (0, 0, 0, -1, 0, 0), # Roll -
+ 'j': (0, 0, 0, 0, 1, 0), # Pitch +
+ 'l': (0, 0, 0, 0, -1, 0), # Pitch -
+ 'u': (0, 0, 0, 0, 0, 1), # Yaw +
+ 'o': (0, 0, 0, 0, 0, -1), # Yaw -
+ 'n': (0, 0, 0, 0, 0, 0) # Reset
+ }
+
+ # Initialize twist message and node parameters
+ self.last_twist = TwistStamped()
+ self.linear_increment = 0.02
+ self.angular_increment = 0.02
+ self.finished_work = False
+
+ self.pub = rospy.Publisher('/servo_server/delta_twist_cmds', TwistStamped, queue_size=10)
+
+ # Terminal settings
+ self.settings = termios.tcgetattr(sys.stdin)
+
+ # Start the publishing thread
+ self.pub_thread = threading.Thread(target=self.publish_twist)
+ self.pub_thread.start()
+
+ def publish_twist(self):
+ rate = rospy.Rate(50) # Publish rate in Hz
+ while not self.finished_work and not rospy.is_shutdown():
+ self.last_twist.header.stamp = rospy.Time.now()
+ self.last_twist.header.frame_id = self.end_effector_link
+ self.pub.publish(self.last_twist)
+ rate.sleep()
+
+ def get_key(self):
+ tty.setraw(sys.stdin.fileno())
+ select.select([sys.stdin], [], [], 0)
+ key = sys.stdin.read(1)
+ termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
+ return key
+
+ def reset_velocities(self):
+ self.last_twist = TwistStamped() # Reset velocities
+
+ def run(self):
+ try:
+ print("""
+ Control the end-effector using keyboard commands:
+ ------------------------------------------------
+ Linear velocity controls:
+ w/s : increase/decrease x-axis velocity
+ a/d : increase/decrease y-axis velocity
+ r/f : increase/decrease z-axis velocity
+
+ Angular velocity controls:
+ i/k : increase/decrease roll (x-axis rotation)
+ j/l : increase/decrease pitch (y-axis rotation)
+ u/o : increase/decrease yaw (z-axis rotation)
+
+ n: Reset all velocities
+
+ Press 'Ctrl+C' to exit.
+ """)
+
+ while not rospy.is_shutdown():
+ key = self.get_key()
+ if key in self.key_bindings:
+ # Get the corresponding linear and angular directions
+ lin_x, lin_y, lin_z, ang_x, ang_y, ang_z = self.key_bindings[key]
+
+ # Update the twist message with incremental values
+ self.last_twist.twist.linear.x += lin_x * self.linear_increment
+ self.last_twist.twist.linear.y += lin_y * self.linear_increment
+ self.last_twist.twist.linear.z += lin_z * self.linear_increment
+ self.last_twist.twist.angular.x += ang_x * self.angular_increment
+ self.last_twist.twist.angular.y += ang_y * self.angular_increment
+ self.last_twist.twist.angular.z += ang_z * self.angular_increment
+
+ if key == 'n':
+ self.reset_velocities()
+ if key == '\x03': # Ctrl+C
+ raise KeyboardInterrupt
+
+ except (rospy.ROSInterruptException, KeyboardInterrupt):
+ rospy.loginfo("Shutting down. Publishing zero velocities to stop the robot.")
+ self.reset_velocities()
+
+ finally:
+ # Reset terminal settings on exit and signal the publishing thread to stop
+ termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
+ self.finished_work = True
+ self.pub_thread.join()
+
+
+if __name__ == "__main__":
+ # Initialize ROS node
+ rospy.init_node('minimal_teleop_twist_keyboard')
+ teleop = TeleopTwistKeyboard()
+ teleop.run()
diff --git a/ur_gazebo/config/ur5_controllers.yaml b/ur_gazebo/config/ur5_controllers.yaml
index bf1ca1b1e..1ca0cd9f1 100644
--- a/ur_gazebo/config/ur5_controllers.yaml
+++ b/ur_gazebo/config/ur5_controllers.yaml
@@ -54,3 +54,7 @@ pos_joint_traj_controller:
joint_group_pos_controller:
type: position_controllers/JointGroupPositionController
joints: *robot_joints
+
+joint_group_vel_controller:
+ type: velocity_controllers/JointGroupVelocityController
+ joints: *robot_joints
\ No newline at end of file
diff --git a/ur_gazebo/launch/inc/ur_control.launch.xml b/ur_gazebo/launch/inc/ur_control.launch.xml
index 3062ed17a..a68b77221 100644
--- a/ur_gazebo/launch/inc/ur_control.launch.xml
+++ b/ur_gazebo/launch/inc/ur_control.launch.xml
@@ -53,12 +53,12 @@
"
output="screen" respawn="false" />
-
-
+
+
-
-
+
+
diff --git a/ur_gazebo/launch/ur5_bringup.launch b/ur_gazebo/launch/ur5_bringup.launch
index 7dc7b7399..482a5fa88 100644
--- a/ur_gazebo/launch/ur5_bringup.launch
+++ b/ur_gazebo/launch/ur5_bringup.launch
@@ -37,7 +37,7 @@
-
+
@@ -50,6 +50,7 @@
+
@@ -73,5 +74,6 @@
+