def first_move(self):
# Turn head for showing the edge drawing
head = intera_interface.Head()
head.set_pan(angle=0.0,
speed=0.3,
timeout=10,
active_cancellation=True)
# First observe all the AR_tags
# Target point
self.point = Point()
self.point.x = 0.700
self.point.y = 0.1603
self.point.z = 0.650
# This is the initial configuration of right_hand_camera in base frame
# The desired end-effector configuration
self.T = np.array([[0, 0, 1, self.point.x], [-1, 0, 0, self.point.y],
[0, -1, 0, self.point.z], [0, 0, 0, 1]])
# thetalist0; current joint angles
self.thetalist = [
0.00722578, -1.13799861, -0.01079448, 1.7510739, 0.00573321,
0.95772104
]
# Move to initial position
self.move_to_high(self.thetalist)
def __init__(self):
global TF, DIST_THRE, LINE_LENGTH, HOLD_TIME, CHECKER_CENTER_COORD, FROM_CENTER_D
self._limb = intera_interface.Limb("right")
self._head = intera_interface.Head()
self.center = np.array([])
self.target_list = []
self.draw_status_list = []
self.hold_init_time = None
self.line_init_time = None
self.current_target = np.array([])
self.current_draw_status = 0 #-1 is going up, 0 is not draw, 1 is apply force control
self.if_hold = 0 #0 is not to hold, -1 is not to hold but just get back from hold. 1 is to hold
self.line_init_pose = None
self.s = 0
self.object_2_draw = "idle"
#go to camera_pose
self.go_to_camera_or_standoff('camera')
# michael camera object to pass to bennett
self.camera = gs.Camera()
self.camera.updateBoard()
self.gameRunning = True
def __init__(self):
rospy.init_node("pick_and_place_node", log_level=rospy.DEBUG)
self._hover_distance = rospy.get_param("~hover_distance", 0.2)
self._limb_name = rospy.get_param("~limb", 'right')
self._limb = intera_interface.Limb(self._limb_name)
self._limb.set_joint_position_speed(0.3)
self._gripper = intera_interface.Gripper(self._limb_name)
if self._gripper is None:
rospy.logerr("Gripper error")
else:
rospy.logdebug("Gripper OK")
self._head = intera_interface.Head()
self._iksvc_name = "ExternalTools/" + self._limb_name + "/PositionKinematicsNode/IKService"
rospy.wait_for_service(self._iksvc_name, 5.0)
self._iksvc = rospy.ServiceProxy(self._iksvc_name, SolvePositionIK)
# Enable the robot
_rs = intera_interface.RobotEnable(intera_interface.CHECK_VERSION)
_init_state = _rs.state().enabled
rospy.logdebug("Enabling robot... ")
_rs.enable()
rospy.Service("pick_and_place", PickAndPlace, self.pnp)
rospy.Service("pick_and_throw", PickAndPlace, self.pnt)
rospy.Service("move_to_position", PositionMovement,
self.move_to_position)
rospy.Service("move_to_joint", JointMovement, self.move_to_joint)
rospy.Service("move_head", HeadMovement, self.move_head)
#rospy.Service("throw", Throw, self.throw)
rospy.logdebug("PNP Ready")
def _init_nodes(self, limb, light):
try:
self._limb=intera_interface.Limb(limb)
self._head=intera_interface.Head()
self._light=SawyerLight(light)
#self._head_display=intera_interface.HeadDisplay()
self._display=SawyerDisplay()
self._cuff=intera_interface.Cuff()
self._limits=intera_interface.JointLimits()
self._navigator=intera_interface.Navigator()
self._joint_names=self._limb.joint_names()
self._velocity_limits=self._limits.joint_velocity_limits()
self._stop_cmd={}
for i,name in enumerate(self._joint_names):
self._stop_cmd[name]=0.0
except:
print("Warning caught exception...")
traceback.print_exc()
pass
def __init__(self):
self.rs = intera_interface.RobotEnable(CHECK_VERSION)
self.gripper = intera_interface.Gripper('right_gripper')
self.limb = intera_interface.Limb()
self.head = intera_interface.Head()
self.head_angle = .5
self.joint_names = self.limb.joint_names()
self.target_pose = Pose()
self.select_throw = 1
self.underhand_throw_speed = 0.8
self.underhand_target_angle = -1.5
self.underhand_release_angle = .75
self.overhand_throw_speed = 1
self.overhand_target_angle = -3
self.overhand_release_angle = -1.3
self.overhand_throw_offset = 0
def displayFace(mode):
"""
Displays a face on the sawyer robot corresponding to a mode:
0: display a face for a Tie
1: display a face for a Win
2: display a face for a Loss
11: display a face for Robot's turn
22: display a face for the Player's turn
"""
_head = intera_interface.Head()
head_display = intera_interface.HeadDisplay()
rospack = rospkg.RosPack()
pkgpath = rospack.get_path('final-project-tic-tac-toe')
if mode == 0:
filepath = pkgpath + "/images/tie" + str(randrange(4)) + ".jpg"
if mode == 1:
filepath = pkgpath + "/images/win" + str(randrange(4)) + ".jpg"
if mode == 2:
filepath = pkgpath + "/images/lose.jpg"
if mode == 11:
filepath = pkgpath + "/images/board.jpg"
if mode == 22:
filepath = pkgpath + "/images/turn" + str(randrange(4)) + ".jpg"
head_display.display_image(filepath)
def main():
try:
rospy.init_node("head_turner")
# head_turner = HeadTurner()
# #rospy.on_shutdown(head_turner.clean_shutdown)
# head_turner.set_neutral()
# head_turner.turn_head(.4)
command_rate = rospy.Rate(1)
control_rate = rospy.Rate(100)
head = intera_interface.Head()
rs = intera_interface.RobotEnable(CHECK_VERSION)
init_state = rs.state().enabled
print("Enabling robot... ")
rs.enable()
angle = 0.95
while not rospy.is_shutdown(): #and (not (abs(head.pan() - angle) <=
# intera_interface.HEAD_PAN_ANGLE_TOLERANCE)):
head.set_pan(angle, speed=0.3, timeout=0)
print("Current head angle = {}".format(head.pan()))
print("Desired head angle = {}".format(angle))
control_rate.sleep()
command_rate.sleep()
#after_turn = head_turner.get_head_pan()
#print("Angle after turn = {}".format(after_turn))
#rospy.signal_shutdown("Head finished turning")
except:
print("bad things")
def __init__(self):
self._done = False
self._head = intera_interface.Head()
# verify robot is enabled
print("Getting robot state... ")
self.robot_state = 0 # normal
self.breath_state =0 # false
rospy.Subscriber("cs_sawyer/breath", Bool, self.callback_update_breath2)
rospy.Subscriber("cs_sawyer/head_light", UInt8, self.callback_update_move)
def __init__(self):
self._orientation = 0.0
self._head = intera_interface.Head()
# verify robot is enabled
print("initializing head turner")
print("Getting robot state... ")
self._rs = intera_interface.RobotEnable(CHECK_VERSION)
self._init_state = self._rs.state().enabled
print("Enabling robot... ")
self._rs.enable()
print("Running. Ctrl-c to quit")
def __init__(self):
"""
'Wobbles' the head
"""
self._done = False
self._head = intera_interface.Head()
# verify robot is enabled
print("Getting robot state... ")
self._rs = intera_interface.RobotEnable(CHECK_VERSION)
self._init_state = self._rs.state().enabled
print("Enabling robot... ")
self._rs.enable()
print("Running. Ctrl-c to quit")
def __init__(self, config=None):
"""Initialize.
See the parent class.
"""
super(FrankaPandaReal, self).__init__(config=config)
if rospy.get_name() == '/unnamed':
rospy.init_node('franka_panda')
assert rospy.get_name() != '/unnamed', 'Must call rospy.init_node().'
tic = time.time()
rospy.loginfo('Initializing robot...')
self._head = intera_interface.Head()
self._display = intera_interface.HeadDisplay()
self._lights = intera_interface.Lights()
self._limb = intera_interface.Limb()
self._joint_names = self._limb.joint_names()
self.cmd = []
self._command_msg = JointCommand()
self._command_msg.names = self._joint_names
self._commanders = dict(velocity=None, torque=None)
try:
self._gripper = intera_interface.Gripper()
self._has_gripper = True
except Exception:
self._has_gripper = False
self._robot_enable = intera_interface.RobotEnable(True)
self._params = intera_interface.RobotParams()
self._motion_planning = self.config.MOTION_PLANNING
if self._motion_planning == 'moveit':
rospy.loginfo('Initializing moveit toolkit...')
moveit_commander.roscpp_initialize(sys.argv)
self._scene = moveit_commander.PlanningSceneInterface()
self._group = moveit_commander.MoveGroupCommander('right_arm')
toc = time.time()
rospy.loginfo('Initialization finished after %.3f seconds.'
% (toc - tic))
def __init__(self, Name):
CONNECTION_STRING = "HostName=RobotForman.azure-devices.net;DeviceId=PythonTest;SharedAccessKey=oh9Fj0mAMWJZpNNyeJ+bSecVH3cBQwbzjDnoVmeSV5g="
self.protocol = IoTHubTransportProvider.HTTP
self.client = IoTHubClient(CONNECTION_STRING, self.protocol)
self.client.set_option("messageTimeout", MESSAGE_TIMEOUT)
self.client.set_option("MinimumPollingTime", MINIMUM_POLLING_TIME)
self.client.set_message_callback(receive_message_callback,
RECEIVE_CONTEXT)
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('mid' + str(uuid.uuid4().hex), anonymous=True)
print 'mid' + str(uuid.uuid4().hex)
self.head_display = intera_interface.HeadDisplay()
self.head_display.display_image("/home/microshak/Pictures/Ready.png",
False, 1.0)
self.head = intera_interface.Head()
rp = RobotParams()
valid_limbs = rp.get_limb_names()
robot = moveit_commander.RobotCommander()
rp.max_velocity_scaling_factor = .5
scene = moveit_commander.PlanningSceneInterface()
self.group = moveit_commander.MoveGroupCommander("right_arm")
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
self.light = Lights()
self.headLight("green")
# rs = intera_interface.RobotEnable(CHECK_VERSION)
self.group.clear_pose_targets()
self.endeffector = intera_interface.Gripper("right")
self.uri = "mongodb://*****:*****@sawyer-mongo.documents.azure.com:10255/?ssl=true"
self.Mongoclient = MongoClient(self.uri)
if Name == None:
self.poleIoTHub()
else:
self.completeCommands(Name)
def init_sawyer():
""" basic setup stuff """
rospy.loginfo("initializing sawyer...")
limb = intera_interface.Limb('right')
hd = intera_interface.HeadDisplay()
head = intera_interface.Head()
limb.move_to_neutral(timeout=TIMEOUT)
# TODO: Hardocoded to jon's computer
hd.display_image(
"/home/jon/catkin_ws/src/sawyer_simulator/sawyer_gazebo/share/images/sawyer_sdk_research.png"
)
head.set_pan(0.0)
rospy.loginfo("sawyer initialized")
return limb, hd, head
def __init__():
head=intera_interface.Head()
head.set_pan(0.0)
limb=intera_interface.Limb("right")
limb.set_joint_position_speed(0.15)
angle=get_position.main()
if((angle[1]<0.89)or(angle[1]>0.91)):
move_position.move([0,0,0,0,0,0,3.3])
move_position.move([0,0.9,0,-2.2,0,2,3.3])
elif((angle[3]<-2.21)or(angle[3]>-2.19)):
move_position.move([0,0,0,0,0,0,3.3])
move_position.move([0,0.9,0,-2.2,0,2,3.3])
elif((angle[5]<1.99)or(angle[5]>2.01)):
move_position.move([0,0,0,0,0,0,3.3])
move_position.move([0,0.9,0,-2.2,0,2,3.3])
elif((angle[6]<3.2)or(angle[6]>3.4)):
move_joint.j6(3.3)
def __init__(self):
# Initialize ROS node
rospy.init_node("manipulation", anonymous=True)
self.point_pub = rospy.Publisher("/me495/current_position",
Point,
queue_size=10)
# Communicate with perception node
self.tag_number = 0
rospy.Subscriber('/me495/target_position', Point, self.callback_point)
# Wait for command
rospy.Subscriber("/me495/command", String, self.callback_command)
# Enable the Sawyer
rs = intera_interface.RobotEnable()
rs.enable()
# Set the right arm and velocity ratio
self.mylimb = intera_interface.Limb("right")
self.mylimb.set_joint_position_speed(0.2)
# Dictionary for store target position in joint space
self.waypoints = self.mylimb.joint_angles()
# Command sent to perception node
self.command = Point()
self.command.x = 0
self.command.y = 0
self.command.z = 0
# IK parameter
self.Slist = np.array([[0, 0, 1, 0, 0, 0], [0, 1, 0, -0.317, 0, 0.081],
[1, 0, 0, 0, 0.317, -0.1925],
[0, 1, 0, -0.317, 0, 0.481],
[1, 0, 0, 0, 0.317, -0.024],
[0, 1, 0, -0.317, 0, 0.881]]).T
self.M = np.array([[0, 0, 1, 1.01475], [-1, 0, 0, 0.1603],
[0, -1, 0, 0.317], [0, 0, 0, 1]])
self.eomg = 0.01
self.ev = 0.001
# Turn head for taking photo
head = intera_interface.Head()
head.set_pan(angle=-np.pi,
speed=0.3,
timeout=10,
active_cancellation=True)
def show_image_callback(img_data):
"""The callback function to show image by using CvBridge and cv
"""
global position
head = intera_interface.Head()
head.set_pan(position)
face_cascade = cv2.CascadeClassifier(
'/home/raj/sawyer_ws/src/Raj/scripts/haarcascade_frontalface_default.xml'
)
eye_cascade = cv2.CascadeClassifier(
'/home/raj/sawyer_ws/src/Raj/scripts/haarcascade_eye.xml')
bridge = CvBridge()
image_msg = []
try:
cv_image = bridge.imgmsg_to_cv2(img_data, "bgr8")
except CvBridgeError, err:
rospy.logerr(err)
return
def __init__(self):
self._done = False
self._head = intera_interface.Head()
self._limb = intera_interface.Limb()
self._robot_state = intera_interface.RobotEnable(CHECK_VERSION)
self._init_state = self._robot_state.state().enabled
self._robot_state.enable()
self._display = intera_interface.HeadDisplay()
self._last_angles = self._limb.joint_angles()
self._last_head_pan = self._head.pan()
move_arm_out_of_way_service = rospy.Service('arm/move_away', MoveArm,
self.move_arm_out_of_way)
return_to_last_position = rospy.Service('arm/return_to_position',
MoveArm,
self.return_to_last_position)
move_to_rest = rospy.Service('arm/move_to_rest', MoveArm,
self.move_to_rest)
move_arm_to_rest = rospy.Service('arm/move_arm_to_rest', MoveArm,
self.move_arm_to_rest)
def __init__(self, side):
self.ar_track_data = []
self.table_x, self.table_y, self.table_z = 0, 0, 0
self.limb = intera_interface.Limb(side)
self.head = intera_interface.Head();
self.move_to_standby();
def __init__(self,
config='cfg/sawyer_ctrl_config.yaml',
controlType='EEImpedance',
use_safenet=False,
node_name='sawyer_interface'):
"""
Initialize Sawyer Robot for control
Args:
use_safenet: bool
whether to use the Safenet to constrain ee positions
and velocities
node_name: str
name of the node for ROS
Attributes:
ee_position
ee_orientation
"""
self.log_start_times = []
self.log_end_times = []
self.cmd_start_times = []
self.cmd_end_times = []
# Connect to redis client
# use default port 6379 at local host.
self.config = YamlConfig(config)
self.redisClient = RobotRedis(**self.config['redis'])
self.redisClient.flushall()
# Timing
self.startTime = time.time()
self.endTime = time.time()
self.action_set = False
self.model = Model()
self.ee_name = self.config['sawyer']['end_effector_name']
if config is not None:
if self.config['controller']['interpolator']['type'] == 'linear':
interp_kwargs = {
'max_dx': 0.005,
'ndim': 3,
'controller_freq': 500,
'policy_freq': 20,
'ramp_ratio': 0.2
}
self.interpolator_pos = LinearInterpolator(**interp_kwargs)
self.interpolator_ori = LinearOriInterpolator(**interp_kwargs)
rospy.loginfo(
"Linear interpolator created with params {}".format(
interp_kwargs))
else:
self.interpolator_pos = None
self.interpolator_ori = None
self.interpolator_goal_set = False
start = time.time()
rospy.loginfo('Initializing Sawyer robot interface...')
try:
self._head = iif.Head()
self._display = iif.HeadDisplay()
self._lights = iif.Lights()
self._has_head = True
except:
rospy.logerr('No head found, head functionality disabled')
self._has_head = False
self._limb = iif.Limb(limb="right", synchronous_pub=False)
self._joint_names = self._limb.joint_names()
self.cmd = []
try:
self._gripper = iif.Gripper()
self._has_gripper = True
except:
rospy.logerr('No gripper found, gripper control disabled')
self._has_gripper = False
self._robot_enable = iif.RobotEnable(True)
self._params = iif.RobotParams()
self.blocking = False
# set up internal pybullet simulation for jacobian and mass matrix calcs.
self._clid = pb.connect(pb.DIRECT)
pb.resetSimulation(physicsClientId=self._clid)
# TODO: make this not hard coded
sawyer_urdf_path = self.config['sawyer']['arm']['path']
self._pb_sawyer = pb.loadURDF(
fileName=sawyer_urdf_path,
basePosition=self.config['sawyer']['arm']['pose'],
baseOrientation=pb.getQuaternionFromEuler(
self.config['sawyer']['arm']['orn']),
globalScaling=1.0,
useFixedBase=self.config['sawyer']['arm']['is_static'],
flags=pb.URDF_USE_SELF_COLLISION_EXCLUDE_PARENT
| pb.URDF_USE_INERTIA_FROM_FILE,
physicsClientId=self._clid)
# For pybullet dof
self._motor_joint_positions = self.get_motor_joint_positions()
try:
ns = "ExternalTools/right/PositionKinematicsNode/IKService"
self._iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
rospy.wait_for_service(ns, 5.0)
self._ik_service = True
except:
rospy.logerr('IKService from Intera timed out')
self._ik_service = False
self._joint_names = self.config['sawyer']['limb_joint_names']
self.free_joint_dict = self.get_free_joint_dict()
self.joint_dict = self.get_joint_dict()
self._link_id_dict = self.get_link_dict()
rospy.loginfo('Sawyer initialization finished after {} seconds'.format(
time.time() - start))
# Set desired pose to initial
self.neutral_joint_position = self.config['sawyer'][
'neutral_joint_angles'] #[0, -1.18, 0.00, 2.18, 0.00, 0.57, 3.3161]
self.prev_cmd = np.asarray(self.neutral_joint_position)
self.current_cmd = self.prev_cmd
self.reset_to_neutral()
self.default_control_type = self.config['controller']['selected_type']
self.default_params = self.config['controller']['Real'][
self.default_control_type]
self.redisClient.set(CONTROLLER_CONTROL_TYPE_KEY,
self.default_control_type)
self.redisClient.set(CONTROLLER_CONTROL_PARAMS_KEY,
json.dumps(self.default_params))
self.pb_ee_pos_log = []
self.pb_ee_ori_log = []
self.pb_ee_v_log = []
self.pb_ee_w_log = []
# Set initial redis keys
self._linear_jacobian = None
self._angular_jacobian = None
self._jacobian = None
self._calc_mass_matrix()
self._calc_jacobian()
self.update_redis()
self.update_model()
self.controlType = self.get_control_type()
self.control_dict = self.get_controller_params()
self.controller = self.make_controller_from_redis(
self.controlType, self.control_dict)
self.redisClient.set(ROBOT_CMD_TSTAMP_KEY, time.time())
self.redisClient.set(ROBOT_SET_GRIPPER_CMD_KEY, 0.1)
self.redisClient.set(ROBOT_SET_GRIPPER_CMD_TSTAMP_KEY, time.time())
self.last_cmd_tstamp = self.get_cmd_tstamp()
self.last_gripper_cmd_tstamp = self.get_gripper_cmd_tstamp()
self.prev_gripper_state = self.des_gripper_state
rospy.logdebug('Control Interface initialized')
# TIMING TEST ONLY
self.timelog_start = open('cmd_tstamp.txt', 'w')
self.timelog_end = open('cmd_set_time.txt', 'w')
self.controller_times = []
self.loop_times = []
self.cmd_end_time = []
def __init__(self):
print "Initializing Node"
rospy.init_node('Sawyer_peripherals')
self._running = True
self._valid_limb_names = {'right': 'right', 'r': 'right'}
# gripper initialization
self._grippers = {'right': intera_interface.Gripper('right')}
# Set grippers to defaults
#self._grippers['right'].set_parameters(
# self._grippers['right'].valid_parameters())
# accelerometer initialization
self._accelerometers = {'right': [0.0] * 3}
rospy.Subscriber("/robot/accelerometer/right_accelerometer/state", Imu,
self.right_accel_callback)
# head control initialization
self._head = intera_interface.Head()
# suppressions
self._suppress_collision_avoidance = {'right': False}
self._supp_coll_avoid_pubs = {
'right':
rospy.Publisher("/robot/limb/right/suppress_collision_avoidance",
Empty,
latch=True)
}
self._suppress_contact_safety = {'right': False}
self._supp_con_safety_pubs = {
'right':
rospy.Publisher("/robot/limb/right/suppress_contact_safety",
Empty,
latch=True)
}
self._suppress_cuff_interaction = {'right': False}
self._supp_cuff_int_pubs = {
'right':
rospy.Publisher("/robot/limb/right/suppress_cuff_interaction",
Empty,
latch=True)
}
self._suppress_gravity_compensation = {'right': False}
self._supp_grav_comp_pubs = {
'right':
rospy.Publisher("/robot/limb/right/suppress_gravity_compensation",
Empty,
latch=True)
}
# start suppressions background thread
self._t_suppressions = threading.Thread(
target=self.suppressions_worker)
self._t_suppressions.daemon = True
self._t_suppressions.start()
# gravity compensation subscription
self._grav_comp_lock = threading.Lock()
self._gravity_compensation_torques = OrderedDict(
zip(intera_interface.Limb('right').joint_names(), [0.0] * 7))
rospy.Subscriber("/robot/limb/right/gravity_compensation_torques",
SEAJointState, self.grav_comp_callback)
def neutral():
head=intera_interface.Head()
head.set_pan(0.0)
limb=intera_interface.Limb("right")
limb.set_joint_position_speed(0.15)
limb.move_to_neutral()
def __init__(self):
print "Initializing Node"
rospy.init_node('sawyer_peripherals')
self._running = True
# self._valid_limb_names = {'left': 'left',
# 'l': 'left',
# 'right': 'right',
# 'r': 'right'}
self._valid_limb_names = {'right': 'right', 'r': 'right'}
# gripper initialization
#self._grippers = {'right': intera_interface.Gripper('right')}
# self._grippers = {#'left': intera_interface.Gripper('left'),
# 'right': intera_interface.Gripper('right')}
# Set grippers to defaults
# self._grippers['left'].set_parameters(
# self._grippers['left'].valid_parameters())
#self._grippers['right'].set_parameters(
#self._grippers['right'].valid_parameters())
# ranger initialization
#self._rangers = {'right': intera_interface.AnalogIO('right_hand_range')}
# self._rangers = {'left': intera_interface.AnalogIO('left_hand_range'),
# 'right': intera_interface.AnalogIO('right_hand_range')}
# accelerometer initialization
#self._accelerometers = {'left': [0.0]*3, 'right': [0.0]*3}
self._accelerometers = {'right': [0.0] * 3}
# rospy.Subscriber("/robot/accelerometer/left_accelerometer/state",
# Imu,
# self.left_accel_callback)
rospy.Subscriber("/robot/accelerometer/right_accelerometer/state", Imu,
self.right_accel_callback)
# head control initialization
self._head = intera_interface.Head()
# sonar initialization
self._sonar_pointcloud = RR.RobotRaconteurNode.s.NewStructure(
'SawyerPeripheral_interface.SonarPointCloud')
self._sonar_state_sub = rospy.Subscriber(
"/robot/sonar/head_sonar/state", PointCloud, self.sonar_callback)
self._sonar_enable_pub = rospy.Publisher(
"/robot/sonar/head_sonar/set_sonars_enabled", UInt16, latch=True)
# initially all sonar sensors on
self._sonar_enabled = True
self._sonar_enable_pub.publish(4095)
# suppressions
self._suppress_body_avoidance = {'right': False}
#self._suppress_body_avoidance = {'left': False, 'right': False}
self._supp_body_avoid_pubs = {
'left':
rospy.Publisher("/robot/limb/left/suppress_body_avoidance",
Empty,
latch=True),
'right':
rospy.Publisher("/robot/limb/right/suppress_body_avoidance",
Empty,
latch=True)
}
self._supp_body_avoid_pubs = {
'right':
rospy.Publisher("/robot/limb/right/suppress_body_avoidance",
Empty,
latch=True)
}
self._suppress_collision_avoidance = {'right': False}
# self._suppress_collision_avoidance = {'left': False, 'right': False}
self._supp_coll_avoid_pubs = {
'right':
rospy.Publisher("/robot/limb/right/suppress_collision_avoidance",
Empty,
latch=True)
}
# self._supp_coll_avoid_pubs = {'left':
# rospy.Publisher("/robot/limb/left/suppress_collision_avoidance",
# Empty,
# latch=True),
# 'right':
# rospy.Publisher("/robot/limb/right/suppress_collision_avoidance",
# Empty,
# latch=True)}
self._suppress_contact_safety = {'right': False}
# self._suppress_contact_safety = {'left': False, 'right': False}
self._supp_con_safety_pubs = {
'right':
rospy.Publisher("/robot/limb/right/suppress_contact_safety",
Empty,
latch=True)
}
# self._supp_con_safety_pubs = {'left':
# rospy.Publisher("/robot/limb/left/suppress_contact_safety",
# Empty,
# latch=True),
# 'right':
# rospy.Publisher("/robot/limb/right/suppress_contact_safety",
# Empty,
# latch=True)}
self._suppress_cuff_interaction = {'right': False}
# self._suppress_cuff_interaction = {'left': False, 'right': False}
self._supp_cuff_int_pubs = {
'right':
rospy.Publisher("/robot/limb/right/suppress_cuff_interaction",
Empty,
latch=True)
}
# self._supp_cuff_int_pubs = {'left':
# rospy.Publisher("/robot/limb/left/suppress_cuff_interaction",
# Empty,
# latch=True),
# 'right':
# rospy.Publisher("/robot/limb/right/suppress_cuff_interaction",
# Empty,
# latch=True)}
self._suppress_gravity_compensation = {'right': False}
# self._suppress_gravity_compensation = {'left': False, 'right': False}
self._supp_grav_comp_pubs = {
'right':
rospy.Publisher("/robot/limb/right/suppress_gravity_compensation",
Empty,
latch=True)
}
# self._supp_grav_comp_pubs = {'left':
# rospy.Publisher("/robot/limb/left/suppress_gravity_compensation",
# Empty,
# latch=True),
# 'right':
# rospy.Publisher("/robot/limb/right/suppress_gravity_compensation",
# Empty,
# latch=True)}
# start suppressions background thread
self._t_suppressions = threading.Thread(
target=self.suppressions_worker)
self._t_suppressions.daemon = True
self._t_suppressions.start()
# gravity compensation subscription
self._grav_comp_lock = threading.Lock()
self._gravity_compensation_torques = OrderedDict(
zip(intera_interface.Limb('right').joint_names(), [0.0] * 7))
# self._gravity_compensation_torques = OrderedDict(
# zip(intera_interface.Limb('left').joint_names() + \
# intera_interface.Limb('right').joint_names(),
# [0.0]*14))
# rospy.Subscriber("/robot/limb/left/gravity_compensation_torques",
# SEAJointState, self.grav_comp_callback)
rospy.Subscriber("/robot/limb/right/gravity_compensation_torques",
SEAJointState, self.grav_comp_callback)
# navigators
self._navigators = {'right': intera_interface.Navigator('right')}
# self._navigators = {'left': intera_interface.Navigator('left'),
# 'right': intera_interface.Navigator('right'),
# 'torso_left':
# intera_interface.Navigator('torso_left'),
# 'torso_right':
# intera_interface.Navigator('torso_right')}
# initialize frame transform
self._listener = tf.TransformListener()
def __init__(self):
self.head = intera_interface.Head()
def __init__(self):
print "Initializing Node"
rospy.init_node('poirot_rrhost')
print "Enabling Robot"
rs = intera_interface.RobotEnable()
rs.enable()
self.rs1 = intera_interface.RobotEnable()
rospy.Subscriber("spacenav/joy", Joy, self.SpaceNavJoyCallback)
rospy.Subscriber("/robot/digital_io/right_lower_cuff/state",
DigitalIOState, self.CuffCallback)
rospy.Subscriber("/robot/digital_io/right_button_ok/state",
DigitalIOState, self.NavWheelCallback)
rospy.Subscriber("/robot/digital_io/right_button_triangle/state",
DigitalIOState, self.XCallback)
rospy.Subscriber("/robot/digital_io/right_lower_button/state",
DigitalIOState, self.GripperCloseCallback)
rospy.Subscriber("/robot/digital_io/right_upper_button/state",
DigitalIOState, self.GripperOpenCallback)
rospy.Subscriber("/pushed1", Bool, self.pushed1callback)
rospy.Subscriber("/pushed2", Bool, self.pushed2callback)
rospy.Subscriber("/pushed3", Bool, self.pushed3callback)
rospy.Subscriber("/pushed4", Bool, self.pushed4callback)
self.DIOpub = rospy.Publisher('/io/comms/io/command',
IOComponentCommand,
queue_size=10)
self.h = IOComponentCommand()
self.h.time = rospy.Time.now()
self.h.op = "set"
self.h.args = "{ \"signals\" : { \"port_sink_0\" : { \"format\" : { \"type\" : \"int\", \"dimensions\" : [ 1] }, \"data\" : [ 0] } } }"
rospy.Subscriber("Robotiq2FGripperRobotInput",
inputMsg.Robotiq2FGripper_robot_input,
self.GripperStatusCallback)
self.gripperpub = rospy.Publisher(
'Robotiq2FGripperRobotOutput',
outputMsg.Robotiq2FGripper_robot_output)
self.grippercommand = outputMsg.Robotiq2FGripper_robot_output()
self.gripperstatus = inputMsg.Robotiq2FGripper_robot_input()
self.grip_des_pos = 444
#self.des_gripper_pos=0
#activate-reset-activate gripper
self.grippercommand.rACT = 1
self.grippercommand.rGTO = 1
self.grippercommand.rSP = 255
self.grippercommand.rFR = 150
self.gripperpub.publish(self.grippercommand)
time.sleep(.5)
self.grippercommand = outputMsg.Robotiq2FGripper_robot_output()
self.grippercommand.rACT = 0
self.gripperpub.publish(self.grippercommand)
time.sleep(.5)
self.grippercommand = outputMsg.Robotiq2FGripper_robot_output()
self.grippercommand.rACT = 1
self.grippercommand.rGTO = 1
self.grippercommand.rSP = 255
self.grippercommand.rFR = 150
self.pb1 = 1
self.pb2 = 1
self.pb3 = 1
self.pb4 = 1
time.sleep(.5)
self.nav = intera_interface.Navigator()
self.head = intera_interface.Head()
self.headangle = 0
self._valid_limb_names = {'right': 'right', 'r': 'right'}
# get information from the SDK
# self._left = intera_interface.Limb('left')
self._right = intera_interface.Limb('right')
#self._l_jnames = self._left.joint_names()
self._r_jnames = self._right.joint_names()
self.kin = sawyer_kinematics('right')
# data initializations
self._jointpos = [0] * 7
self._jointvel = [0] * 7
self._jointtor = [0] * 7
self._ee_pos = [0] * 3
self._ee_or = [0] * 4
self._ee_tw = [0] * 6
self._ee_wr = [0] * 6
self._ee_vel = [0] * 6
self._pijac = [
] #numpy.matrix('0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0')
self._jac = [
] #numpy.matrix('0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0')
self._inertia_mat = [
] #numpy.matrix('0,0,0,0,0,0,0;0,0,0,0,0,0,0;0,0,0,0,0,0,0;0,0,0,0,0,0,0;0,0,0,0,0,0,0;0,0,0,0,0,0,0;0,0,0,0,0,0,0')
self.Jxl_raw = 0
self.Jyl_raw = 0
self.Jzl_raw = 0
self.Jxa_raw = 0
self.Jya_raw = 0
self.Jza_raw = 0
self.leftbutton = 0
self.rightbutton = 0
self.spacenav = []
#self._l_joint_command = dict(zip(self._l_jnames,[0.0]*7))
self._r_joint_command = dict(zip(self._r_jnames, [0.0] * 7))
self.MODE_POSITION = 0
self.MODE_VELOCITY = 1
self.MODE_TORQUE = 2
self._mode = self.MODE_POSITION
self.RMH_delay = .01
# initial joint command is current pose
self.readJointPositions()
#self.setJointCommand('left',self._jointpos[0:7])
self.setJointCommand('right', self._jointpos[0:7])
self.hascollided = 0
self.head_display = intera_interface.HeadDisplay()
print('head display!')
#self.image="/home/rachel/rawhide/rawhide_ws/src/sawyer_simulator/sawyer_sim_examples/models/cafe_table/materials/textures/Wood_Floor_Dark.jpg"
self.image = "/home/rachel/rawhide/rmh_code/puppy.png"
self.head_display.display_image(self.image, display_rate=100)
print('finished first head display')
# Start background threads
self._running = True
self._t_joints = threading.Thread(target=self.jointspace_worker)
self._t_joints.daemon = True
self._t_joints.start()
self._t_effector = threading.Thread(target=self.endeffector_worker)
self._t_effector.daemon = True
self._t_effector.start()
self._t_command = threading.Thread(target=self.command_worker)
self._t_command.daemon = True
self._t_command.start()
self._t_display = threading.Thread(target=self.display_worker)
self._t_display.daemon = True
self._t_display.start()
self._t_dio = threading.Thread(target=self.dio_worker)
self._t_dio.daemon = True
self._t_dio.start()
#!/usr/bin/env python
import intera_interface
import os
import sys
import rospy
import curses
from robot_control.msg import TurnHead
sys.path.append(os.path.join(os.path.dirname(__file__), '../..', 'scripts'))
import helper_scripts
pub = rospy.Publisher('/head/turn', TurnHead, queue_size=10)
rospy.init_node('head_turner')
head = intera_interface.Head()
screen = curses.initscr()
curses.noecho()
curses.cbreak()
screen.keypad(True)
def turn_with_arrow_keys(direction):
head_angle = head.pan()
angle = head_angle + direction * .05
return angle_error_checker(angle)
def angle_error_checker(radian):
if radian > 0.896:
radian = -5.14
elif radian < -5.14:
radian = 0.896
return radian
