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