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 __init__(self, limb, starting_pos=None, push_thresh=100, mode='positions', rgbd=False): self.limb_name = limb self._limb = baxter_interface.Limb(self.limb_name) self._neutral_pos = starting_pos self._mode = mode rospy.loginfo("Moving to neutral position") self.move_to_neutral() rospy.loginfo("Initialising PoseGenerator") self._pg = PoseGenerator(limb, self._mode, self._limb.endpoint_pose())
class ArmController(object): def __init__(self, limb, starting_pos=None, push_thresh=100, mode='positions', rgbd=False): self.limb_name = limb self._limb = baxter_interface.Limb(self.limb_name) self._neutral_pos = starting_pos self._mode = mode rospy.loginfo("Moving to neutral position") self.move_to_neutral() rospy.loginfo("Initialising PoseGenerator") self._pg = PoseGenerator(limb, self._mode, self._limb.endpoint_pose()) # rospy.loginfo("Recording offset") # self.set_offset() def move_to_neutral(self): self._limb.move_to_joint_positions(self._neutral_pos) def set_offset(self): pose = self._limb.endpoint_pose() self.initial_end_pose = pose eu = tf.transformations.euler_from_quaternion(pose['orientation']) self.baxter_off.linear.x = pose['position'][0] self.baxter_off.linear.y = pose['position'][1] self.baxter_off.linear.z = pose['position'][2] self.baxter_off.angular.x = eu[0] self.baxter_off.angular.y = eu[1] self.baxter_off.angular.z = eu[2] def is_pushing(self): e = self._limb.joint_efforts() s = sum([abs(e[i]) for i in e.keys()]) return s > self.push_thresh def step(self): pos = self._pg.gen_next() if pos is not None: print "Moving to: " + str(pos) self._limb.move_to_joint_positions(pos, timeout=0.2) else: rospy.logwarn("Generated pos is null!")
class ArmController(object): 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 move_to_neutral(self): if self._mode == "one_arm": self._right_limb.move_to_joint_positions(self._right_neutral_pos) elif self._mode == "two_arms": self._right_limb.move_to_joint_positions(self._right_neutral_pos) self._left_limb.move_to_joint_positions(self._left_neutral_pos) else: raise ValueError("Mode %s is invalid!" % self._mode) def is_right_pushing(self): """ Checks if any of the joints is under external stress. Returns true if the maximum recorded stress above specified threshold. """ e = self._right_limb.joint_efforts() max_effort = max([abs(e[i]) for i in e.keys()]) return max_effort > self.push_thresh def is_left_pushing(self): """ Checks if any of the joints is under external stress. Returns true if the maximum recorded stress above specified threshold. """ e = self._left_limb.joint_efforts() max_effort = max([abs(e[i]) for i in e.keys()]) return max_effort > self.push_thresh def _command_right_gripper(self): """ Reads state from Myo and opens/closes gripper as needed. """ if not self._right_gripper.ready(): return if self._right_gripper.moving(): return if self._is_right_fist_closed: self._right_gripper.close() else: self._right_gripper.open() def _command_left_gripper(self): """ Reads state from Myo and opens/closes gripper as needed. """ if not self._left_gripper.ready(): return if self._left_gripper.moving(): return if self._is_left_fist_closed: self._left_gripper.close() else: self._left_gripper.open() def step(self): """ Executes a step of the main routine. Fist checks the status of the gripper and """ os.system('clear') if self._mode == "one_arm": return self.one_arm_step() elif self._mode == "two_arms": return self.two_arms_step() else: raise ValueError("Mode %s is invalid!" % self.mode) def one_arm_step(self): self._command_right_gripper() pos = self._pg.generate_pose() if pos is not None: if not self.is_right_pushing(): self._right_limb.set_joint_positions(pos) else: rospy.logwarn("Arm is being pushed!") else: rospy.logwarn("Generated position is invalid") def two_arms_step(self): self._command_right_gripper() self._command_left_gripper() poss = self._pg.generate_pose() if poss is not None: if not self.is_right_pushing(): self._right_limb.set_joint_positions(poss[0]) if not self.is_left_pushing(): self._left_limb.set_joint_positions(poss[1]) else: rospy.logwarn("Arm is being pushed!") else: rospy.logwarn("Generated position is invalid") def _right_gesture_callback(self, data): self._is_right_fist_closed = (data.data == 1) def _left_gesture_callback(self, data): self._is_left_fist_closed = (data.data != 0)