def __init__(self): self._rs = RobotEnable() self._cvbr = CvBridge() self._sm = RobotStateMachine(self) self.ik = IKHelper() self.ik.set_right(0.5, 0.0, 0.0, wait=True) self.left_img = None self.right_img = None self._left_camera_sub = rospy.Subscriber( '/cameras/left_hand_camera/image_rect_avg', Image, self.on_left_imagemsg_received) self._right_camera_sub = rospy.Subscriber( '/cameras/right_hand_camera/image_rect_avg', Image, self.on_right_imagemsg_received) self._display_pub = rospy.Publisher('/robot/xdisplay', Image, tcp_nodelay=True, latch=True) self.range = None self._range_sub = rospy.Subscriber( '/robot/range/right_hand_range/state', Range, self.on_rangemsg_received) self.itb = None self._itb_sub = rospy.Subscriber('/robot/itb/right_itb/state', ITBState, self.on_itbmsg_received) self.gripper = Gripper('right') self.gripper.calibrate() self.gripper.close(block=True)
def __init__(self, arm, lights=True): """ @type arm: str @param arm: arm of gripper to control {left, right} @type lights: bool @param lights: if lights should activate on cuff grasp """ self._arm = arm # inputs self._close_io = DigitalIO('%s_upper_button' % (arm, )) # 'dash' btn self._open_io = DigitalIO('%s_lower_button' % (arm, )) # 'circle' btn self._light_io = DigitalIO('%s_lower_cuff' % (arm, )) # cuff squeeze # outputs self._gripper = Gripper('%s' % (arm, )) self._nav = Navigator('%s' % (arm, )) # connect callback fns to signals if self._gripper.type() != 'custom': self._gripper.calibrate() self._open_io.state_changed.connect(self._open_action) self._close_io.state_changed.connect(self._close_action) else: msg = (("%s (%s) not capable of gripper commands." " Running cuff-light connection only.") % (self._gripper.name.capitalize(), self._gripper.type())) rospy.logwarn(msg) if lights: self._light_io.state_changed.connect(self._light_action) rospy.loginfo("%s Cuff Control initialized...", self._gripper.name.capitalize())
def __init__(self, pr_init, pl_init): self.centerx = 365 self.centery = 140 self.coefx = 0.1 / (526 - 369) self.coefy = 0.1 / (237 - 90) self.count = 0 self.right = MoveGroupCommander("right_arm") self.left = MoveGroupCommander("left_arm") self.gripper = Gripper("right") self.gripper.calibrate() self.gripper.set_moving_force(0.1) self.gripper.set_holding_force(0.1) self.pr = pr_init self.pl = pl_init self.angle = 0 self.position_list = (0.7, -0.3, -0.06, 0, 0.0, self.angle) self.busy = False self.blank_image = np.zeros((400, 640, 3), np.uint8) cv2.putText(self.blank_image, "DEMO", (180, 200), cv2.FONT_HERSHEY_SIMPLEX, fontScale=4, color=255, thickness=10) self.movenum = 0 self.gripper_distance = 100 self.subscribe_gripper() self.robotx = -1 self.roboty = -1 self.H = homography() self.framenumber = 0 self.history = np.arange(0, 20) * -1 self.newPosition = True self.bowlcamera = None
def __init__(self, starting_poss=None, push_thresh=10, mode='one_arm', arm_mode='first'): """ Initialises parameters and moves the arm to a neutral position. """ self.push_thresh = push_thresh self._right_neutral_pos = starting_poss[0] self._left_neutral_pos = starting_poss[1] self._mode = mode self._arm_mode = arm_mode rospy.loginfo("Creating interface and calibrating gripper") self._right_limb = Limb('right') self._left_limb = Limb('left') self._right_gripper = Gripper('right', CHECK_VERSION) self._right_gripper.calibrate() self._left_gripper = Gripper('left', CHECK_VERSION) self._left_gripper.calibrate() self._is_right_fist_closed = False self._is_left_fist_closed = False rospy.loginfo("Moving to neutral position") self.move_to_neutral() rospy.loginfo("Initialising PoseGenerator") self._pg = PoseGenerator(self._mode, self._arm_mode) self._sub_right_gesture = rospy.Subscriber( "/low_myo/gesture", UInt8, self._right_gesture_callback) self._sub_left_gesture = rospy.Subscriber("/top_myo/gesture", UInt8, self._left_gesture_callback) self._last_data = None self._pg.calibrate()
def pickUp3Point2(): above3point2 = { 'left_w0': 0.0, 'left_w1': 1.3157720208087136, 'left_w2': -0.002684466378799474, 'left_e0': 0, 'left_e1': 0.7850146682003605, 'left_s0': -0.73, 'left_s1': -0.6293156182299909 } down3point2 = { 'left_w0': 0.13077186216723152, 'left_w1': 1.1, 'left_w2': 0.0015339807878854137, 'left_e0': -0.16605342028859604, 'left_e1': 0.7, 'left_s0': -0.62, 'left_s1': -0.28 } Gripper.calibrate(leftg) leftg.open() time.sleep(1) left.move_to_joint_positions(above3point2, timeout=4) left.move_to_joint_positions(down3point2, timeout=4) leftg.open() leftg.close() time.sleep(1) left.move_to_joint_positions(above3point2) leftg.open()
def __init__(self): self.centerx = 365 self.centery = 120 self.coefx = 0.1/(526-369) self.coefy = 0.1/(237-90) self.count = 0 self.hdr = Header(stamp=rospy.Time.now(), frame_id='base') self.gripper = Gripper("right") self.gripper.calibrate() self.gripper.set_moving_force(0.1) rospy.sleep(0.5) self.gripper.set_holding_force(0.1) rospy.sleep(0.5) self.busy = False self.gripper_distance = 100 self.subscribe_gripper() self.robotx = -1 self.roboty = -1 self.framenumber = 0 self.history = np.arange(0,20)*-1 self.newPosition = True self.bowlcamera = None self.kin = baxter_kinematics('right') self.J = self.kin.jacobian() self.J_inv = self.kin.jacobian_pseudo_inverse() self.jointVelocity = np.asarray([1,2,3,4,5,6,7],np.float32) self.control_arm = Limb("right") self.control_joint_names = self.control_arm.joint_names() self.dx = 0 self.dy = 0 self.distance = 1000 self.finish = False self.found = 0 ifr = rospy.Subscriber("/robot/range/right_hand_range/state", Range, self._read_distance)
def __init__(self, usegripper = True): rospy.init_node("Baxter") threading.Thread(target=self.__jointtraj_server).start() self.trajrgt = Trajectory("right") self.trajlft = Trajectory("left") if usegripper: self.__right_hand = Gripper("right") self.__left_hand = Gripper("left") self.__right_hand.calibrate() self.__left_hand.calibrate() self.__right_limb = Limb("right") self.__left_limb = Limb("left") self.baxter = RobotEnable() self.__enabled = self.baxter.state().enabled self.enable_baxter() self.img = None for camname in ['head_camera', 'left_hand_camera', 'right_hand_camera']: try: cam = CameraController(camname) # cam.resolution = (1280, 800) cam.resolution = (320, 200) except: pass print("baxter opened")
def __init__(self, max_vscale=1.0): self.grasp_queue = Queue() initialized = False while not initialized: try: moveit_commander.roscpp_initialize(sys.argv) self.robot = moveit_commander.RobotCommander() self.scene = moveit_commander.PlanningSceneInterface() self.left_arm = moveit_commander.MoveGroupCommander('left_arm') self.left_arm.set_max_velocity_scaling_factor(max_vscale) self.go_to_pose('left', L_HOME_STATE) self.right_arm = moveit_commander.MoveGroupCommander( 'right_arm') self.right_arm.set_max_velocity_scaling_factor(max_vscale) self.go_to_pose('right', R_HOME_STATE) self.left_gripper = Gripper('left') self.left_gripper.set_holding_force(GRIPPER_CLOSE_FORCE) self.left_gripper.open() self.right_gripper = Gripper('right') self.left_gripper.set_holding_force(GRIPPER_CLOSE_FORCE) self.right_gripper.open() initialized = True except rospy.ServiceException as e: rospy.logerr(e)
def main(): global thetas configureLeftHand() planner = PathPlanner("right_arm") grip = Gripper('right') grip.calibrate() raw_input("gripper calibrated") grip.open() table_size = np.array([3, 1, 10]) table_pose = PoseStamped() table_pose.header.frame_id = "base" table_pose.pose.position.x = .9 table_pose.pose.position.y = 0.0 table_pose.pose.position.z = -5 - .112 #put cup cup_pos = start_pos_xy end_pos = goal_pos_xy putCupObstacle(planner, cup_pos, "cup1") putCupObstacle(planner, end_pos, "cup2") planner.add_box_obstacle(table_size, "table", table_pose) #move gripper to behind cup position = cup_pos + np.array([-0.1, 0, -0.02]) orientation = np.array([0, np.cos(np.pi / 4), 0, np.cos(np.pi / 4)]) executePositions(planner, [position], [orientation]) raw_input("moved behind cup, press enter") #move to cup and remove cup1 as obstacle since picking up removeCup(planner, "cup1") position = cup_pos + np.array([0, 0, -0.05]) executePositions(planner, [position], [orientation]) raw_input("moved to cup, press to close") grip.close() rospy.sleep(1) raw_input("gripped") moveAndPour(planner, end_pos) raw_input("poured") executePositions(planner, [position + np.array([0, 0, 0.02])], [orientation]) grip.open() removeCup(planner, "cup2")
def __init__(self, side_name): """ Constructor. @param side_name: which arm side we are refering to 'left' or 'right'. """ self.__side_name = side_name # MoveIt interface to a group of arm joints. # Either left arm joint group or right arm joint group. self.moveit_limb = MoveGroupCommander('{}_arm'.format(side_name)) # MoveIt limb setting. self.moveit_limb.set_end_effector_link('{}_gripper'.format(side_name)) self.moveit_limb.set_planner_id('RRTConnectKConfigDefault') self.moveit_limb.set_goal_position_tolerance(0.01) self.moveit_limb.set_goal_orientation_tolerance(0.01) # MoveIt does not provide support for Baxter gripper. # Thus we use Baxter SDK gripper instead. self.gripper = Gripper(side_name, CHECK_VERSION) self.gripper.calibrate() self.baxter_sdk_limb = Limb(side_name)
class Baxter_Interactive(object): def __init__(self, arm): #Vector to record poses self.recorded = [] self.arm = arm #enable Baxter self._en = RobotEnable() self._en.enable() #use DigitalIO to connect to gripper buttons self._close_button = DigitalIO(self.arm + '_upper_button') self._open_button = DigitalIO(self.arm + '_lower_button') #set up gripper self._gripper = Gripper(self.arm) self._gripper.calibrate() #set up navigator self._navigator = Navigator(self.arm) #set up limb self._limb = Limb(self.arm) self._limb.set_joint_position_speed(0.5) self._limb.move_to_neutral() print 'Ready to record...' def recorder(self): doneRecording = False while not doneRecording: if self._navigator.button0: self.recorded.append(self._limb.joint_angles()) print 'Waypoint Added' rospy.sleep(1) if self._close_button.state: self.recorded.append('CLOSE') self._gripper.close() print 'Gripper Closed' rospy.sleep(1) if self._open_button.state: self.recorded.append('OPEN') self._gripper.open() print 'Gripper Opened' rospy.sleep(1) if self._navigator.button1: print 'END RECORDING' doneRecording = True rospy.sleep(3) while doneRecording: for waypoint in self.recorded: if waypoint == 'OPEN': self._gripper.open() rospy.sleep(1) elif waypoint == 'CLOSE': self._gripper.close() rospy.sleep(1) else: self._limb.move_to_joint_positions(waypoint)
def __init__(self): rospy.loginfo("++++++++++++Ready to move the robot+++++++++++") #Initialize moveit_commander self.robot = moveit_commander.RobotCommander() self.scene = moveit_commander.PlanningSceneInterface() self.right_arm_group = moveit_commander.MoveGroupCommander("right_arm") #Display Trajectory in RVIZ self.display_trajectory_publisher = rospy.Publisher( '/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=20) self.moving_publisher = rospy.Publisher('amimoving', String, queue_size=10) rospy.sleep(3.0) # print self.robot.get_current_state() # print "" self.planning_frame = self.right_arm_group.get_planning_frame() rospy.loginfo("============ Reference frame: %s" % self.planning_frame) self.eef_link = self.right_arm_group.get_end_effector_link() print "============ End effector: %s" % self.eef_link self.group_names = self.robot.get_group_names() print "============ Robot Groups:", self.robot.get_group_names() #set planning and execution parameters self.right_arm_group.set_goal_position_tolerance(0.01) self.right_arm_group.set_goal_orientation_tolerance(0.01) self.right_arm_group.set_planning_time(5.0) self.right_arm_group.allow_replanning(False) self.right_arm_group.set_max_velocity_scaling_factor(0.6) self.right_arm_group.set_max_acceleration_scaling_factor(0.6) #specify which gripper and limb are taking command self.right_gripper = Gripper('right', CHECK_VERSION) self.right_gripper.reboot() self.right_gripper.calibrate() # self.limb = baxter_interface.Limb('right') # self.angles = self.limb.joint_angles() # self.wave_1 = {'right_s0': -0.459, 'right_s1': -0.202, 'right_e0': 1.807, 'right_e1': 1.714, 'right_w0': -0.906, 'right_w1': -1.545, 'right_w2': -0.276} # rospy.sleep(3) # self.limb.move_to_joint_positions(self.wave_1) self.pose_target = Pose() self.standoff = 0.1 self.__right_sensor = rospy.Subscriber( '/robot/range/right_hand_range/state', Range, self.rangecallback) self.rangestatetemp = Range() self.box_name = 'table' self.moving = '0' self.moving_publisher.publish(self.moving)
def calibrate_gripper(self): """ Initialize the gripper and its vacuum sensor. """ self.gripper = Gripper(self.gripper_side) self.gripper.calibrate() self.gripper.open() self.vacuum_sensor = AnalogIO(self.gripper_side + '_vacuum_sensor_analog') if rospy.get_param('verbose'): rospy.loginfo('Calibrated gripper. (type={})'.format( self.gripper.type()))
def __init__( self, camera_averaging=False): self._cvbr = CvBridge() self.rs = RobotEnable() self.ik = IKHelper() camera_topic = '/cameras/{0}_camera/image_rect' if camera_averaging: camera_topic += '_avg' self.left_img = None self._left_camera_sub = rospy.Subscriber( camera_topic.format('left_hand'), Image, self._on_left_imagemsg_received) self.right_img = None self._right_camera_sub = rospy.Subscriber( camera_topic.format('right_hand'), Image, self._on_right_imagemsg_received) self.head_img = None self._head_camera_sub = rospy.Subscriber( camera_topic.format('head'), Image, self._on_head_imagemsg_received) self.left_itb = None self._left_itb_sub = rospy.Subscriber( '/robot/itb/left_itb/state', ITBState, self._on_left_itbmsg_received) self.right_itb = None self._right_itb_sub = rospy.Subscriber( '/robot/itb/right_itb/state', ITBState, self._on_right_itbmsg_received) self.left_gripper = Gripper('left') self.right_gripper = Gripper('right') self.head = Head() self._display_pub = rospy.Publisher( '/robot/xdisplay', Image, tcp_nodelay=True, latch=True)
def callback(self, request=True): """ Checks the status of the grasping components. :param request: request from brain to check the handling :return: True if all checks are good and 'False' otherwise """ # 1. Enable Arms if not self.enable_arms(): return False # 2. Untuck Arms if not self.untuck_arms(): return False # 3. Gripper Calibration gripper = Gripper('left') if not self.calibrate_grippers(gripper): return False gripper.open(block=True) gripper = Gripper('right') if not self.calibrate_grippers(gripper): return False gripper.open(block=True) rospy.loginfo("All grasping checks were completed successfully.") return True
def __init__(self): rospy.loginfo("=============Ready to move the robot================") #Initialize moveit_commander self.robot=moveit_commander.RobotCommander() self.scene=moveit_commander.PlanningSceneInterface() self.right_arm_group = moveit_commander.MoveGroupCommander("right_arm") #Display Trajectory in RVIZ self.display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=20) rospy.sleep(3.0) self.planning_frame = self.right_arm_group.get_planning_frame() rospy.loginfo( "============ Reference frame: %s" % self.planning_frame) self.eef_link = self.right_arm_group.get_end_effector_link() print "============ End effector: %s" % self.eef_link self.group_names = self.robot.get_group_names() print "============ Robot Groups:", self.robot.get_group_names() #set planning and execution parameters self.right_arm_group.set_goal_position_tolerance(0.01) self.right_arm_group.set_goal_orientation_tolerance(0.01) self.right_arm_group.set_planning_time(5.0) self.right_arm_group.allow_replanning(False) self.right_arm_group.set_max_velocity_scaling_factor(0.5) self.right_arm_group.set_max_acceleration_scaling_factor(0.5) #specify which gripper and limb are taking command self.right_gripper = Gripper('right', CHECK_VERSION) self.right_gripper.reboot() self.right_gripper.calibrate() self.pose_target=Pose() self.startpose=Pose() self.standoff=0.2 #Where to place the block self.place_target=Pose() self.placex_coord= 0.65 self.placey_coord=0 self.__right_sensor=rospy.Subscriber('/robot/range/right_hand_range/state',Range,self.rangecallback) # self.location_data=rospy.Subscriber('square_location',String,self.compute_cartesian) self.compute_cartesian() self.rangestatetemp=Range()
def __init__(self, calibrate_grippers=True): self._baxter_state = RobotEnable() self._left = Limb(LEFT) self._right = Limb(RIGHT) self._limbs = {LEFT: self._left, RIGHT: self._right} self._head = Head() self._left_gripper, self._right_gripper = Gripper(LEFT), Gripper(RIGHT) if calibrate_grippers: self.calibrate() self._left_ikservice = IKService(LEFT) self._right_ikservice = IKService(RIGHT)
def initBaxterObjects(): global rightGripper global leftGripper global rightMover global leftMover rightGripper = Gripper('right', versioned = False) leftGripper = Gripper('left', versioned = False) rightMover = Limb('right') leftMover = Limb('left') # Initialize based on which is over the blocks global numBlocks global isOneArmSolution gridToCartesian.initToBaxter(rightMover)
class GripperConnect(object): """ Connects wrist button presses to gripper open/close commands. Uses the DigitalIO Signal feature to make callbacks to connected action functions when the button values change. """ def __init__(self, arm, lights=True): """ @type arm: str @param arm: arm of gripper to control {left, right} @type lights: bool @param lights: if lights should activate on cuff grasp """ self._arm = arm # inputs self._close_io = DigitalIO('%s_upper_button' % (arm,)) # 'dash' btn self._open_io = DigitalIO('%s_lower_button' % (arm,)) # 'circle' btn self._light_io = DigitalIO('%s_lower_cuff' % (arm,)) # cuff squeeze # outputs self._gripper = Gripper('%s' % (arm,)) self._nav = Navigator('%s' % (arm,)) # connect callback fns to signals if self._gripper.type() != 'custom': self._gripper.calibrate() self._open_io.state_changed.connect(self._open_action) self._close_io.state_changed.connect(self._close_action) else: msg = (("%s (%s) not capable of gripper commands." " Running cuff-light connection only.") % (self._gripper.name.capitalize(), self._gripper.type())) rospy.logwarn(msg) if lights: self._light_io.state_changed.connect(self._light_action) rospy.loginfo("%s Cuff Control initialized...", self._gripper.name.capitalize()) def _open_action(self, value): if value: rospy.logdebug("gripper open triggered") self._gripper.open() def _close_action(self, value): if value: rospy.logdebug("gripper close triggered") self._gripper.close() def _light_action(self, value): if value: rospy.logdebug("cuff grasp triggered") else: rospy.logdebug("cuff release triggered") self._nav.inner_led = value self._nav.outer_led = value
def __init__(self): self._rs = RobotEnable() self._cvbr = CvBridge() self._sm = RobotStateMachine(self) self.ik = IKHelper() self.ik.set_right(0.5, 0.0, 0.0, wait=True) self.gripper = Gripper('right') self.gripper.calibrate() self.gripper.close(block=True) self.right_img = None self._right_camera_sub = rospy.Subscriber( '/cameras/right_hand_camera/image_rect_avg', Image, self.on_right_imagemsg_received) self.itb = None self._itb_sub = rospy.Subscriber( '/robot/itb/right_itb/state', ITBState, self.on_itbmsg_received) self._display_pub = rospy.Publisher( '/robot/xdisplay', Image, tcp_nodelay=True, latch=True)
def __init__(self, pr_init, pl_init): self.centerx = 365 self.centery = 140 self.coefx = 0.1/(526-369) self.coefy = 0.1/(237-90) self.count = 0 self.right = MoveGroupCommander("right_arm") self.left = MoveGroupCommander("left_arm") self.gripper = Gripper("right") self.gripper.calibrate() self.gripper.set_moving_force(0.1) self.gripper.set_holding_force(0.1) self.pr = pr_init self.pl = pl_init self.angle = 0 self.position_list = (0.7,-0.3,-0.06,0,0.0,self.angle) self.busy = False self.blank_image = np.zeros((400,640,3),np.uint8) cv2.putText(self.blank_image,"DEMO", (180,200), cv2.FONT_HERSHEY_SIMPLEX, fontScale = 4, color = 255, thickness = 10 ) self.movenum = 0 self.gripper_distance = 100 self.subscribe_gripper() self.robotx = -1 self.roboty = -1 self.H = homography() self.framenumber = 0 self.history = np.arange(0,20)*-1 self.newPosition = True self.bowlcamera = None
def __init__(self, arm, lights=True): """ @type arm: str @param arm: arm of gripper to control {left, right} @type lights: bool @param lights: if lights should activate on cuff grasp """ self._arm = arm # inputs self._close_io = DigitalIO('%s_upper_button' % (arm,)) # 'dash' btn self._open_io = DigitalIO('%s_lower_button' % (arm,)) # 'circle' btn self._light_io = DigitalIO('%s_lower_cuff' % (arm,)) # cuff squeeze # outputs self._gripper = Gripper('%s' % (arm,)) self._nav = Navigator('%s' % (arm,)) # connect callback fns to signals if self._gripper.type() != 'custom': self._gripper.calibrate() self._open_io.state_changed.connect(self._open_action) self._close_io.state_changed.connect(self._close_action) else: msg = (("%s (%s) not capable of gripper commands." " Running cuff-light connection only.") % (self._gripper.name.capitalize(), self._gripper.type())) rospy.logwarn(msg) if lights: self._light_io.state_changed.connect(self._light_action) rospy.loginfo("%s Cuff Control initialized...", self._gripper.name.capitalize())
class GripperConnect(object): """ Connects wrist button presses to gripper open/close commands. Uses the DigitalIO Signal feature to make callbacks to connected action functions when the button values change. """ def __init__(self, arm, lights=True): """ @type arm: str @param arm: arm of gripper to control {left, right} @type lights: bool @param lights: if lights should activate on cuff grasp """ self._arm = arm # inputs self._close_io = DigitalIO('%s_upper_button' % (arm, )) # 'dash' btn self._open_io = DigitalIO('%s_lower_button' % (arm, )) # 'circle' btn self._light_io = DigitalIO('%s_lower_cuff' % (arm, )) # cuff squeeze # outputs self._gripper = Gripper('%s' % (arm, )) self._nav = Navigator('%s' % (arm, )) # connect callback fns to signals if self._gripper.type() != 'custom': self._gripper.calibrate() self._open_io.state_changed.connect(self._open_action) self._close_io.state_changed.connect(self._close_action) else: msg = (("%s (%s) not capable of gripper commands." " Running cuff-light connection only.") % (self._gripper.name.capitalize(), self._gripper.type())) rospy.logwarn(msg) if lights: self._light_io.state_changed.connect(self._light_action) rospy.loginfo("%s Cuff Control initialized...", self._gripper.name.capitalize()) def _open_action(self, value): if value: rospy.logdebug("gripper open triggered") self._gripper.open() def _close_action(self, value): if value: rospy.logdebug("gripper close triggered") self._gripper.close() def _light_action(self, value): if value: rospy.logdebug("cuff grasp triggered") else: rospy.logdebug("cuff release triggered") self._nav.inner_led = value self._nav.outer_led = value
def __init__(self, name, rate=100, fk='robot', ik='trac', default_kv_max=1., default_ka_max=0.5): """ :param name: 'left' or 'right' :param rate: Rate of the control loop for execution of motions :param fk: Name of the Forward Kinematics solver, "robot", "kdl", "trac" or "ros" :param ik: Name of the Inverse Kinematics solver, "robot", "kdl", "trac" or "ros" :param default_kv_max: Default K maximum for velocity :param default_ka_max: Default K maximum for acceleration """ Limb.__init__(self, name) self._world = 'base' self.kv_max = default_kv_max self.ka_max = default_ka_max self._gripper = Gripper(name) self._rate = rospy.Rate(rate) self._tf_listener = TransformListener() self.recorder = Recorder(name) # Kinematics services: names and services (if relevant) self._kinematics_names = {'fk': {'ros': 'compute_fk'}, 'ik': {'ros': 'compute_ik', 'robot': 'ExternalTools/{}/PositionKinematicsNode/IKService'.format(name), 'trac': 'trac_ik_{}'.format(name)}} self._kinematics_services = {'fk': {'ros': {'service': rospy.ServiceProxy(self._kinematics_names['fk']['ros'], GetPositionFK), 'func': self._get_fk_ros}, 'kdl': {'func': self._get_fk_pykdl}, 'robot': {'func': self._get_fk_robot}}, 'ik': {'ros': {'service': rospy.ServiceProxy(self._kinematics_names['ik']['ros'], GetPositionIK), 'func': self._get_ik_ros}, 'robot': {'service': rospy.ServiceProxy(self._kinematics_names['ik']['robot'], SolvePositionIK), 'func': self._get_ik_robot}, 'trac': {'service': rospy.ServiceProxy(self._kinematics_names['ik']['trac'], GetConstrainedPositionIK), 'func': self._get_ik_trac}, 'kdl': {'func': self._get_ik_pykdl}}} self._selected_ik = ik self._selected_fk = fk # Kinematics services: PyKDL self._kinematics_pykdl = baxter_kinematics(name) if self._selected_ik in self._kinematics_names['ik']: rospy.wait_for_service(self._kinematics_names['ik'][self._selected_ik]) if self._selected_fk in self._kinematics_names['fk']: rospy.wait_for_service(self._kinematics_names['fk'][self._selected_fk]) # Execution attributes rospy.Subscriber('/robot/limb/{}/collision_detection_state'.format(name), CollisionDetectionState, self._cb_collision, queue_size=1) rospy.Subscriber('/robot/digital_io/{}_lower_cuff/state'.format(name), DigitalIOState, self._cb_dig_io, queue_size=1) self._stop_reason = '' # 'cuff' or 'collision' could cause a trajectory to be stopped self._stop_lock = Lock() action_server_name = "/robot/limb/{}/follow_joint_trajectory".format(self.name) self.client = SimpleActionClient(action_server_name, FollowJointTrajectoryAction) self._display_traj = rospy.Publisher("/move_group/display_planned_path", DisplayTrajectory, queue_size=1) self._gripper.calibrate() self.client.wait_for_server()
class Robot_Gripper(object): def __init__(self): self._gripper = Gripper('left') self._gripper.calibrate() #right gripper isn't calibrated so it won't drop the RFID reader #self._gripper_right = Gripper('right') #self._gripper_right.calibrate() def close(self, left=True): if left: self._gripper.close(block=True) else: self._gripper_right.close(block=True) def open(self, left=True): if left: self._gripper.open(block=True) else: self._gripper_right.open(block=True) def gripping(self, left=True): if left: return self._gripper.gripping() else: return self._gripper_right.gripping()
class Abbe_Gripper(object): def __init__(self): self._gripper = Gripper('left') self._gripper.calibrate() #right gripper isn't calibrated so it won't drop the RFID reader #self._gripper_right = Gripper('right') #self._gripper_right.calibrate() def close(self,left=True): if left: self._gripper.close(block=True) else: self._gripper_right.close(block=True) def open(self,left=True): if left: self._gripper.open(block=True) else: self._gripper_right.open(block=True) def gripping(self,left=True): if left: return self._gripper.gripping() else: return self._gripper_right.gripping()
def __init__(self, camera_averaging=False): self._cvbr = CvBridge() self.rs = RobotEnable() self.ik = IKHelper() camera_topic = '/cameras/{0}_camera/image_rect' if camera_averaging: camera_topic += '_avg' self.left_img = None self._left_camera_sub = rospy.Subscriber( camera_topic.format('left_hand'), Image, self._on_left_imagemsg_received) self.right_img = None self._right_camera_sub = rospy.Subscriber( camera_topic.format('right_hand'), Image, self._on_right_imagemsg_received) self.head_img = None self._head_camera_sub = rospy.Subscriber( camera_topic.format('head'), Image, self._on_head_imagemsg_received) self.left_itb = None self._left_itb_sub = rospy.Subscriber('/robot/itb/left_itb/state', ITBState, self._on_left_itbmsg_received) self.right_itb = None self._right_itb_sub = rospy.Subscriber('/robot/itb/right_itb/state', ITBState, self._on_right_itbmsg_received) self.left_gripper = Gripper('left') self.right_gripper = Gripper('right') self.head = Head() self._display_pub = rospy.Publisher('/robot/xdisplay', Image, tcp_nodelay=True, latch=True)
def __init__(self): # create a RobotCommander self.robot = RobotCommander() # create a PlanningScene Interface object self.scene = PlanningSceneInterface() # create left arm move group and gripper self.left_arm = Arm("left_arm") self.left_gripper = self.left_arm.gripper = Gripper("left") # create right arm move group and gripper self.right_arm = Arm("right_arm") self.right_gripper = self.right_arm.gripper = Gripper("right") self.gripper_offset = 0.0 # reset robot self.reset()
def main(): factor=9 print("Right Init node") rospy.init_node("Right_Ik_service_client", anonymous=True) rate= rospy.Rate(1000) right_gripper=Gripper('right') right_gripper.calibrate() right_gripper.open() while not rospy.is_shutdown(): initial_position(round(6000*factor))#10ms go_position(round(10000*factor),0.25) for x in range (0,int(round(2000*factor))): right_gripper.close() #initial_position(10000)#3ms go_box(round(250*factor)) for x in range (0,int(round(800*factor))): right_gripper.open() print "Objecto dejado"
def __init__(self): rospy.init_node('vision_server_right') self.pub = rospy.Publisher('/robot/xdisplay', Image, latch=True) self.busy = False self.dx = 0 self.dy = 0 self.avg_dx = -1 self.avg_dy = -1 self.H = homography() self.framenumber = 0 self.history_x = np.arange(0, 10) * -1 self.history_y = np.arange(0, 10) * -1 self.bowlcamera = None self.newPosition = True self.foundBowl = 0 self.centerx = 400 #self.centerx = 250 self.centery = 150 #self.centery = 190 self.coefx = 0.1 / (526 - 369) self.coefy = 0.1 / (237 - 90) self.v_joint = np.arange(7) self.v_end = np.arange(6) self.cmd = {} self.found = False self.finish = 0 self.distance = 10 self.gripper = Gripper("right") #self.gripper.calibrate() cv2.namedWindow('image') self.np_image = np.zeros((400, 640, 3), np.uint8) cv2.imshow('image', self.np_image) #self._set_threshold() s = rospy.Service('vision_server_vertical', Vision, self.handle_vision_req) camera_topic = '/cameras/right_hand_camera/image' self.right_camera = rospy.Subscriber(camera_topic, Image, self._on_camera) ifr = rospy.Subscriber("/robot/range/right_hand_range/state", Range, self._read_distance) print "\nReady to use right hand vision server\n" self.kin = baxter_kinematics('right') self.J = self.kin.jacobian() self.J_inv = self.kin.jacobian_pseudo_inverse() self.jointVelocity = np.asarray([1, 2, 3, 4, 5, 6, 7], np.float32) self.control_arm = Limb("right") self.control_joint_names = self.control_arm.joint_names()
def __init__(self, side_name): """Will configure and initialise Baxter's hand.""" self._side_name = side_name # This is the reference to Baxter's arm. self.limb = MoveGroupCommander("{}_arm".format(side_name)) self.limb.set_end_effector_link("{}_gripper".format(side_name)) # Unfortunetly, MoveIt was not able to make the gripper work, neither # did was able to find a very good pose using Forward Kinematics, # so instead the Baxter SDK is used here to do these two jobs. self.gripper = Gripper(side_name, CHECK_VERSION) self.gripper.calibrate() self._limb = Limb(side_name) # This solver seems to be better for finding solution among obstacles self.limb.set_planner_id("RRTConnectkConfigDefault") # Error tollerance should be as low as possible for better accuracy. self.limb.set_goal_position_tolerance(0.01) self.limb.set_goal_orientation_tolerance(0.01)
def __init__(self, arm): #Vector to record poses self.recorded = [] self.arm = arm #enable Baxter self._en = RobotEnable() self._en.enable() #use DigitalIO to connect to gripper buttons self._close_button = DigitalIO(self.arm + '_upper_button') self._open_button = DigitalIO(self.arm + '_lower_button') #set up gripper self._gripper = Gripper(self.arm) self._gripper.calibrate() #set up navigator self._navigator = Navigator(self.arm) #set up limb self._limb = Limb(self.arm) self._limb.set_joint_position_speed(0.5) self._limb.move_to_neutral() print 'Ready to record...'
def __init__(self, side, baxter=None): self.gripper = Gripper(side) self.prepare() self.side = side self.opened = True self.outside_grip = True self.grippd = False self.state_pos = 100 self.weight_pub = rospy.Publisher( "/robot/end_effector/" + side + "_gripper/command", EndEffectorCommand) self.baxter = baxter self.id = 65537 self.touch_links = [ "pedestal", side + "_gripper", side + "_gripper_base", side + "_hand_camera", side + "_hand_range", side + "_hand", side + "_wrist" ] rospy.sleep(1) self.gripper_weight = 0.8 self.object_weight = 0.3 self.setWeight(0.8)
def __init__(self, arm): self.as_goal = {'left': baxterGoal(), 'right': baxterGoal()} self.as_feed = {'left': baxterFeedback(), 'right': baxterFeedback()} self.as_res = {'left': baxterResult(), 'right': baxterResult()} self.action_server_left = actionlib.SimpleActionServer( "baxter_action_server_left", baxterAction, self.execute_left, auto_start=False) self.action_server_right = actionlib.SimpleActionServer( "baxter_action_server_right", baxterAction, self.execute_right, auto_start=False) self.left_arm = Limb('left') self.right_arm = Limb('right') self.left_gripper = Gripper('left') self.right_gripper = Gripper('right') self.left_gripper.calibrate() self.arm = arm
class MoveItArm: """ Represents Baxter's Arm in MoveIt! world. Represents a hand of Baxter through MoveIt! Group. Some reusable code has been packed together into this class to make the code more readable. """ def __init__(self, side_name): """Will configure and initialise Baxter's hand.""" self._side_name = side_name # This is the reference to Baxter's arm. self.limb = MoveGroupCommander("{}_arm".format(side_name)) self.limb.set_end_effector_link("{}_gripper".format(side_name)) # Unfortunetly, MoveIt was not able to make the gripper work, neither # did was able to find a very good pose using Forward Kinematics, # so instead the Baxter SDK is used here to do these two jobs. self.gripper = Gripper(side_name, CHECK_VERSION) self.gripper.calibrate() self._limb = Limb(side_name) # This solver seems to be better for finding solution among obstacles self.limb.set_planner_id("RRTConnectkConfigDefault") # Error tollerance should be as low as possible for better accuracy. self.limb.set_goal_position_tolerance(0.01) self.limb.set_goal_orientation_tolerance(0.01) def __str__(self): """ String representation of the arm. String representation of the arm, either 'left' or 'right' string will be returned. Useful when the string representation of the arm is needed, like when accessing the IK solver. """ return self._side_name def is_left(self): """Will return True if this is the left arm, false otherwise.""" return True if self._side_name == "left" else False def is_right(self): """Will return True if this is the right arm, false otherwise.""" return True if self._side_name == "right" else False def open_gripper(self): """Will open Baxter's gripper on his active hand.""" # Block ensures that the function does not return until the operation # is completed. self.gripper.open(block=True) def close_gripper(self): """Will close Baxter's gripper on his active hand.""" # Block ensures that the function does not return until the operation # is completed. self.gripper.close(block=True)
def __init__(self): self.rightg = Gripper('right') self.rightg.set_holding_force(100) self.leftg = Gripper('left') self.right = Limb('right') self.left = Limb('left') self.head = Head() self.angles= list() rospy.Subscriber("command", String, self.command) rospy.spin()
class Baxter_Deli(object): def __init__(self): self.rightg = Gripper('right') self.rightg.set_holding_force(100) self.leftg = Gripper('left') self.right = Limb('right') self.left = Limb('left') self.head = Head() self.angles= list() rospy.Subscriber("command", String, self.command) rospy.spin() def command(self, comm): if comm.data == "left": self.angles.append(self.left.joint_angles()) elif comm.data == "right": self.angles.append(self.right.joint_angles()) elif comm.data == "done": print self.angles
def create_arms(self): """ Create arm interface objects for Baxter. An arm consists of a Limb and its Gripper. """ # create arm objects if self.sim: self.left_arm = pybullet_interface.Limb(self.baxter_id, "left", self.config) # self.left_arm.gripper = pybullet_interface.Gripper("left") self.right_arm = pybullet_interface.Limb(self.baxter_id, "right", self.config) # self.right_arm.gripper = pybullet_interface.Gripper("right") else: self.left_arm = Limb("left") self.left_arm.gripper = Gripper("left") self.left_arm.kin = baxter_kinematics("left") self.right_arm = Limb("right") self.right_arm.gripper = Gripper("right") self.right_arm.kin = baxter_kinematics("right") return
class track(): def __init__(self): self.centerx = 365 self.centery = 120 self.coefx = 0.1/(526-369) self.coefy = 0.1/(237-90) self.count = 0 self.hdr = Header(stamp=rospy.Time.now(), frame_id='base') self.gripper = Gripper("right") self.gripper.calibrate() self.gripper.set_moving_force(0.1) rospy.sleep(0.5) self.gripper.set_holding_force(0.1) rospy.sleep(0.5) self.busy = False self.gripper_distance = 100 self.subscribe_gripper() self.robotx = -1 self.roboty = -1 self.framenumber = 0 self.history = np.arange(0,20)*-1 self.newPosition = True self.bowlcamera = None self.kin = baxter_kinematics('right') self.J = self.kin.jacobian() self.J_inv = self.kin.jacobian_pseudo_inverse() self.jointVelocity = np.asarray([1,2,3,4,5,6,7],np.float32) self.control_arm = Limb("right") self.control_joint_names = self.control_arm.joint_names() self.dx = 0 self.dy = 0 self.distance = 1000 self.finish = False self.found = 0 ifr = rospy.Subscriber("/robot/range/right_hand_range/state", Range, self._read_distance) def _read_distance(self,data): self.distance = data.range def vision_request_right(self, controlID, requestID): try: rospy.wait_for_service('vision_server_vertical') vision_server_req = rospy.ServiceProxy('vision_server_vertical', VisionVertical) return vision_server_req(controlID, requestID) except (rospy.ServiceException,rospy.ROSInterruptException), e: print "Service call failed: %s" % e self.clean_shutdown_service()
def __init__(self, dmp_record_file_dir, dmp_dims=7, lateral_record_file=LATERAL_RECORD_FILE, vertical_record_file=VERTICAL_RECORD_FILE, return_record_file=RETURN_RECORD_FILE, dummy=False): self.K = 100 # Spring constant, value taken from http://wiki.ros.org/dmp self.D = 2.0 * np.sqrt(self.K) # Damping constant, value taken from http://wiki.ros.org/dmp self.dt = 1.0 # Time resolution, Value taken from http://wiki.ros.org/dmp self.num_bases = 4 # No of basis functions, value taken from http://wiki.ros.org/dmp self.dmp_record_file_dir = dmp_record_file_dir self.dmp_dims = dmp_dims self.lateral_record_file = os.path.join(self.dmp_record_file_dir, lateral_record_file) self.vertical_record_file = os.path.join(self.dmp_record_file_dir, vertical_record_file) self.return_record_file = os.path.join(self.dmp_record_file_dir, return_record_file) self.dummy = dummy self.tf_listener = tf.TransformListener() # the init velocity and goal threshold doesn't change across dmps, so just store them now self.init_dot = [] for i in range(self.dmp_dims): self.init_dot.append(0) self.goal_thresh = [] for i in range(self.dmp_dims): self.goal_thresh.append(0) # Initialize move it group moveit_commander.roscpp_initialize(sys.argv) self.robot = moveit_commander.RobotCommander() self.group = moveit_commander.MoveGroupCommander("right_arm") self.display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=10) self.integrate_iter = 5 # value taken from http://wiki.ros.org/dmp self.seq_length = -1 self.right_gripper = Gripper('right') # save the initial state try: self.init_pos, self.init_orient = self.tf_listener.lookupTransform('/base', '/right_gripper', rospy.Time(0)) #Plan to a different cup than demo except: time.sleep(1) self.init_pos, self.init_orient = self.tf_listener.lookupTransform('/base', '/right_gripper', rospy.Time(0)) self.right_gripper.calibrate()
class track(): def __init__(self, pr_init, pl_init): self.centerx = 365 self.centery = 140 self.coefx = 0.1/(526-369) self.coefy = 0.1/(237-90) self.count = 0 self.right = MoveGroupCommander("right_arm") self.left = MoveGroupCommander("left_arm") self.gripper = Gripper("right") self.gripper.calibrate() self.gripper.set_moving_force(0.1) self.gripper.set_holding_force(0.1) self.pr = pr_init self.pl = pl_init self.angle = 0 self.position_list = (0.7,-0.3,-0.06,0,0.0,self.angle) self.busy = False self.blank_image = np.zeros((400,640,3),np.uint8) cv2.putText(self.blank_image,"DEMO", (180,200), cv2.FONT_HERSHEY_SIMPLEX, fontScale = 4, color = 255, thickness = 10 ) self.movenum = 0 self.gripper_distance = 100 self.subscribe_gripper() self.robotx = -1 self.roboty = -1 self.H = homography() self.framenumber = 0 self.history = np.arange(0,20)*-1 self.newPosition = True self.bowlcamera = None def vision_request_right(self, controlID, requestID): rospy.wait_for_service('vision_server_vertical') try: vision_server_req = rospy.ServiceProxy('vision_server_vertical', Vision) return vision_server_req(controlID, requestID) except rospy.ServiceException, e: print "Service call failed: %s" % e
#!/usr/bin/env python import sys import time import rospy from std_msgs.msg import UInt32 from msg_types import * import baxter_interface from baxter_interface import Gripper, Head, Limb rospy.init_node('run_condition') voice_pub = rospy.Publisher('/voice', UInt32, queue_size=10) time.sleep(1) rightg = Gripper('right') rightg.set_holding_force(50) leftg = Gripper('left') right = Limb('right') left = Limb('left') head = Head() neutral_right = {'right_s0': 0.476301034638421, 'right_s1': -0.5606699779721187, 'right_w0': 0.07094661143970038, 'right_w1': 0.7037136864424336, 'right_w2': -0.28033498898605935, 'right_e0': -0.16566992509162468, 'right_e1': 1.3077186216723151} neutral_left = {'left_w0': -0.15339807878854136, 'left_w1': 0.34552917247118947, 'left_w2': 3.0158062289827234, 'left_e0': 0.18676216092504913, 'left_e1': 1.5715633171886063, 'left_s0': -0.5836796897904, 'left_s1': -0.6845389265938658} right.move_to_joint_positions(neutral_right) left.move_to_joint_positions(neutral_left) head.set_pan(0.0, speed = 0.8, timeout = 0.0) Gripper.calibrate(rightg) Gripper.calibrate(leftg)
#!/usr/bin/env python import rospy from baxter_interface import Gripper if __name__ == '__main__': rospy.init_node('gripperTools', anonymous=True) gripper = Gripper('left') gripper.calibrate()
def __init__(self): pygame.init() rospy.init_node('node_mouse', anonymous=True) right_gripper=Gripper('right') left_gripper=Gripper('left') #limb_0=baxter_interface.Limb('right') #limb_0.set_joint_positions({'right_w2': 0.00}) def set_w2(): limb=baxter_interface.Limb('left') current_position = limb.joint_angle('left_w2') #send=current_position+delta #if(send>2.80 or send<-2.80): # delta=-delta # time.sleep(0.15) #print send return current_position #joint_command = {joint_name: send} #limb.set_joint_positions(joint_command) #if(current_position-send>0): # delta=-1 def set_s1(): limb=baxter_interface.Limb('left') current_position = limb.joint_angle('left_s1') #send=current_position+delta #if(send>2.80 or send<-2.80): # delta=-delta # time.sleep(0.15) #print send return current_position def set_e1(): limb=baxter_interface.Limb('left') current_position = limb.joint_angle('left_e1') #send=current_position+delta #if(send>2.80 or send<-2.80): # delta=-delta # time.sleep(0.15) #print send return current_position pygame.display.set_caption('Game') pygame.mouse.set_visible(True) vec = Vector3(1, 2, 3) right_gripper.calibrate() left_gripper.calibrate() try: pub = rospy.Publisher('mouse', Vector3, queue_size=1) rate = rospy.Rate(1000) # 10hz screen = pygame.display.set_mode((640,480), 0, 32) pygame.mixer.init() a=0 ,0 ,0 while not rospy.is_shutdown(): for event in pygame.event.get(): if event.type == pygame.MOUSEBUTTONDOWN or event.type == pygame.MOUSEBUTTONUP: a = pygame.mouse.get_pressed() print a weq=0 if a[0]==1: left_gripper.open() else: left_gripper.close() if a[2]==1: right_gripper.open() else: right_gripper.close() vec.x=set_w2() vec.y=a[1] vec.z=0 print vec pub.publish(vec) pygame.mixer.quit() pygame.quit() except rospy.ROSInterruptException: pass
def __init__(self): self.centerx = 365 self.centery = 120 self.coefx = 0.1/(526-369) self.coefy = 0.1/(237-90) self.count = 0 self.hdr = Header(stamp=rospy.Time.now(), frame_id='base') self.gripper = Gripper("right") self.gripper.calibrate() rospy.sleep(2) # self.gripper.set_moving_force(0.1) # rospy.sleep(0.5) # self.gripper.set_holding_force(0.1) # rospy.sleep(0.5) self.busy = False self.gripper_distance = 100 self.subscribe_gripper() self.robotx = -1 self.roboty = -1 self.framenumber = 0 self.history = np.arange(0,20)*-1 self.newPosition = True self.bowlcamera = None self.kin = baxter_kinematics('right') self.J = self.kin.jacobian() self.J_inv = self.kin.jacobian_pseudo_inverse() self.jointVelocity = np.asarray([1,2,3,4,5,6,7],np.float32) self.control_arm = Limb("right") self.control_joint_names = self.control_arm.joint_names() self.dx = 0 self.dy = 0 self.distance = 1000 self.finish = False self.found = 0 ifr = rospy.Subscriber("/robot/range/right_hand_range/state", Range, self._read_distance) self.joint_states = { # 'observe':{ # 'right_e0': -0.365, # 'right_e1': 1.061, # 'right_s0': 0.920, # 'right_s1': -0.539, # 'right_w0': 0.350, # 'right_w1': 1.105, # 'right_w2': -0.221, # }, 'observe':{ 'right_e0': -0.631, 'right_e1': 0.870, 'right_s0': 0.742, 'right_s1': -0.6087, 'right_w0': 0.508, 'right_w1': 1.386, 'right_w2': -0.5538, }, 'observe_vertical':{ 'right_e0': 0.699, 'right_e1': 1.451, 'right_s0': -1.689, 'right_s1': 0.516, 'right_w0': 0.204, 'right_w1': 0.935, 'right_w2': -2.706, }, }
class Abbe_Gripper(object): def __init__(self): self._gripper = Gripper('left') self._gripper.calibrate() #don't want it to drop rfid reader so disabling right gripperq self._gripper_right = Gripper('right') self._gripper_right.calibrate() def close(self,left=True): if left: self._gripper.close(block=True) else: self._gripper_right.close(block=True) def open(self,left=True): if left: self._gripper.open(block=True) else: self._gripper_right.open(block=True) def gripping(self,left=True): if left: return self._gripper.gripping() else: return self._gripper_right.gripping()
#!/usr/bin/env python import rospy from baxter_interface import Gripper if __name__ == '__main__': rospy.init_node('gripperTools', anonymous=True) gripper = Gripper('left') gripper.open(True)
#oatmeal raisin cookie( one ): #gripper: narrow (left) #holding force: 40 #replace cookie with drink #individually wrapped triangle sandwich will not do #!/usr/bin/env python import rospy, baxter_interface from baxter_interface import Gripper, Head, Limb rospy.init_node('deli_Baxter') rightg = Gripper('right') rightg.set_holding_force(100) leftg = Gripper('left') right = Limb('right') left = Limb('left') head = Head() head.set_pan(0.0, speed = 0.8, timeout = 0.0) Gripper.calibrate(rightg) Gripper.calibrate(leftg) neutral_right ={'right_s0': 0.5971020216843973, 'right_s1': -0.4237621926533455, 'right_w0': 0.4226117070624315, 'right_w1': 0.8471408901097197, 'right_w2': -0.9725438195193523, 'right_e0': -0.40727189918357737, 'right_e1': 1.161990446823201} neutral_left = {'left_w0': -0.3006602344255411, 'left_w1': 0.5549175500175484, 'left_w2': 3.050704291907117, 'left_e0': 0.5161845351234418, 'left_e1': 1.1984224905354794, 'left_s0': -0.8904758473674826, 'left_s1': -0.38387869216832476}
def __init__(self): self._gripper = Gripper('left') self._gripper.calibrate() #don't want it to drop rfid reader so disabling right gripperq self._gripper_right = Gripper('right') self._gripper_right.calibrate()
def __init__(self): self.centerx = 365 self.centery = 120 self.coefx = 0.1/(526-369) self.coefy = 0.1/(237-90) self.count = 0 self.hdr = Header(stamp=rospy.Time.now(), frame_id='base') self.gripper_left = Gripper("left") self.gripper_left.calibrate() self.gripper_left.set_moving_force(0.01) rospy.sleep(0.5) self.gripper_left.set_holding_force(0.01) rospy.sleep(0.5) self.gripper_right = Gripper("right") self.gripper_right.calibrate() rospy.sleep(1) self.busy = False self.gripper_distance = 100 self.subscribe_gripper() self.robotx = -1 self.roboty = -1 self.framenumber = 0 self.history = np.arange(0,20)*-1 self.newPosition = True self.bowlcamera = None self.kin_right = baxter_kinematics('right') self.kin_left = baxter_kinematics('left') self.J = self.kin_right.jacobian() self.J_inv = self.kin_right.jacobian_pseudo_inverse() self.jointVelocity = np.asarray([1,2,3,4,5,6,7],np.float32) self.control_arm = Limb("right") self.left_arm = Limb("left") self.control_joint_names = self.control_arm.joint_names() self.dx = 0 self.dy = 0 self.distance = 1000 self.finish = False self.found = 0 self.pour_x = 0 self.pour_y = 0 ifr = rospy.Subscriber("/robot/range/right_hand_range/state", Range, self._read_distance) self.joint_states = { 'observe':{ 'right_e0': -0.631, 'right_e1': 0.870, 'right_s0': 0.742, 'right_s1': -0.6087, 'right_w0': 0.508, 'right_w1': 1.386, 'right_w2': -0.5538, }, 'observe_ladle':{ 'right_e0': -0.829, 'right_e1': 0.831, 'right_s0': 0.678, 'right_s1': -0.53, 'right_w0': 0.716, 'right_w1': 1.466, 'right_w2': -0.8099, }, 'observe_left':{ 'left_w0': 2.761932405432129, 'left_w1': -1.5700293346069336, 'left_w2': -0.8893253607604981, 'left_e0': -0.9805972175354004, 'left_e1': 1.8300390778564455, 'left_s0': 1.4783739826354982, 'left_s1': -0.9503010970092775, }, 'stir':{ 'right_e0': -0.179, 'right_e1': 1.403, 'right_s0': 0.381, 'right_s1': -0.655, 'right_w0': 1.3, 'right_w1': 2.04, 'right_w2': 0.612, }, 'observe_vertical':{ 'right_e0': 0.699, 'right_e1': 1.451, 'right_s0': -1.689, 'right_s1': 0.516, 'right_w0': 0.204, 'right_w1': 0.935, 'right_w2': -2.706, }, 'observe_midpoint':{ 'right_e0': -0.606, 'right_e1': 0.968, 'right_s0': 0.0, 'right_s1': -0.645, 'right_w0': 0.465, 'right_w1': 1.343, 'right_w2': -0.437, }, 'dressing':{ 'right_e0': 0.967, 'right_e1': 1.386, 'right_s0': -0.348, 'right_s1': -0.155, 'right_w0': 0.264, 'right_w1': 1.521, 'right_w2': -2.199, }, }
def __init__(self): self._gripper = Gripper('left') self._gripper.calibrate()
class BaxterNode(object): """Helper class for creating Baxter nodes quickly, intended to be sublclassed. Attributes: rs -- An instance of RobotEnable provided by baxter_interface. ik -- An instance of IKHelper. left_img -- Last recieved image from left camera, rectified and as an OpenCV numpy array. right_img -- Last recieved image from right camera, rectified and as an OpenCV numpy array. left_itb -- Last received state from left ITB (shoulder buttons). left_itb -- Last received state from right ITB (shoulder buttons). left_gripper -- Instance of Gripper from baxter_interface, for left hand. right_gripper -- Instance of Gripper from baxter_interface, for right hand. """ ############################################################################ def __init__( self, camera_averaging=False): self._cvbr = CvBridge() self.rs = RobotEnable() self.ik = IKHelper() camera_topic = '/cameras/{0}_camera/image_rect' if camera_averaging: camera_topic += '_avg' self.left_img = None self._left_camera_sub = rospy.Subscriber( camera_topic.format('left_hand'), Image, self._on_left_imagemsg_received) self.right_img = None self._right_camera_sub = rospy.Subscriber( camera_topic.format('right_hand'), Image, self._on_right_imagemsg_received) self.head_img = None self._head_camera_sub = rospy.Subscriber( camera_topic.format('head'), Image, self._on_head_imagemsg_received) self.left_itb = None self._left_itb_sub = rospy.Subscriber( '/robot/itb/left_itb/state', ITBState, self._on_left_itbmsg_received) self.right_itb = None self._right_itb_sub = rospy.Subscriber( '/robot/itb/right_itb/state', ITBState, self._on_right_itbmsg_received) self.left_gripper = Gripper('left') self.right_gripper = Gripper('right') self.head = Head() self._display_pub = rospy.Publisher( '/robot/xdisplay', Image, tcp_nodelay=True, latch=True) ############################################################################ def start(self, spin=False, calibrate=False): """Start up the node and initalise Baxter. Keyword arguments: spin -- Enter a spin loop once initialised (default False). calibrate -- Calibrate the grippers (default False). """ self.rs.enable() # Wait for initial topic messages to come in. while self.left_itb is None or \ self.right_itb is None: rospy.sleep(100) # Calibrate both grippers. if calibrate: self.left_gripper.calibrate() self.right_gripper.calibrate() if spin: rospy.spin() def display_image(self, img): """Displays an image on the screen. Arguments: img -- A OpenCV numpy array to be displayed. """ img = cv2.resize(img, (1024, 600)) if len(img.shape) == 2: img = cv2.cvtColor(img, cv2.COLOR_GRAY2BGR) img_msg = self._cv2_to_display_imgmsg(img) self._display_pub.publish(img_msg) ############################################################################ def on_left_image_received(self, img): """Called when a image is received from the left camera. Intended to be overridden. Arguments: img -- The rectified OpenCV numpy array from the camera. """ pass def on_right_image_received(self, img): """Called when a image is received from the right camera. Intended to be overridden. Arguments: img -- The rectified OpenCV numpy array from the camera. """ pass def on_head_image_received(self, img): """Called when a image is received from the head camera. Intended to be overridden. Arguments: img -- The rectified OpenCV numpy array from the camera. """ pass def on_left_itb_received(self, itb): """Called when a left ITB state update is received. Intended to be overridden. Arguments: itb -- The new ITB state. """ pass def on_right_itb_received(self, itb): """Called when a right ITB state update is received. Intended to be overridden. Arguments: itb -- The new ITB state. """ pass ############################################################################ def _camera_imgmsg_to_cv2(self, img_msg): return self._cvbr.imgmsg_to_cv2(img_msg, 'bgr8') def _cv2_to_display_imgmsg(self, img_msg): return self._cvbr.cv2_to_imgmsg(img_msg, 'bgr8') ############################################################################ def _on_left_imagemsg_received(self, img_msg): self.left_img = self._camera_imgmsg_to_cv2(img_msg) self.on_left_image_received(self.left_img) def _on_right_imagemsg_received(self, img_msg): self.right_img = self._camera_imgmsg_to_cv2(img_msg) self.on_right_image_received(self.right_img) def _on_head_imagemsg_received(self, img_msg): img = self._camera_imgmsg_to_cv2(img_msg) img = cv2.flip(img, 0) self.head_img = img self.on_head_image_received(self.head_img) def _on_left_itbmsg_received(self, itb_msg): self.left_itb = itb_msg self.on_left_itb_received(self.left_itb) def _on_right_itbmsg_received(self, itb_msg): self.right_itb = itb_msg self.on_right_itb_received(self.right_itb)
class ArmCommander(Limb): """ This class overloads Limb from the Baxter Python SDK adding the support of trajectories via RobotState and RobotTrajectory messages Allows to control the entire arm either in joint space, or in task space, or (later) with path planning, all with simulation """ def __init__(self, name, rate=100, fk='robot', ik='trac', default_kv_max=1., default_ka_max=0.5): """ :param name: 'left' or 'right' :param rate: Rate of the control loop for execution of motions :param fk: Name of the Forward Kinematics solver, "robot", "kdl", "trac" or "ros" :param ik: Name of the Inverse Kinematics solver, "robot", "kdl", "trac" or "ros" :param default_kv_max: Default K maximum for velocity :param default_ka_max: Default K maximum for acceleration """ Limb.__init__(self, name) self._world = 'base' self.kv_max = default_kv_max self.ka_max = default_ka_max self._gripper = Gripper(name) self._rate = rospy.Rate(rate) self._tf_listener = TransformListener() self.recorder = Recorder(name) # Kinematics services: names and services (if relevant) self._kinematics_names = {'fk': {'ros': 'compute_fk'}, 'ik': {'ros': 'compute_ik', 'robot': 'ExternalTools/{}/PositionKinematicsNode/IKService'.format(name), 'trac': 'trac_ik_{}'.format(name)}} self._kinematics_services = {'fk': {'ros': {'service': rospy.ServiceProxy(self._kinematics_names['fk']['ros'], GetPositionFK), 'func': self._get_fk_ros}, 'kdl': {'func': self._get_fk_pykdl}, 'robot': {'func': self._get_fk_robot}}, 'ik': {'ros': {'service': rospy.ServiceProxy(self._kinematics_names['ik']['ros'], GetPositionIK), 'func': self._get_ik_ros}, 'robot': {'service': rospy.ServiceProxy(self._kinematics_names['ik']['robot'], SolvePositionIK), 'func': self._get_ik_robot}, 'trac': {'service': rospy.ServiceProxy(self._kinematics_names['ik']['trac'], GetConstrainedPositionIK), 'func': self._get_ik_trac}, 'kdl': {'func': self._get_ik_pykdl}}} self._selected_ik = ik self._selected_fk = fk # Kinematics services: PyKDL self._kinematics_pykdl = baxter_kinematics(name) if self._selected_ik in self._kinematics_names['ik']: rospy.wait_for_service(self._kinematics_names['ik'][self._selected_ik]) if self._selected_fk in self._kinematics_names['fk']: rospy.wait_for_service(self._kinematics_names['fk'][self._selected_fk]) # Execution attributes rospy.Subscriber('/robot/limb/{}/collision_detection_state'.format(name), CollisionDetectionState, self._cb_collision, queue_size=1) rospy.Subscriber('/robot/digital_io/{}_lower_cuff/state'.format(name), DigitalIOState, self._cb_dig_io, queue_size=1) self._stop_reason = '' # 'cuff' or 'collision' could cause a trajectory to be stopped self._stop_lock = Lock() action_server_name = "/robot/limb/{}/follow_joint_trajectory".format(self.name) self.client = SimpleActionClient(action_server_name, FollowJointTrajectoryAction) self._display_traj = rospy.Publisher("/move_group/display_planned_path", DisplayTrajectory, queue_size=1) self._gripper.calibrate() self.client.wait_for_server() ######################################### CALLBACKS ######################################### def _cb_collision(self, msg): if msg.collision_state: with self._stop_lock: self._stop_reason = 'collision' def _cb_dig_io(self, msg): if msg.state > 0: with self._stop_lock: self._stop_reason = 'cuff' ############################################################################################# def endpoint_pose(self): """ Returns the pose of the end effector :return: [[x, y, z], [x, y, z, w]] """ pose = Limb.endpoint_pose(self) return [[pose['position'].x, pose['position'].y, pose['position'].z], [pose['orientation'].x, pose['orientation'].y, pose['orientation'].z, pose['orientation'].w]] def endpoint_name(self): return self.name+'_gripper' def group_name(self): return self.name+'_arm' def joint_limits(self): xml_urdf = rospy.get_param('robot_description') dict_urdf = xmltodict.parse(xml_urdf) joints_urdf = [] joints_urdf.append([j['@name'] for j in dict_urdf['robot']['joint'] if j['@name'] in self.joint_names()]) joints_urdf.append([[float(j['limit']['@lower']), float(j['limit']['@upper'])] for j in dict_urdf['robot']['joint'] if j['@name'] in self.joint_names()]) # reorder the joints limits return dict(zip(self.joint_names(), [joints_urdf[1][joints_urdf[0].index(name)] for name in self.joint_names()])) def get_current_state(self, list_joint_names=[]): """ Returns the current RobotState describing all joint states :param list_joint_names: If not empty, returns only the state of the requested joints :return: a RobotState corresponding to the current state read on /robot/joint_states """ if len(list_joint_names) == 0: list_joint_names = self.joint_names() state = RobotState() state.joint_state.name = list_joint_names state.joint_state.position = map(self.joint_angle, list_joint_names) state.joint_state.velocity = map(self.joint_velocity, list_joint_names) state.joint_state.effort = map(self.joint_effort, list_joint_names) return state def get_ik(self, eef_poses, seeds=(), source=None, params=None): """ Return IK solutions of this arm's end effector according to the method declared in the constructor :param eef_poses: a PoseStamped or a list [[x, y, z], [x, y, z, w]] in world frame or a list of PoseStamped :param seeds: a single seed or a list of seeds of type RobotState for each input pose :param source: 'robot', 'trac', 'kdl'... the IK source for this call (warning: the source might not be instanciated) :param params: dictionary containing optional non-generic IK parameters :return: a list of RobotState for each input pose in input or a single RobotState TODO: accept also a Point (baxter_pykdl's IK accepts orientation=None) Child methods wait for a *list* of pose(s) and a *list* of seed(s) for each pose """ if not isinstance(eef_poses, list) or isinstance(eef_poses[0], list) and not isinstance(eef_poses[0][0], list): eef_poses = [eef_poses] if not seeds: seeds=[] elif not isinstance(seeds, list): seeds = [seeds]*len(eef_poses) input = [] for eef_pose in eef_poses: if isinstance(eef_pose, list): input.append(list_to_pose(eef_pose, self._world)) elif isinstance(eef_pose, PoseStamped): input.append(eef_pose) else: raise TypeError("ArmCommander.get_ik() accepts only a list of Postamped or [[x, y, z], [x, y, z, w]], got {}".format(str(type(eef_pose)))) output = self._kinematics_services['ik'][self._selected_ik if source is None else source]['func'](input, seeds, params) return output if len(eef_poses)>1 else output[0] def get_fk(self, frame_id=None, robot_state=None): """ Return The FK solution oth this robot state according to the method declared in the constructor robot_state = None will give the current endpoint pose in frame_id :param robot_state: a RobotState message :param frame_id: the frame you want the endpoint pose into :return: [[x, y, z], [x, y, z, w]] """ if frame_id is None: frame_id = self._world if isinstance(robot_state, RobotState) or robot_state is None: return self._kinematics_services['fk'][self._selected_fk]['func'](frame_id, robot_state) else: raise TypeError("ArmCommander.get_fk() accepts only a RobotState, got {}".format(str(type(robot_state)))) def _get_fk_pykdl(self, frame_id, state=None): if state is None: state = self.get_current_state() fk = self._kinematics_pykdl.forward_position_kinematics(dict(zip(state.joint_state.name, state.joint_state.position))) return [fk[:3], fk[-4:]] def _get_fk_robot(self, frame_id = None, state=None): # Keep this half-working FK, still used by generate_cartesian_path (trajectories.py) if state is not None: raise NotImplementedError("_get_fk_robot has no FK service provided by the robot except for its current endpoint pose") ps = list_to_pose(self.endpoint_pose(), self._world) return self._tf_listener.transformPose(frame_id, ps) def _get_fk_ros(self, frame_id = None, state=None): rqst = GetPositionFKRequest() rqst.header.frame_id = self._world if frame_id is None else frame_id rqst.fk_link_names = [self.endpoint_name()] if isinstance(state, RobotState): rqst.robot_state = state elif isinstance(state, JointState): rqst.robot_state.joint_state = state elif state is None: rqst.robot_state = self.get_current_state() else: raise AttributeError("Provided state is an invalid type") fk_answer = self._kinematics_services['fk']['ros']['service'].call(rqst) if fk_answer.error_code.val==1: return fk_answer.pose_stamped[0] else: return None def _get_ik_pykdl(self, eef_poses, seeds=(), params=None): solutions = [] for pose_num, eef_pose in enumerate(eef_poses): if eef_pose.header.frame_id.strip('/') != self._world.strip('/'): raise NotImplementedError("_get_ik_pykdl: Baxter PyKDL implementation does not accept frame_ids other than {}".format(self._world)) pose = pose_to_list(eef_pose) resp = self._kinematics_pykdl.inverse_kinematics(pose[0], pose[1], [seeds[pose_num].joint_state.position[seeds[pose_num].joint_state.name.index(joint)] for joint in self.joint_names()] if len(seeds)>0 else None) if resp is None: rs = None else: rs = RobotState() rs.is_diff = False rs.joint_state.name = self.joint_names() rs.joint_state.position = resp solutions.append(rs) return solutions def _get_ik_robot(self, eef_poses, seeds=(), params=None): ik_req = SolvePositionIKRequest() for eef_pose in eef_poses: ik_req.pose_stamp.append(eef_pose) ik_req.seed_mode = ik_req.SEED_USER if len(seeds) > 0 else ik_req.SEED_CURRENT for seed in seeds: ik_req.seed_angles.append(seed.joint_state) resp = self._kinematics_services['ik']['robot']['service'].call(ik_req) solutions = [] for j, v in zip(resp.joints, resp.isValid): solutions.append(RobotState(is_diff=False, joint_state=j) if v else None) return solutions def _get_ik_trac(self, eef_poses, seeds=(), params=None): ik_req = GetConstrainedPositionIKRequest() if params is None: ik_req.num_steps = 1 else: ik_req.end_tolerance = params['end_tolerance'] ik_req.num_steps = params['num_attempts'] for eef_pose in eef_poses: ik_req.pose_stamp.append(eef_pose) if len(seeds) == 0: seeds = [self.get_current_state()]*len(eef_poses) for seed in seeds: ik_req.seed_angles.append(seed.joint_state) resp = self._kinematics_services['ik']['trac']['service'].call(ik_req) solutions = [] for j, v in zip(resp.joints, resp.isValid): solutions.append(RobotState(is_diff=False, joint_state=j) if v else None) return solutions def _get_ik_ros(self, eef_poses, seeds=()): rqst = GetPositionIKRequest() rqst.ik_request.avoid_collisions = True rqst.ik_request.group_name = self.group_name() solutions = [] for pose_num, eef_pose in enumerate(eef_poses): rqst.ik_request.pose_stamped = eef_pose # Do we really to do a separate call for each pose? _vector not used ik_answer = self._kinematics_services['ik']['ros']['service'].call(rqst) if len(seeds) > 0: rqst.ik_request.robot_state = seeds[pose_num] if ik_answer.error_code.val==1: # Apply a filter to return only joints of this group try: ik_answer.solution.joint_state.velocity = [value for id_joint, value in enumerate(ik_answer.solution.joint_state.velocity) if ik_answer.solution.joint_state.name[id_joint] in self.joint_names()] ik_answer.solution.joint_state.effort = [value for id_joint, value in enumerate(ik_answer.solution.joint_state.effort) if ik_answer.solution.joint_state.name[id_joint] in self.joint_names()] except IndexError: pass ik_answer.solution.joint_state.position = [value for id_joint, value in enumerate(ik_answer.solution.joint_state.position) if ik_answer.solution.joint_state.name[id_joint] in self.joint_names()] ik_answer.solution.joint_state.name = [joint for joint in ik_answer.solution.joint_state.name if joint in self.joint_names()] solutions.append(ik_answer.solution) else: solutions.append(None) return solutions def translate_to_cartesian(self, path, frame_id, time, n_points=50, max_speed=np.pi/4, min_success_rate=0.5, display_only=False, timeout=0, stop_test=lambda:False, pause_test=lambda:False): """ Translate the end effector in straight line, following path=[translate_x, translate_y, translate_z] wrt frame_id :param path: Path [x, y, z] to follow wrt frame_id :param frame_id: frame_id of the given input path :param time: Time of the generated trajectory :param n_points: Number of 3D points of the cartesian trajectory :param max_speed: Maximum speed for every single joint in rad.s-1, allowing to avoid jumps in joints configuration :param min_success_rate: Raise RuntimeError in case the success rate is lower than min_success_rate :param display_only: :param timeout: In case of cuff interaction, indicates the max time to retry before giving up (default is 0 = do not retry) :param stop_test: pointer to a function that returns True if execution must stop now. /!\ Should be fast, it will be called at 100Hz! :param pause_test: pointer to a function that returns True if execution must pause now. If yes it blocks until pause=False again and relaunches the same goal /!\ Test functions must be fast, they will be called at 100Hz! :return: :raises: RuntimeError if success rate is too low """ def trajectory_callable(start): traj, success_rate = trajectories.generate_cartesian_path(path, frame_id, time, self, None, n_points, max_speed) if success_rate < min_success_rate: raise RuntimeError("Unable to generate cartesian path (success rate : {})".format(success_rate)) return traj return self._relaunched_move_to(trajectory_callable, display_only, timeout, stop_test, pause_test) def move_to_controlled(self, goal, rpy=[0, 0, 0], display_only=False, timeout=15, stop_test=lambda:False, pause_test=lambda:False): """ Move to a goal using interpolation in joint space with limitation of velocity and acceleration :param goal: Pose, PoseStamped or RobotState :param rpy: Vector [Roll, Pitch, Yaw] filled with 0 if this axis must be preserved, 1 otherwise :param display_only: :param timeout: In case of cuff interaction, indicates the max time to retry before giving up :param stop_test: pointer to a function that returns True if execution must stop now. /!\ Should be fast, it will be called at 100Hz! :param pause_test: pointer to a function that returns True if execution must pause now. If yes it blocks until pause=False again and relaunches the same goal /!\ Test functions must be fast, they will be called at 100Hz! :return: None :raises: ValueError if IK has no solution """ assert callable(stop_test) assert callable(pause_test) if not isinstance(goal, RobotState): goal = self.get_ik(goal) if goal is None: raise ValueError('This goal is not reachable') # collect the robot state start = self.get_current_state() # correct the orientation if rpy is set if np.array(rpy).any(): # convert the starting point to rpy pose pos, rot = states.state_to_pose(start, self, True) for i in range(3): if rpy[i]: rpy[i] = rot[i] goal = states.correct_state_orientation(goal, rpy, self) # parameters for trapezoidal method kv_max = self.kv_max ka_max = self.ka_max return self._relaunched_move_to(lambda start: trajectories.trapezoidal_speed_trajectory(goal, start=start ,kv_max=kv_max, ka_max=ka_max), display_only, timeout, stop_test, pause_test) def rotate_joint(self, joint_name, goal_angle, display_only=False, stop_test=lambda:False, pause_test=lambda:False): goal = self.get_current_state() joint = goal.joint_state.name.index(joint_name) # JTAS accepts all angles even out of limits #limits = self.joint_limits()[joint_name] goal.joint_state.position[joint] = goal_angle return self.move_to_controlled(goal, display_only=display_only, stop_test=stop_test, pause_test=pause_test) def _relaunched_move_to(self, trajectory_callable, display_only=False, timeout=15, stop_test=lambda:False, pause_test=lambda:False): """ Relaunch several times (until cuff interaction or failure) a move_to() whose trajectory is generated by the callable passed in parameter :param trajectory_callable: Callable to call to recompute the trajectory :param display_only: :param timeout: In case of cuff interaction, indicates the max time to retry before giving up :param stop_test: pointer to a function that returns True if execution must stop now. /!\ Should be fast, it will be called at 100Hz! :param pause_test: pointer to a function that returns True if execution must pause now. If yes it blocks until pause=False again and relaunches the same goal :return: """ assert callable(trajectory_callable) retry = True t0 = rospy.get_time() while retry and rospy.get_time()-t0 < timeout or timeout == 0: start = self.get_current_state() trajectory = trajectory_callable(start) if display_only: self.display(trajectory) break else: retry = not self.execute(trajectory, test=lambda: stop_test() or pause_test()) if pause_test(): if not stop_test(): retry = True while pause_test(): rospy.sleep(0.1) if timeout == 0: return not display_only and not retry if retry: rospy.sleep(1) return not display_only and not retry def get_random_pose(self): # get joint names joint_names = self.joint_names() # create a random joint state bounds = [] for key, value in self.joint_limits().iteritems(): bounds.append(value) bounds = np.array(bounds) joint_state = np.random.uniform(bounds[:, 0], bounds[:, 1], len(joint_names)) # append it in a robot state goal = RobotState() goal.joint_state.name = joint_names goal.joint_state.position = joint_state goal.joint_state.header.stamp = rospy.Time.now() goal.joint_state.header.frame_id = 'base' return goal ######################## OPERATIONS ON TRAJECTORIES # TO BE MOVED IN trajectories.py def interpolate_joint_space(self, goal, total_time, nb_points, start=None): """ Interpolate a trajectory from a start state (or current state) to a goal in joint space :param goal: A RobotState to be used as the goal of the trajectory param total_time: The time to execute the trajectory :param nb_points: Number of joint-space points in the final trajectory :param start: A RobotState to be used as the start state, joint order must be the same as the goal :return: The corresponding RobotTrajectory """ dt = total_time*(1.0/nb_points) # create the joint trajectory message traj_msg = JointTrajectory() rt = RobotTrajectory() if start == None: start = self.get_current_state() joints = [] start_state = start.joint_state.position goal_state = goal.joint_state.position for j in range(len(goal_state)): pose_lin = np.linspace(start_state[j],goal_state[j],nb_points+1) joints.append(pose_lin[1:].tolist()) for i in range(nb_points): point = JointTrajectoryPoint() for j in range(len(goal_state)): point.positions.append(joints[j][i]) # append the time from start of the position point.time_from_start = rospy.Duration.from_sec((i+1)*dt) # append the position to the message traj_msg.points.append(point) # put name of joints to be moved traj_msg.joint_names = self.joint_names() # send the message rt.joint_trajectory = traj_msg return rt def display(self, trajectory): """ Display a joint-space trajectory or a robot state in RVIz loaded with the Moveit plugin :param trajectory: a RobotTrajectory, JointTrajectory, RobotState or JointState message """ # Publish the DisplayTrajectory (only for trajectory preview in RViz) # includes a convert of float durations in rospy.Duration() def js_to_rt(js): rt = RobotTrajectory() rt.joint_trajectory.joint_names = js.name rt.joint_trajectory.points.append(JointTrajectoryPoint(positions=js.position)) return rt dt = DisplayTrajectory() if isinstance(trajectory, RobotTrajectory): dt.trajectory.append(trajectory) elif isinstance(trajectory, JointTrajectory): rt = RobotTrajectory() rt.joint_trajectory = trajectory dt.trajectory.append(rt) elif isinstance(trajectory, RobotState): dt.trajectory.append(js_to_rt(trajectory.joint_state)) elif isinstance(trajectory, JointState): dt.trajectory.append(js_to_rt(trajectory)) else: raise NotImplementedError("ArmCommander.display() expected type RobotTrajectory, JointTrajectory, RobotState or JointState, got {}".format(str(type(trajectory)))) self._display_traj.publish(dt) def execute(self, trajectory, test=None, epsilon=0.1): """ Safely executes a trajectory in joint space on the robot or simulate through RViz and its Moveit plugin (File moveit.rviz must be loaded into RViz) This method is BLOCKING until the command succeeds or failure occurs i.e. the user interacted with the cuff or collision has been detected Non-blocking needs should deal with the JointTrajectory action server :param trajectory: either a RobotTrajectory or a JointTrajectory :param test: pointer to a function that returns True if execution must stop now. /!\ Should be fast, it will be called at 100Hz! :param epsilon: distance threshold on the first point. If distance with first point of the trajectory is greater than espilon execute a controlled trajectory to the first point. Put float(inf) value to bypass this functionality :return: True if execution ended successfully, False otherwise """ def distance_to_first_point(point): joint_pos = np.array(point.positions) return np.linalg.norm(current_array - joint_pos) self.display(trajectory) with self._stop_lock: self._stop_reason = '' if isinstance(trajectory, RobotTrajectory): trajectory = trajectory.joint_trajectory elif not isinstance(trajectory, JointTrajectory): raise TypeError("Execute only accept RobotTrajectory or JointTrajectory") ftg = FollowJointTrajectoryGoal() ftg.trajectory = trajectory # check if it is necessary to move to the first point current = self.get_current_state() current_array = np.array([current.joint_state.position[current.joint_state.name.index(joint)] for joint in trajectory.joint_names]) if distance_to_first_point(trajectory.points[0]) > epsilon: # convert first point to robot state rs = RobotState() rs.joint_state.name = trajectory.joint_names rs.joint_state.position = trajectory.points[0].positions # move to the first point self.move_to_controlled(rs) # execute the input trajectory self.client.send_goal(ftg) # Blocking part, wait for the callback or a collision or a user manipulation to stop the trajectory while self.client.simple_state != SimpleGoalState.DONE: if callable(test) and test(): self.client.cancel_goal() return True if self._stop_reason!='': self.client.cancel_goal() return False self._rate.sleep() return True def close(self): """ Open the gripper :return: True if an object has been grasped """ return self._gripper.close(True) def open(self): """ Close the gripper return: True if an object has been released """ return self._gripper.open(True) def gripping(self): return self._gripper.gripping() def wait_for_human_grasp(self, threshold=1, rate=10, ignore_gripping=True): """ Blocks until external motion is given to the arm :param threshold: :param rate: rate of control loop in Hertz :param ignore_gripping: True if we must wait even if no object is gripped """ def detect_variation(): new_effort = np.array(self.get_current_state([self.name+'_w0', self.name+'_w1', self.name+'_w2']).joint_state.effort) delta = np.absolute(effort - new_effort) return np.amax(delta) > threshold # record the effort at calling time effort = np.array(self.get_current_state([self.name+'_w0', self.name+'_w1', self.name+'_w2']).joint_state.effort) # loop till the detection of changing effort rate = rospy.Rate(rate) while not detect_variation() and not rospy.is_shutdown() and (ignore_gripping or self.gripping()): rate.sleep()
class DmpLibrary(object): """ The DMP Library class, currently supports the following DMPs * Lateral grip * Overhead grip * return Current non DMP actions that are supported * pour constructor: required arg -> dmp_record_file_dir : Path to the directory with dmp record files optional arg -> dmp_dims : The dimensions of the dmp to be trained (default: 7) lateral_record_file : Name of the lateral record file (Default: lat_record_file) vertical_record_file : Name of the vertical record file (Default: ver_record_file) return_record_file : Name of the return record file (Default: return_record_file) dummy : If set true, it will not execute but will only plan (Default: False) """ def __init__(self, dmp_record_file_dir, dmp_dims=7, lateral_record_file=LATERAL_RECORD_FILE, vertical_record_file=VERTICAL_RECORD_FILE, return_record_file=RETURN_RECORD_FILE, dummy=False): self.K = 100 # Spring constant, value taken from http://wiki.ros.org/dmp self.D = 2.0 * np.sqrt(self.K) # Damping constant, value taken from http://wiki.ros.org/dmp self.dt = 1.0 # Time resolution, Value taken from http://wiki.ros.org/dmp self.num_bases = 4 # No of basis functions, value taken from http://wiki.ros.org/dmp self.dmp_record_file_dir = dmp_record_file_dir self.dmp_dims = dmp_dims self.lateral_record_file = os.path.join(self.dmp_record_file_dir, lateral_record_file) self.vertical_record_file = os.path.join(self.dmp_record_file_dir, vertical_record_file) self.return_record_file = os.path.join(self.dmp_record_file_dir, return_record_file) self.dummy = dummy self.tf_listener = tf.TransformListener() # the init velocity and goal threshold doesn't change across dmps, so just store them now self.init_dot = [] for i in range(self.dmp_dims): self.init_dot.append(0) self.goal_thresh = [] for i in range(self.dmp_dims): self.goal_thresh.append(0) # Initialize move it group moveit_commander.roscpp_initialize(sys.argv) self.robot = moveit_commander.RobotCommander() self.group = moveit_commander.MoveGroupCommander("right_arm") self.display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=10) self.integrate_iter = 5 # value taken from http://wiki.ros.org/dmp self.seq_length = -1 self.right_gripper = Gripper('right') # save the initial state try: self.init_pos, self.init_orient = self.tf_listener.lookupTransform('/base', '/right_gripper', rospy.Time(0)) #Plan to a different cup than demo except: time.sleep(1) self.init_pos, self.init_orient = self.tf_listener.lookupTransform('/base', '/right_gripper', rospy.Time(0)) self.right_gripper.calibrate() def getDMPplan(self, start_state, traj, goal): """ Function provided by the tutorial http://wiki.ros.org/dmp """ demotraj = DMPTraj() for i in range(len(traj)): pt = DMPPoint(); pt.positions = traj[i] demotraj.points.append(pt) demotraj.times.append(self.dt*i) k_gains = [self.K]*self.dmp_dims d_gains = [self.D]*self.dmp_dims try: # Learn from the trajectory (can probably move it to a dictionary) rospy.wait_for_service('learn_dmp_from_demo') learner_service = rospy.ServiceProxy('learn_dmp_from_demo', LearnDMPFromDemo) learned_dmp = learner_service(demotraj, k_gains, d_gains, self.num_bases) # Make the new dmp the current one rospy.wait_for_service('set_active_dmp') sad = rospy.ServiceProxy('set_active_dmp', SetActiveDMP) sad(learned_dmp.dmp_list) # Return a plan for the dmp rospy.wait_for_service('get_dmp_plan') gdp = rospy.ServiceProxy('get_dmp_plan', GetDMPPlan) plan = gdp(start_state, self.init_dot, 0, goal, self.goal_thresh, self.seq_length, 2* learned_dmp.tau, self.dt, self.integrate_iter) except rospy.ServiceException, e: print "There was an issue, with the service"%e return plan;
class track(): def __init__(self): self.centerx = 365 self.centery = 120 self.coefx = 0.1/(526-369) self.coefy = 0.1/(237-90) self.count = 0 self.hdr = Header(stamp=rospy.Time.now(), frame_id='base') self.gripper = Gripper("right") self.gripper.calibrate() rospy.sleep(2) # self.gripper.set_moving_force(0.1) # rospy.sleep(0.5) # self.gripper.set_holding_force(0.1) # rospy.sleep(0.5) self.busy = False self.gripper_distance = 100 self.subscribe_gripper() self.robotx = -1 self.roboty = -1 self.framenumber = 0 self.history = np.arange(0,20)*-1 self.newPosition = True self.bowlcamera = None self.kin = baxter_kinematics('right') self.J = self.kin.jacobian() self.J_inv = self.kin.jacobian_pseudo_inverse() self.jointVelocity = np.asarray([1,2,3,4,5,6,7],np.float32) self.control_arm = Limb("right") self.control_joint_names = self.control_arm.joint_names() self.dx = 0 self.dy = 0 self.distance = 1000 self.finish = False self.found = 0 ifr = rospy.Subscriber("/robot/range/right_hand_range/state", Range, self._read_distance) self.joint_states = { # 'observe':{ # 'right_e0': -0.365, # 'right_e1': 1.061, # 'right_s0': 0.920, # 'right_s1': -0.539, # 'right_w0': 0.350, # 'right_w1': 1.105, # 'right_w2': -0.221, # }, 'observe':{ 'right_e0': -0.631, 'right_e1': 0.870, 'right_s0': 0.742, 'right_s1': -0.6087, 'right_w0': 0.508, 'right_w1': 1.386, 'right_w2': -0.5538, }, 'observe_vertical':{ 'right_e0': 0.699, 'right_e1': 1.451, 'right_s0': -1.689, 'right_s1': 0.516, 'right_w0': 0.204, 'right_w1': 0.935, 'right_w2': -2.706, }, 'observe_ladle':{ 'right_e0': -0.829, 'right_e1': 0.831, 'right_s0': 0.678, 'right_s1': -0.53, 'right_w0': 0.716, 'right_w1': 1.466, 'right_w2': -0.8099, }, } def _read_distance(self,data): self.distance = data.range def vision_request_right(self, controlID, requestID): try: rospy.wait_for_service('vision_server_vertical') vision_server_req = rospy.ServiceProxy('vision_server_vertical', VisionVertical) return vision_server_req(controlID, requestID) except (rospy.ServiceException,rospy.ROSInterruptException), e: print "Service call failed: %s" % e self.clean_shutdown_service()
#!/usr/bin/env python import rospy from baxter_interface import Gripper if __name__ == '__main__': rospy.init_node('gripperTools', anonymous=True) gripper = Gripper('right') gripper.close(True)
class BlockStackerNode(object): RATE = 250 ############################################################################ def __init__(self): self._rs = RobotEnable() self._cvbr = CvBridge() self._sm = RobotStateMachine(self) self.ik = IKHelper() self.ik.set_right(0.5, 0.0, 0.0, wait=True) self.left_img = None self.right_img = None self._left_camera_sub = rospy.Subscriber( '/cameras/left_hand_camera/image_rect_avg', Image, self.on_left_imagemsg_received) self._right_camera_sub = rospy.Subscriber( '/cameras/right_hand_camera/image_rect_avg', Image, self.on_right_imagemsg_received) self._display_pub = rospy.Publisher( '/robot/xdisplay', Image, tcp_nodelay=True, latch=True) self.range = None self._range_sub = rospy.Subscriber( '/robot/range/right_hand_range/state', Range, self.on_rangemsg_received) self.itb = None self._itb_sub = rospy.Subscriber( '/robot/itb/right_itb/state', ITBState, self.on_itbmsg_received) self.gripper = Gripper('right') self.gripper.calibrate() self.gripper.close(block=True) ############################################################################ def start(self): self._rs.enable() self._sm.start() rate = rospy.Rate(BlockStackerNode.RATE) while not rospy.is_shutdown(): self._sm.run_step() rate.sleep() ############################################################################ def on_left_imagemsg_received(self, img_msg): img = self._cvbr.imgmsg_to_cv2(img_msg, 'bgr8') img = cv2.resize(img, (640, 400)) self.left_img = img.copy() self._sm.on_left_image_received(img) def on_right_imagemsg_received(self, img_msg): img = self._cvbr.imgmsg_to_cv2(img_msg, 'bgr8') img = cv2.resize(img, (640, 400)) self.right_img = img.copy() self._sm.on_right_image_received(img) def on_rangemsg_received(self, range_msg): self.range = range_msg.range def on_itbmsg_received(self, itb_msg): self.itb = itb_msg def display_image(self, img): img = cv2.resize(img, (1024, 600)) print img.shape if img.shape[2] == 1: img = cv2.cvtColor(img, cv2.COLOR_GRAY2BGR) img_msg = self._cvbr.cv2_to_imgmsg(img, 'bgr8') self._display_pub.publish(img_msg)