def main(self):
# Initialize ROS Node
rospy.init_node("stretch_force_sensors")
self.node_name = rospy.get_name()
rospy.loginfo("For use with S T R E T C H (TM) RESEARCH EDITION from Hello Robot Inc.")
rospy.loginfo("{:s} started".format(self.node_name))
self.robot = rb.Robot()
self.robot.startup()
# Get the rate for publishing the force sensor data
self.force_sensor_rate = rospy.get_param("~rate", default=15.0)
self.force_sensor_rate = rospy.Rate(self.force_sensor_rate)
#self.force_sensor_frame_id = ""
self.force_pub = rospy.Publisher('force_sensors', WrenchStamped, queue_size=1)
rospy.loginfo("{0} rate = {1} Hz".format(self.node_name, self.force_sensor_rate))
try:
# Publish force sensor readings until ROS is shutdown
while not rospy.is_shutdown():
self.get_forces_and_publish()
self.force_sensor_rate.sleep()
except (rospy.ROSInterruptException, ThreadServiceExit):
self.robot.stop()
rospy.signal_shutdown("stretch_force_sensors shutdown")
def test_robot_loops(self):
print('Starting test_robot_loops')
r = robot.Robot()
r.startup()
time.sleep(3.0)
r.non_dxl_thread.stats.pretty_print()
r.dxl_thread.stats.pretty_print()
r.stop()
self.assertTrue(r.dxl_thread.stats.status['loop_warns'] == 0)
self.assertTrue(r.non_dxl_thread.stats.status['loop_warns'] == 0)
def main():
global use_head_mapping, use_dex_wrist_mapping, use_stretch_gripper_mapping
xbox_controller = xc.XboxController()
xbox_controller.start()
robot = rb.Robot()
try:
robot.startup()
print('Using key mapping for tool: %s' % robot.end_of_arm.name)
if robot.end_of_arm.name == 'tool_none':
use_head_mapping = True
use_stretch_gripper_mapping = False
use_dex_wrist_mapping = False
if robot.end_of_arm.name == 'tool_stretch_gripper':
use_head_mapping = True
use_stretch_gripper_mapping = True
use_dex_wrist_mapping = False
if robot.end_of_arm.name == 'tool_stretch_dex_wrist':
use_head_mapping = False
use_stretch_gripper_mapping = True
use_dex_wrist_mapping = True
robot.pimu.trigger_beep()
robot.push_command()
time.sleep(0.5)
robot.pimu.trigger_beep()
robot.push_command()
time.sleep(0.5)
while True:
controller_state = xbox_controller.get_state()
if not robot.is_calibrated():
manage_calibration(robot, controller_state)
else:
manage_base(robot, controller_state)
manage_lift_arm(robot, controller_state)
manage_end_of_arm(robot, controller_state)
manage_head(robot, controller_state)
manage_stow(robot, controller_state)
manage_shutdown(robot, controller_state)
robot.push_command()
time.sleep(0.05)
except (ThreadServiceExit, KeyboardInterrupt, SystemExit):
robot.stop()
xbox_controller.stop()
#!/usr/bin/env python import stretch_body.robot as rb import stretch_body.hello_utils as hu hu.print_stretch_re_use() robot = rb.Robot() robot.startup() robot.stow() robot.stop()
#!/usr/bin/env python
from __future__ import print_function
import sys, tty, termios
import stretch_body.robot as hello_robot
from stretch_body.hello_utils import *
import argparse
print_stretch_re_use()
parser = argparse.ArgumentParser(
description=
'Control the robot base, lift, arm, head, and tool from the keyboard')
args = parser.parse_args()
robot = hello_robot.Robot()
robot.startup()
small_move_m = .01
large_move_m = 0.1
small_rotate_rad = deg_to_rad(1.0)
large_rotate_rad = deg_to_rad(10.0)
small_lift_move_m = .01
large_lift_move_m = .075
small_arm_move_m = .010
large_arm_move_m = .075
gg = 0.5
def get_keystroke():
#!/usr/bin/env python
from __future__ import print_function
import stretch_body.robot as robot
import argparse
import stretch_body.hello_utils as hu
hu.print_stretch_re_use()
parser = argparse.ArgumentParser(
description='Calibrate position for all robot joints')
args = parser.parse_args()
r = robot.Robot()
r.startup()
r.home()
r.stop()
def main():
##################
# Setup Xbox controller
xbox_controller = xc.XboxController()
xbox_controller.start()
############################
# Regular Motion
dead_zone = 0.1 # 0.25 #0.1 #0.2 #0.3 #0.4
move_s = 0.6
max_dist_m = 0.06 # 0.04 #0.05
accel_m = 0.2 # 0.1
command_to_linear_motion = CommandToLinearMotion(dead_zone, move_s,
max_dist_m, accel_m)
move_s = 0.05
max_dist_rad = 0.10 # 0.2 #0.25 #0.1 #0.09
accel_rad = 0.8 # 0.05
command_to_rotary_motion = CommandToRotaryMotion(dead_zone, move_s,
max_dist_rad, accel_rad)
############################
############################
# Fast motion for navigation with the mobile base
fast_move_s = 0.6
fast_max_dist_m = 0.12
fast_accel_m = 0.8
# fast, but unstable on thresholds: 0.6 s, 0.15 m, 0.8 m/s^2
fast_command_to_linear_motion = CommandToLinearMotion(
dead_zone, fast_move_s, fast_max_dist_m, fast_accel_m)
fast_move_s = 0.2
fast_max_dist_rad = 0.6
fast_accel_rad = 0.8
fast_command_to_rotary_motion = CommandToRotaryMotion(
dead_zone, fast_move_s, fast_max_dist_rad, fast_accel_rad)
############################
head_stopped = False
stop_loop = False
robot = rb.Robot()
robot.startup()
robot_calibrated = robot.is_calibrated()
robot.pimu.trigger_beep()
robot.push_command()
time.sleep(0.5)
robot.pimu.trigger_beep()
robot.push_command()
time.sleep(0.5)
last_time = time.time()
t_last = time.time()
itr = 0
shutdown_pc = False
ts_shutdown_start = 0
first_home_warn = True
wrist_yaw_target = 0.0
head_pan_target = 0.0
head_tilt_target = 0.0
while not stop_loop:
try:
##################
#xbox_controller.update()
# get latest controller state
controller_state = xbox_controller.get_state()
##################
# convert controller state to new robot commands
calibrate_the_robot = controller_state['start_button_pressed']
if controller_state['select_button_pressed']:
if not shutdown_pc:
ts_shutdown_start = time.time()
shutdown_pc = True
if time.time() - ts_shutdown_start > 2.0:
robot.pimu.trigger_beep()
robot.stow()
robot.stop()
time.sleep(1.0)
os.system(
'paplay --device=alsa_output.pci-0000_00_1f.3.analog-stereo /usr/share/sounds/ubuntu/stereo/desktop-logout.ogg'
)
os.system(
'sudo shutdown now'
) #sudoers should be set up to not need a password
else:
shutdown_pc = False
#run_stop = controller_state['middle_led_ring_button_pressed']
#run_stop = controller_state['select_button_pressed']
#if run_stop:
# print('runstop event trigger not yet implemented')
# robot.pimu.runstop_event_trigger()
forward_command = controller_state['left_stick_y']
turn_command = controller_state['left_stick_x']
lift_command = controller_state['right_stick_y']
arm_command = controller_state['right_stick_x']
wrist_yaw_left = controller_state['left_shoulder_button_pressed']
wrist_yaw_right = controller_state['right_shoulder_button_pressed']
# controller_state['right_trigger_pulled']
close_gripper = controller_state['bottom_button_pressed']
open_gripper = controller_state['right_button_pressed']
stow_robot = controller_state['top_button_pressed']
#turn_on_the_relay = controller_state['left_button_pressed']
camera_pan_right = controller_state['right_pad_pressed']
camera_pan_left = controller_state['left_pad_pressed']
camera_tilt_up = controller_state['top_pad_pressed']
camera_tilt_down = controller_state['bottom_pad_pressed']
lift_height_m = robot.lift.status['pos']
arm_extension_m = robot.arm.status['pos']
if robot.end_of_arm is not None:
wrist_yaw_rad = robot.end_of_arm.motors['wrist_yaw'].status[
'pos']
else:
wrist_yaw_rad = 0
print_joints = False
if print_joints:
print('lift_height_m =', lift_height_m)
print('arm_extension_m =', arm_extension_m)
print('wrist_yaw_rad =', wrist_yaw_rad)
# FAST NAVIGATION MODE
fast_navigation_mode = False
navigation_mode_trigger = controller_state['right_trigger_pulled']
if (navigation_mode_trigger > 0.5):
fast_navigation_mode = True
##################
# convert robot commands to robot movement
# only allow a pure translation or a pure rotation command
if abs(forward_command) > abs(turn_command):
if abs(forward_command) > dead_zone:
output_sign = math.copysign(1, forward_command)
if not fast_navigation_mode:
d_m, v_m, a_m = command_to_linear_motion.get_dist_vel_accel(
output_sign, forward_command)
else:
d_m, v_m, a_m = fast_command_to_linear_motion.get_dist_vel_accel(
output_sign, forward_command)
#print('Translate %f %.2f %.2f %.2f'%(time.time(),d_m,v_m,a_m))
tt = time.time()
#print('DT %f',tt-last_time)
last_time = tt
robot.base.translate_by(d_m, v_m, a_m)
else:
if abs(turn_command) > dead_zone:
output_sign = -math.copysign(1, turn_command)
if not fast_navigation_mode:
d_rad, v_rad, a_rad = command_to_rotary_motion.get_dist_vel_accel(
output_sign, turn_command)
else:
d_rad, v_rad, a_rad = fast_command_to_rotary_motion.get_dist_vel_accel(
output_sign, turn_command)
#print('DR', d_rad,'J',turn_command)
robot.base.rotate_by(d_rad, v_rad, a_rad)
# if (abs(forward_command) < dead_zone) and (abs(turn_command) < dead_zone):
# robot.base.translate_by(0.0)
# robot.base.rotate_by(0.0)
if abs(lift_command) > dead_zone:
output_sign = math.copysign(1, lift_command)
d_m, v_m, a_m = command_to_linear_motion.get_dist_vel_accel(
output_sign, lift_command)
robot.lift.move_by(d_m, v_m, a_m)
if abs(arm_command) > dead_zone:
output_sign = math.copysign(1, arm_command)
d_m, v_m, a_m = command_to_linear_motion.get_dist_vel_accel(
output_sign, arm_command)
robot.arm.move_by(d_m, v_m, a_m)
if robot.end_of_arm is not None:
wrist_rotate_rad = deg_to_rad(25.0) # 60.0 #10.0 #5.0
wrist_accel = 15.0 # 25.0 #30.0 #15.0 #8.0 #15.0
wrist_vel = 25.0 # 10.0 #6.0 #3.0 #6.0
wrist_slew_down = 0.15
wrist_slew_up = .07
#Slew target to zero if no buttons pushed
if not wrist_yaw_left and not wrist_yaw_right:
if wrist_yaw_target >= 0:
wrist_yaw_target = max(
0, wrist_yaw_target - wrist_slew_down)
else:
wrist_yaw_target = min(
0, wrist_yaw_target + wrist_slew_down)
#Or slew up to max
if wrist_yaw_left:
wrist_yaw_target = max(wrist_yaw_target - wrist_slew_up,
-wrist_rotate_rad)
if wrist_yaw_right:
wrist_yaw_target = min(wrist_yaw_target + wrist_slew_up,
wrist_rotate_rad)
robot.end_of_arm.move_by('wrist_yaw', wrist_yaw_target,
wrist_vel, wrist_accel)
head_pan_rad = deg_to_rad(40.0) # 25.0 #40.0
head_pan_accel = 14.0 # 15.0
head_pan_vel = 7.0 # 6.0
head_pan_slew_down = 0.15
head_pan_slew_up = .15
# Slew target to zero if no buttons pushed
if not camera_pan_left and not camera_pan_right:
if head_pan_target >= 0:
head_pan_target = max(0,
head_pan_target - head_pan_slew_down)
else:
head_pan_target = min(0,
head_pan_target + head_pan_slew_down)
# Or slew up to max
if camera_pan_right:
head_pan_target = max(head_pan_target - head_pan_slew_up,
-head_pan_rad)
if camera_pan_left:
head_pan_target = min(head_pan_target + head_pan_slew_up,
head_pan_rad)
robot.head.move_by('head_pan', head_pan_target, head_pan_vel,
head_pan_accel)
# Head tilt
head_tilt_rad = deg_to_rad(40.0) # 25.0 #40.0
head_tilt_accel = 14.0 # 12.0 # 15.0
head_tilt_vel = 7.0 # 10.0 # 6.0
head_tilt_slew_down = 0.15
head_tilt_slew_up = .15
# Slew target to zero if no buttons pushed
if not camera_tilt_up and not camera_tilt_down:
if head_tilt_target >= 0:
head_tilt_target = max(
0, head_tilt_target - head_tilt_slew_down)
else:
head_tilt_target = min(
0, head_tilt_target + head_tilt_slew_down)
# Or slew up to max
if camera_tilt_down:
head_tilt_target = max(head_tilt_target - head_tilt_slew_up,
-head_tilt_rad)
if camera_tilt_up:
head_tilt_target = min(head_tilt_target + head_tilt_slew_up,
head_tilt_rad)
robot.head.move_by('head_tilt', head_tilt_target, head_tilt_vel,
head_tilt_accel)
if robot.end_of_arm is not None and robot.end_of_arm.is_tool_present(
'StretchGripper'):
gripper_rotate_pct = 10.0
gripper_accel = robot.end_of_arm.motors[
'stretch_gripper'].params['motion']['max']['accel']
gripper_vel = robot.end_of_arm.motors[
'stretch_gripper'].params['motion']['max']['vel']
if open_gripper:
robot.end_of_arm.move_by('stretch_gripper',
gripper_rotate_pct, gripper_vel,
gripper_accel)
elif close_gripper:
robot.end_of_arm.move_by('stretch_gripper',
-gripper_rotate_pct, gripper_vel,
gripper_accel)
##################
# execute robot's motor movements
if stow_robot and robot_calibrated:
#Reset motion params as fast for xbox
v = robot.end_of_arm.motors['wrist_yaw'].params['motion'][
'default']['vel']
a = robot.end_of_arm.motors['wrist_yaw'].params['motion'][
'default']['accel']
robot.end_of_arm.motors['wrist_yaw'].set_motion_params(v, a)
robot.stow()
else:
if robot_calibrated:
robot.push_command()
elif calibrate_the_robot:
print('begin calibrating the robot')
robot.home()
print('finished calibrating the robot')
robot_calibrated = True
else:
if first_home_warn:
print('press the start button to calibrate the robot')
else:
first_home_warn = False
time.sleep(0.05)
except (ThreadServiceExit, KeyboardInterrupt, SystemExit):
stop_loop = True
xbox_controller.stop()
robot.stop()
