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)
class Baxter(object): """ Iinterface for controlling the real and simulated Baxter robot. """ def __init__(self, sim=False, config=None, time_step=1.0, rate=100.0, missed_cmds=20000.0): """ Initialize Baxter Interface Args: sim (bool): True specifies using the PyBullet simulator False specifies using the real robot arm (str): 'right' or 'left' control (int): Specifies control type TODO: change to str ['ee', 'position', 'velocity'] config (class): Specifies joint ranges, ik null space paramaters, etc time_step (float): TODO: probably get rid of this rate (float): missed_cmds (float): """ self.sim = sim self.dof = { 'left': { 'ee': 6, 'position': 7, 'velocity': 7 }, 'right': { 'ee': 6, 'position': 7, 'velocity': 7 }, 'both': { 'ee': 12, 'position': 14, 'velocity': 14 } } if self.sim: import pybullet as p import baxter_pybullet_interface as pybullet_interface self.baxter_path = "assets/baxter_robot/baxter_description/urdf/baxter.urdf" self.time_step = time_step else: import rospy from baxter_pykdl import baxter_kinematics import baxter_interface from baxter_interface import CHECK_VERSION, Limb, Gripper from geometry_msgs.msg import PoseStamped, Pose, Point, Quaternion from std_msgs.msg import Header from baxter_core_msgs.srv import SolvePositionIK, SolvePositionIKRequest self.rate = rate self.freq = 1 / rate self.missed_cmds = missed_cmds self.config = config self.initial_setup() def set_command_time_out(self): """ Set command timeout for sending ROS commands """ if self.sim: pass else: self.left_arm.set_command_timeout(self.freq * self.missed_cmds) self.right_arm.set_command_timeout(self.freq * self.missed_cmds) return def initial_setup(self): if self.sim: # load baxter objects = [p.loadURDF(self.baxter_path, useFixedBase=True)] self.baxter_id = objects[0] print("baxter_id: ", self.baxter_id) if self.config: self.use_nullspace = True self.use_orientation = True self.ll = self.config.nullspace.ll self.ul = self.config.nullspace.ul self.jr = self.config.nullspace.jr self.rp = self.config.nullspace.rp else: self.use_nullspace = False self.create_arms() else: self.create_arms() self.set_command_time_out() self.control_rate = rospy.Rate(self.rate) def reset(self, initial_pose=None): if self.sim: self._reset_sim(initial_pose) else: self._reset_real(initial_pose) def _reset_real(self, initial_pose=None): self.enable() time.sleep(0.5) self.set_initial_position('right') self.set_initial_position('left', blocking=True) self.calibrate_grippers() def _reset_sim(self, control_type=None, initial_pose=None): # set control type if control_type == 'position' or control_type == 'ee' or control_type is None: control_mode = p.POSITION_CONTROL elif control_type == 'velocity': control_mode = p.VELOCITY_CONTROL elif control_mode == 'torque': control_mode = p.TORQUE_CONTROL else: raise ValueError( "control_type must be in ['position', 'ee', 'velocity', 'torque']." ) # set initial position if initial_pose: for joint_index, joint_val in initial_pose: p.resetJointState(0, joint_index, joint_val) p.setJointMotorControl2(self.baxter_id, jointIndex=joint_index, controlMode=control_mode, targetPosition=joint_val) else: num_joints = p.getNumJoints(self.baxter_id) for joint_index in range(num_joints): p.setJointMotorControl2(self.baxter_id, joint_index, control_mode, maxForce=self.config.max_force) return 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 def calibrate_grippers(self): """ (Blocking) Calibrates gripper(s) if not yet calibrated """ if not self.left_arm.gripper.calibrated(): self.left_arm.gripper.calibrate() if not self.right_arm.gripper.calibrated(): self.right_arm.gripper.calibrate() return def enable(self): self._rs = baxter_interface.RobotEnable(CHECK_VERSION) self._init_state = self._rs.state().enabled self._rs.enable() def shutdown(self): pass def set_joint_velocities(self, arm, joint_velocities): """ Set joint velocites for a given arm Args arm (str): 'left' or 'right' or 'both' joint_velocities (list or tuple or numpy array): Array of len 7 or 14 """ if arm == 'left': action_dict = self.create_action_dict('left', 'velocity', joint_velocities) self.left_arm.set_joint_velocities(action_dict) elif arm == 'right': action_dict = self.create_action_dict('right', 'velocity', joint_velocities) self.right_arm.set_joint_velocities(action_dict) elif arm == 'both': l_velocities = joint_velocities[:7] r_velocities = joint_velocities[7:] l_action_dict = self.create_action_dict('left', 'velocity', l_velocities) r_action_dict = self.create_action_dict('right', 'velocity', r_velocities) self.left_arm.set_joint_velocities(l_action_dict) self.right_arm.set_joint_velocities(r_action_dict) return def move_to_joint_positions(self, arm, joint_positions): """ Move specified arm to give joint positions (This function is blocking.) Args arm (str): 'left' or 'right' or 'both' joint_positionns (list or tuple or numpy array): Array of len 7 or 14 """ if arm == 'left': action_dict = self.create_action_dict('left', 'position', joint_positions) self.left_arm.move_to_joint_positions(action_dict) elif arm == 'right': action_dict = self.create_action_dict('right', 'position', joint_positions) self.right_arm.move_to_joint_positions(action_dict) elif arm == 'both': l_joints = joint_positions[:7] r_joints = joint_positions[7:] l_action_dict = self.create_action_dict('left', 'position', l_joints) r_action_dict = self.create_action_dict('right', 'position', r_joints) self.left_arm.move_to_joint_positions(l_action_dict) self.right_arm.move_to_joint_positions(r_action_dict) return def set_joint_positions(self, arm, joint_positions): """ Move specified arm to give joint positions (This function is not blocking.) Args arm (str): 'left' or 'right' or 'both' joint_positionns (list or tuple or numpy array): Array of len 7 or 14 """ if arm == 'left': action_dict = self.create_action_dict('left', 'position', joint_positions) self.left_arm.set_joint_positions(action_dict) elif arm == 'right': action_dict = self.create_action_dict('right', 'position', joint_positions) self.right_arm.set_joint_positions(action_dict) elif arm == 'both': l_joints = joint_positions[:7] r_joints = joint_positions[7:] l_action_dict = self.create_action_dict('left', 'position', l_joints) r_action_dict = self.create_action_dict('right', 'position', r_joints) self.left_arm.set_joint_positions(l_action_dict) self.right_arm.set_joint_positions(r_action_dict) return def move_to_ee_pose(self, arm, pose, blocking=True): """ Move end effector to specified pose Args arm (string): "left" or "right" or "both" pose (list): if arm == 'left' or arm == 'right': pose = [X, Y, Z, r, p, w] else: pose = left_pose + right _pose """ if arm == "both": left_pose = pose[:6] right_pose = pose[6:] left_joints = self.ik('left', left_pose) right_joints = self.ik('right', right_pose) if blocking: self.left_arm.move_to_joint_positions(left_joints) self.right_arm.move_to_joint_positions(right_joints) else: self.left_arm.set_joint_positions(left_joints) self.right_arm.set_joint_positions(right_joints) elif arm == "left": joints = self.ik(arm, pose) if blocking: self.left_arm.move_to_joint_positions(joints) else: self.left_arm.set_joint_positions(joints) elif arm == "right": joints = self.ik(arm, pose) if blocking: self.right_arm.move_to_joint_positions(joints) else: self.right_arm.set_joint_positions(joints) else: raise ValueError("Arm must be 'right', 'left', or 'both'") return def get_ee_pose(self, arm, mode=None): """ Returns end effector pose for specified arm. End effector pose is a list of values corresponding to the 3D cartesion coordinates and roll (r), pitch (p), and yaw (w) Euler angles. Args arm (string): "right" or "left" mode (string): "Quaternion" or "quaterion" or "quat" Returns pose (list): Euler angles [X,Y,Z,r,p,w] or Quaternion [X,Y,Z,x,y,z,w] """ if arm == 'left' or arm == 'right': pos = self.get_ee_position(arm) orn = self.get_ee_orientation(arm, mode) return pos + orn elif arm == 'both': l_pos = self.get_ee_position('left') l_orn = self.get_ee_orientation('left', mode) r_pos = self.get_ee_position('right') r_orn = self.get_ee_orientation('right', mode) return l_pos + l_orn + r_pos + r_orn def get_ee_position(self, arm): """ Returns end effector position for specified arm. Returns the 3D cartesion coordinates of the end effector. Args arm (string): "left" or "right" or "both" Returns [X,Y,Z] """ if arm not in ['left', 'right', 'both']: raise ValueError("Arg arm should be 'left' or 'right' or 'both'.") if arm == "left": return list(self.left_arm.endpoint_pose()['position']) elif arm == "right": return list(self.right_arm.endpoint_pose()['position']) elif arm == "both": return list(self.left_arm.endpoint_pose()['position']) + list( self.right_arm.endpoint_pose()['position']) def get_ee_orientation(self, arm, mode=None): """ Returns a list of the orientations of the end effectors(s) Args arm (string): "right" or "left" or "both" mode (string): specifies angle representation Returns orn (list): list of Euler angles or Quaternion """ if arm not in ['left', 'right', 'both']: raise ValueError("Arg arm should be 'left' or 'right'.") if arm == "left": orn = list(self.left_arm.endpoint_pose()['orientation']) elif arm == "right": orn = list(self.right_arm.endpoint_pose()['orientation']) elif arm == "both": orn = list(self.left_arm.endpoint_pose()['orientation']) + list( self.right_arm.endpoint_pose()['orientation']) return list(p.getEulerFromQuaternion(orn)) def get_joint_angles(self, arm): """ Get joint angles for specified arm Args arm(strin): "right" or "left" or "both" Returns joint_angles (list): List of joint angles starting from the right_s0 ('right_upper_shoulder') and going down the kinematic tree to the end effector. """ if arm == "left": joint_angles = self.left_arm.joint_angles() elif arm == "right": joint_angles = self.right_arm.joint_angles() elif arm == "both": joint_angles = self.left_arm.joint_angles( ) + self.right_arm.joint_angles() else: raise ValueError("Arg arm should be 'left' or 'right' or 'both'.") if not self.sim: joint_angles = joint_angles.values() return joint_angles def get_joint_velocities(self, arm): """ Get joint velocites for specified arm """ if arm == "left": return self.left_arm.joint_velocities() elif arm == "right": return self.right_arm.joint_velocities() elif arm == "both": return self.left_arm.joint_velocities( ) + self.right_arm.joint_velocities() else: raise ValueError("Arg arm should be 'left' or 'right' or 'both'.") if not self.sim: joint_velocities = joint_velocities.values() return joint_velocities def get_joint_efforts(self, arm): """ Get joint torques for specified arm """ if arm == "left": return self.left_arm.joint_effort() elif arm == "right": return self.right_arm.joint_efforts() elif arm == "both": return self.left_arm.joint_efforts( ) + self.right_arm.joint_efforts() else: raise ValueError("Arg arm should be 'left' or 'right' or 'both'.") if not self.sim: joint_efforts = joint_efforts.values() return def apply_action(self, arm, control_type, action): """ Apply a joint action Blocking on real robot Args action - list or tuple specifying end effector pose(6DOF * num_arms) or joint angles (7DOF * num_arms) """ # verify action verified, err = self._verify_action(arm, control_type, action) if not verified: raise err # execute action if control_type == 'position': self._apply_position_control(arm, action) elif control_type == 'ee': self._apply_ee_control(arm, action) elif control_type == 'velocity': self._apply_velocity_control(arm, action) elif control_type == 'torque': self._apply_torque_control(arm, action) else: raise ValueError( "Control type must be ['ee', 'position', 'velocity', 'torque']" ) def _verify_action(self, arm, control_type, action): """ Verify type and dimension of action Args arm (str): "left" or "right" or "both" action (list): list of floats len will vary depending on action type Returns bool: True if action is right dimension, false otherwise """ if arm not in ["left", "right", "both"]: return False, ValueError("Arg arm must be string") if control_type not in ['ee', 'position', 'velocity', 'torque']: return False, ValueError( "Arg control_type must be one of ['ee', 'position', 'velocity', 'torque']" ) if not isinstance(action, (list, tuple, np.ndarray)): return False, TypeError("Action must be a list or tuple.") if len(action) != self.dof[arm][control_type]: return False, ValueError("Action must have len {}".format( self.dof * num_arms)) return True, "" def _apply_torque_control(self, arm, action): """ As of right now, setting joint torques does not does not command the robot as expected """ raise NotImplementedError( 'Cannot apply torque control. Try using joint position or velocity control' ) def _apply_velocity_control(self, arm, action): """ Apply velocity control to a given arm Args arm (str): 'right' or 'left' action (list, tuple, or numpy array) of len 7 """ if self.sim: pass else: if arm == 'left': action_dict = self self.right_arm.set_joint_velocities if arm == 'right': pass if arm == 'both': pass return def _apply_position_control(self, arm, action, blocking=True): """ Apply a joint action Args: arm (str): 'right', 'left', or 'both' action - list or array of joint angles blocking (bool): If true, wait for arm(s) to reach final position(s) """ action = list(action) if blocking: self.move_to_joint_positions(arm, action) else: self.set_joint_positions(arm, action) return def _apply_ee_control(self, arm, action, blocking=True): """ Apply action to move Baxter end effector(s) Args arm (str): 'left' or 'right' or 'both' action (list, tuple, or numpy array) blocking: Bool """ action = list(action) if self.sim: if arm == 'left' or arm == 'right': self.move_to_ee_pose(arm, action) elif arm == 'both': if self.sim: joint_indices = self.left_arm.indices + self.right_arm.indices self.left_arm.move_to_joint_positions( actions, joint_indices) else: self.move_to_ee_pose(arm, action, blocking) return def fk(self, arm, joints): """ Calculate forward kinematics Args arm (str): 'right' or 'left' joints (list): list of joint angles Returns pose(list): [x,y,z,r,p,w] """ if arm == 'right': pose = list(self.right_arm.kin.forward_position_kinematics(joints)) elif arm == 'left': pose = list(self.left_arm.kin.forward_position_kinematics(joints)) else: raise ValueError("Arg arm must be 'right' or 'left'") return pose[:3] + list(self.quat_to_euler(pose[3:])) def _ik(self, arm, pos, orn=None, seed=None): """ Calculate inverse kinematics Args arm (str): 'right' or 'left' pos (list): [X, Y, Z] orn (list): [r, p, w] seed (int): for setting random seed Returns joint angles (list): A list of joint angles Note: This will probably fail if orn is not included """ if orn is not None: orn = list(self.euler_to_quat(orn)) if arm == 'right': joint_angles = self.right_arm.kin.inverse_kinematics(pos, orn) elif arm == 'left': joint_angles = self.left_arm.kin.inverse_kinematics(pos, orn, seed) else: raise ValueError("Arg arm must be 'right' or 'left'") return list(joint_angles.tolist()) def ik(self, arm, ee_pose): """ Calculate inverse kinematics for a given end effector pose Args ee_pose (list or tuple of floats): 6 dimensional array specifying the position and orientation of the end effector arm (string): "right" or "left" Return joints (list): A list of joint angles """ if self.sim: joints = self._sim_ik(arm, ee_pose) else: joints = self._real_ik(arm, ee_pose) if not joints: print("IK failed. Try running again or changing the pose.") return joints def _sim_ik(self, arm, ee_pose): """ (Sim) Calculate inverse kinematics for a given end effector pose Args: ee_pose (tuple or list): [pos, orn] of desired end effector pose pos - x,y,z orn - r,p,w arm (string): "right" or "left" Returns: joint_angles (list): List of joint angles """ if arm == 'right': ee_index = self.right_arm.ee_index elif arm == 'left': ee_index = self.left_arm.ee_index elif arm == 'both': pass else: raise ValueError("Arg arm must be 'left', 'right', or 'both'.") pos = ee_pose[:3] orn = ee_pose[3:] if (self.use_nullspace == 1): if (self.use_orientation == 1): joints = p.calculateInverseKinematics(self.baxter_id, ee_index, pos, orn, self.ll, self.ul, self.jr, self.rp) else: joints = p.calculateInverseKinematics(self.baxter_id, ee_index, pos, lowerLimits=self.ll, upperLimits=self.ul, jointRanges=self.jr, restPoses=self.rp) else: if (self.use_orientation == 1): joints = p.calculateInverseKinematics(self.baxter_id, ee_index, pos, orn, jointDamping=self.jd) else: joints = p.calculateInverseKinematics(self.baxter_id, ee_index, pos) return joints def _real_ik(self, arm, ee_pose): """ (Real) Calculate inverse kinematics for a given end effector pose Args: ee_pose (tuple or list): [pos, orn] of desired end effector pose pos - x,y,z orn - r,p,w Returns: joint_angles (dict): Dictionary containing {'joint_name': joint_angle} """ pos = ee_pose[:3] orn = ee_pose[3:] orn = self.euler_to_quat(orn) ns = "ExternalTools/" + arm + "/PositionKinematicsNode/IKService" iksvc = rospy.ServiceProxy(ns, SolvePositionIK) ikreq = SolvePositionIKRequest() hdr = Header(stamp=rospy.Time.now(), frame_id='base') ik_pose = PoseStamped() ik_pose.pose.position.x = pos[0] ik_pose.pose.position.y = pos[1] ik_pose.pose.position.z = pos[2] ik_pose.pose.orientation.x = orn[0] ik_pose.pose.orientation.y = orn[1] ik_pose.pose.orientation.z = orn[2] ik_pose.pose.orientation.w = orn[3] ik_pose.header = hdr ikreq.pose_stamp.append(ik_pose) try: rospy.wait_for_service(ns, 5.0) resp = iksvc(ikreq) limb_joints = dict( zip(resp.joints[0].name, resp.joints[0].position)) return limb_joints except (rospy.ServiceException, rospy.ROSException), e: # rospy.logerr("Service call failed: %s" % (e,)) return
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)
class get_pos(object): def __init__(self, limb_viewer, limb_checker, parameter_reset): ''' Viewer - Limb that will end up reaching out to the user's hand Checker - Limb that checks whether the user's hand is still in position Parameter Reset - Determines whether the camera parameters should be reset. Reset is only needed once ''' #Initializing for the translation self.left_arm = Limb('left') self.right_arm = Limb('right') self.limb = limb_viewer self.img_viewer = None self.arm_viewer = baxter_interface.Limb( limb_viewer) # Assinging the viewer to the correct limb self.arm_checker = baxter_interface.Limb( limb_checker) # Assigning the checker to the correct limb self.hand_area = 0 # Hand area is used by Checker to determine whether the hand is still detected self.bridge = CvBridge() # ROS to Opencv # Coordinates and global variables setup self.torque = None # Torque will be used as a detection method self.frame = None # Will be used for image shape self.x = 0 # Will be assigned to x-pixel-coordinate of detected hand // Will later be converted to base frame self.y = 0 # Will be assigned to y-pixel-coordinate of detected hand // Will later be converted to base frame self.z_camera = 0 # Will be assigned to z spatial coordinate WRT to camera # Arm sensor setup self.distance = {} root_name = "/robot/range/" sensor_name = ["left_hand_range/state", "right_hand_range/state"] # Assigning the camera topics to viewer and checker depending on the user input if limb_viewer == 'left': self.camera_viewer = 'left_hand_camera' camera_checker = 'right_hand_camera' # Subscribing to the left sensor self.__sensor = rospy.Subscriber(root_name + sensor_name[0], Range, callback=self.sensorCallback, callback_args="left", queue_size=1) else: self.camera_viewer = 'right_hand_camera' camera_checker = 'left_hand_camera' # Subscribing to the right sensor self.__sensor = rospy.Subscriber(root_name + sensor_name[1], Range, callback=self.sensorCallback, callback_args="right", queue_size=1) # resetting the parameters of the viewer and checker if instructed if parameter_reset == True: settings = CameraController.createCameraSettings(width=640, height=400, exposure=-1) CameraController.openCameras(self.camera_viewer, camera_checker, settings=settings, settings2=settings) print "Viewer-camera, checker-camera parameter check" # self.left_camera = baxter_interface.CameraController(self.camera_viewer) # self.left_camera.resolution = (640,400) # self.left_camera.exposure = -1 # print "Viewer-camera parameter check" # self.right_camera = baxter_interface.CameraController(camera_checker) # self.right_camera.resolution = (640,400) # self.right_camera.exposure = -1 # print "Checker-camera parameter check" # Subscribing to the cameras self.viewer_image_sub = rospy.Subscriber( "/cameras/" + self.camera_viewer + "/image", Image, self.callback_viewer) # Subscribing to Camera self.checker_image_sub = rospy.Subscriber( "/cameras/" + camera_checker + "/image", Image, self.callback_checker) # Subscribing to Camera # Rotation matrix set up self.tf = TransformListener() # Orientation of the shaker will determine how the viewer's orientation will be once it reaches its final position self.left_orientation_shaker = [ -0.520, 0.506, -0.453, 0.518 ] # Defined orientation // Obtained through tf_echo // ****GRIPPER**** self.right_orientation_shaker = [0.510, 0.550, 0.457, 0.478] self.camera_gripper_offset = [ 0.038, 0.012, -0.142 ] # Offset of camera from the GRIPPER reference frame self._cur_joint = { 'left_w0': 0.196733035822, 'left_w1': 0.699878733673, 'left_w2': 0, 'left_e0': -0.303344700458, 'left_e1': 1.90098568922, 'left_s0': -0.263844695215, 'left_s1': -1.03467004025 } def follow_up(self, joint_solution=None): ''' Any follow up instructions, after the hand is reached, should be in here ''' if joint_solution == None: joint_solution = self._cur_joint self.__sensor.unregister() # self.viewer_image_sub.unregister() self.checker_image_sub.unregister() self.arm_viewer.move_to_joint_positions(joint_solution) def sensorCallback(self, msg, side): self.distance[side] = msg.range if msg.range < 0.05: # Only assigns a value to sensor if the hand is less than 10cm away self.z_camera = msg.range # Assign the z-coordinate else: self.z_camera = None def callback_viewer(self, data): ''' This is the callback function of the viewer, i.e, the thread that the viewer creates once it's initialized. The viewer callback first runs a skin color detection, creates a mask from the given color range, and then the hand detection is ran on the mask. The hand detection is done through a cascade classifier, and the coordinates of the hands are assigned to the global variables x and y. To be noted that the contour drawing is only to provide a good display; it doesn't affect the skin detection, though it uses the same mask ''' try: # Creates an OpenCV image from the ROS image cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8") except CvBridgeError as e: print(e) # The torque is used as a method of checking whether the arm has reached the user self.torque = self.arm_viewer.endpoint_effort() self.torque = self.torque_mag( self.torque ) # The torque assigned is the magnitude of the torque detected img = cv_image converted = cv2.cvtColor( img, cv2.COLOR_BGR2YCR_CB) # Convert image color scheme to YCrCb min_YCrCb = np.array( [0, 133, 77], np.uint8) # Create a lower bound for the skin color max_YCrCb = np.array([255, 173, 127], np.uint8) # Create an upper bound for skin color skinRegion = cv2.inRange(converted, min_YCrCb, max_YCrCb) # Create a mask with boundaries skinMask = cv2.inRange(converted, min_YCrCb, max_YCrCb) # Duplicate of the mask f kernel = cv2.getStructuringElement( cv2.MORPH_ELLIPSE, (11, 11)) # Apply a series of errosion and dilations to the mask #skinMask = cv2.erode(skinMask, kernel, iterations = 2) # Using an elliptical Kernel #skinMask = cv2.dilate(skinMask, kernel, iterations = 2) skinMask = cv2.GaussianBlur(skinMask, (3, 3), 0) # Blur the image to remove noise skin = cv2.bitwise_and(img, img, mask=skinMask) # Apply the mask to the frame height, width, depth = cv_image.shape # Obtain the dimensions of the image self.frame = cv_image.shape hands_cascade = cv2.CascadeClassifier( '/home/steven/ros_ws/src/test_cam/haarcascade_hand.xml') hands = hands_cascade.detectMultiScale( skinMask, 1.3, 5) # Detect the hands on the converted image contours, hierarchy = cv2.findContours( skinRegion, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) # Find the contour on the skin detection for i, c in enumerate( contours): # Draw the contour on the source frame area = cv2.contourArea(c) if area > 10000: # Noise isn't circled cv2.drawContours(img, contours, i, (255, 255, 0), 2) for (x, y, z, h) in hands: # Get the coordinates of the hands d = h / 2 self.x = x + d # Gets the center of the detected hand self.y = y + d # Gets the center of the detected hand cv2.circle(img, (self.x, self.y), 50, (0, 0, 255), 5) # Circle the detected hand self.img_viewer = img def callback_checker(self, data): ''' This is the callback function of the checker, i.e, the thread that the checker creates once it's initialized. The checker callback runs in the same way as the viewer callback, but its main use is to ensure that a hand is still detected. It does so by checking the area of the contour drawn on the image, and hence, unlike the viewer callback, the contour affects the hand detection. The contour area is however unstable, and might not produce the best results. The skin color detection has a high range of red color as wll, making the contour detection less stable when a red colopr is in range. Hence that is why the exposure of the checker is decreased so as to reduce the noise colors. ''' try: cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8") except CvBridgeError as e: print(e) img = cv_image converted = cv2.cvtColor( img, cv2.COLOR_BGR2YCR_CB) # Convert image color scheme to YCrCb min_YCrCb = np.array( [0, 133, 77], np.uint8) # Create a lower bound for the skin color max_YCrCb = np.array([255, 173, 127], np.uint8) # Create an upper bound for skin color skinRegion = cv2.inRange(converted, min_YCrCb, max_YCrCb) # Create a mask with boundaries skinMask = cv2.inRange( converted, min_YCrCb, max_YCrCb) # Duplicate of the mask for comparison kernel = cv2.getStructuringElement( cv2.MORPH_ELLIPSE, (11, 11)) # Apply a series of errosion and dilations to the mask #skinMask = cv2.erode(skinMask, kernezl, iterations = 2) # Using an elliptical Kernel #skinMask = cv2.dilate(skinMask, kernel, iterations = 2) skinMask = cv2.GaussianBlur(skinMask, (3, 3), 0) # Blur the image to remove noise skin = cv2.bitwise_and(img, img, mask=skinMask) # Apply the mask to the frame height, width, depth = cv_image.shape hands_cascade = cv2.CascadeClassifier( '/home/steven/ros_ws/src/test_cam/haarcascade_hand.xml') hands = hands_cascade.detectMultiScale( skinMask, 1.3, 5) # Detect the hands on the converted image for (x, y, z, h) in hands: # Get the coordinates of the hands d = h / 2 x = x + d y = y + d cv2.circle(img, (x, y), 50, (0, 0, 255), 5) contours, hierarchy = cv2.findContours( skinRegion, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) # Find the contour on the skin detection self.hand_area = 0 for i, c in enumerate( contours): # Draw the contour on the source frame area = cv2.contourArea(c) if area > 10000: cv2.drawContours(img, contours, i, (255, 255, 0), 2) self.hand_area = area # - area_1 # The following function will create a contour based on the red color scheme. This function should be enabled whenever # a red color is detected by the checker. The red color detected will alter the hand area detected, and hence will # detect an large area even if a hand is not in range, area corresponding to the red color. Hence the red area should be subtracted # from the entire detected area, to obtain the actual area of the hand hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV) lower_red = np.array([0, 10, 10]) upper_red = np.array([10, 240, 240]) red_mask = cv2.inRange(hsv, lower_red, upper_red) red = cv2.bitwise_and(img, img, mask=red_mask) cv2.imshow("Viewer Camera // Checker Camera", np.hstack([img, self.img_viewer])) cv2.waitKey(1) def torque_mag(self, a): # Gets the magnitude of the torque detected mag_a = math.sqrt((a['force'].z * a['force'].z) + (a['force'].y * a['force'].y) + (a['force'].x * a['force'].x)) return mag_a def get_starting_pos(self): # Sets the limbs to their correct starting position if self.limb == 'left': self.left_arm.move_to_joint_positions(dict({ 'left_e0': -1.1339952974442922, 'left_e1': 1.2954467753692318, 'left_s0': 0.8252816638823526, 'left_s1': 0.048703890015361885, 'left_w0': -0.14879613642488512, 'left_w1': 1.176179769111141, 'left_w2': 0.5867476513661708 }), timeout=15.0) self.right_arm.move_to_joint_positions(dict({ 'right_e0': 1.0438739261560241, 'right_e1': 1.2797234722934063, 'right_s0': -0.1257864246066039, 'right_s1': -0.3171505278953093, 'right_w0': 0.3186845086831947, 'right_w1': 1.1278593742927505, 'right_w2': -0.215907795894872 }), timeout=15.0) else: self.left_arm.move_to_joint_positions(dict({ 'left_e0': -1.2475098757478127, 'left_e1': 1.1830826826566254, 'left_s0': 0.292990330486114, 'left_s1': -0.12540292940963257, 'left_w0': -0.024927187803137973, 'left_w1': 1.301966193717745, 'left_w2': 0.15953400194008302 }), timeout=15.0) self.right_arm.move_to_joint_positions(dict({ 'right_e0': 1.0933448065653286, 'right_e1': 1.2609322076418101, 'right_s0': -0.6024709544419963, 'right_s1': -0.08053399136398422, 'right_w0': 0.2906893593042859, 'right_w1': 1.212995308020391, 'right_w2': -0.19251458887961942 }), timeout=15.0) def unit_vector_to_point(self, x, y): ''' Creates a unit vector from the camera's frame, given a pixel point from the image ''' height, width, depth = self.frame # Obtain the dimensions of the frame cam_info = rospy.wait_for_message("/cameras/" + self.camera_viewer + "/camera_info", CameraInfo, timeout=None) img_proc = PinholeCameraModel() img_proc.fromCameraInfo(cam_info) # The origin of the camera is initally set to the top left corner # However the projectPixelTo3dRay uses the origin at the center of the camera. Hence the coordinates have to be converted x = x - width / 2 y = height / 2 - y # Creates a unit vector to the given point. Unit vector passes through point from the center of the camera n = img_proc.projectPixelTo3dRay((x, y)) return n def unit_vector_gripper_frame(self, u_vector): ''' Converts the unit vector from the camera's frame to that of the gripper ''' pose = self.arm_viewer.endpoint_pose() pos_gripper = [ pose['position'].x, pose['position'].y, pose['position'].z ] u_vector_gripper = [ u_vector[0] + self.camera_gripper_offset[0], u_vector[1] + self.camera_gripper_offset[1], u_vector[2] + self.camera_gripper_offset[2] ] return u_vector_gripper def align(self, alignment_vector=None): ''' To ensure that a inverse kinematics will return a valid joint solution set, a first alignment is needed. Aligning the gripper with the unit vector to the hand's position will ensure that. ''' if alignment_vector != None: alignment_vector = alignment_vector[: 3] # Ensure that vector is a 3x1 height, width, depth = self.frame # Gets the dimension of the image pose = self.arm_viewer.endpoint_pose( ) # Gets the current pose of the end effector pos = [pose['position'].x, pose['position'].y, pose['position'].z] # Gets the position quat = [ pose['orientation'].x, pose['orientation'].y, pose['orientation'].z, pose['orientation'].w ] # Gets the orientation __matrix = self.tf.fromTranslationRotation( pos, quat) # Rotational matrix is obtained from pos and quat __matrix = __matrix[:3, : 3] # __matrix contain the rotational, and translational component, alongside with a last row of 0,0,0,1 i.e matrix is 4x4 # Only the rotational part is needed for alignement purposes, i.e a 3x3 def alignment_to_vec( b, a ): # b = z-axis vector // a = alignment vector (unit vector to point) ''' Returns the rotational matrix, that wil align the vector b to a ''' a_dot_b = sum(imap(mul, a, b)) #np.dot(a,b) n_a = np.array([float(a[0]), float(a[1]), float(a[2])]) n_b = np.array([float(b[0]), float(b[1]), float(b[2])]) a_x_b = np.cross(n_a, n_b) a_x_b = np.matrix([[float(a_x_b[0])], [float(a_x_b[1])], [float(a_x_b[2])]]) mod_a = math.sqrt((float(a[0]) * float(a[0])) + (float(a[1]) * float(a[1])) + (float(a[2]) * float(a[2]))) mod_b = math.sqrt((float(b[0]) * float(b[0])) + (float(b[1]) * float(b[1])) + (float(b[2]) * float(b[2]))) mod_a_x_b = math.sqrt((float(a_x_b[0]) * float(a_x_b[0])) + (float(a_x_b[1]) * float(a_x_b[1])) + (float(a_x_b[2]) * float(a_x_b[2]))) x = a_x_b / mod_a_x_b alpha = a_dot_b / (mod_a * mod_b) theta = math.acos(alpha) a_matrix = np.matrix([[0, float(-x[2]), float(x[1])], [float(x[2]), 0, float(-x[0])], [float(-x[1]), float(x[0]), 0]]) rotation_matrix = np.identity(3) + (math.sin(theta) * a_matrix) + ( (1 - (math.cos(theta))) * (np.dot(a_matrix, a_matrix))) return rotation_matrix def create_from_rot_matrix(rot): """ Rotation matrix created from quaternion Create from rotation matrix, modified from https://github.com/CMU-ARM/ARBT-Baxter-Nav-Task/blob/stable/scripts/Quaternion.py """ tr = np.trace(rot) if (tr > 0): s = np.sqrt(tr + 1) * 2 w = 0.25 * s x = (rot[2, 1] - rot[1, 2]) / s y = (rot[0, 2] - rot[2, 0]) / s z = (rot[1, 0] - rot[0, 1]) / s elif (rot[0, 0] > rot[1, 1] and rot[0, 0] > rot[2, 2]): s = np.sqrt(1 + rot[0, 0] - rot[1, 1] - rot[2, 2]) * 2 w = (rot[2, 1] - rot[1, 2]) / s x = 0.25 * s y = (rot[0, 1] + rot[1, 0]) / s z = (rot[0, 2] + rot[2, 0]) / s elif (rot[1, 1] > rot[2, 2]): s = np.sqrt(1 + rot[1, 1] - rot[0, 0] - rot[2, 2]) * 2 w = (rot[0, 2] - rot[2, 0]) / s x = (rot[0, 1] + rot[1, 0]) / s y = 0.25 * s z = (rot[1, 2] + rot[2, 1]) / s else: s = np.sqrt(1 + rot[2, 2] - rot[1, 1] - rot[0, 0]) * 2 w = (rot[1, 0] - rot[0, 1]) / s x = (rot[0, 2] + rot[2, 0]) / s y = (rot[1, 2] + rot[2, 1]) / s z = 0.25 * s return x, y, z, w def hamilton_product(b, a): q = [x, y, z, w] q[3] = a[3] * b[3] - a[0] * b[0] - a[1] * b[1] - a[2] * b[2] q[0] = a[3] * b[0] + a[0] * b[3] + a[1] * b[2] - a[2] * b[1] q[1] = a[3] * b[1] - a[0] * b[2] + a[1] * b[3] + a[2] * b[0] q[2] = a[3] * b[2] + a[0] * b[1] - a[1] * b[0] + a[2] * b[3] return q z_vector = np.dot(__matrix, np.matrix( [[0], [0], [1]])) # Converts the z-axis of the camera to the base frame print "Z-vector BF: ", z_vector rotation_matrix = alignment_to_vec( z_vector, alignment_vector ) # Rotation matrix that aligns the z-axis to the unit vector, pointing towards the hand x, y, z, w = create_from_rot_matrix( rotation_matrix) # Gets the orientation of alignement rot_quat = [x, y, z, w] final_quat = hamilton_product( quat, rot_quat) # Gets the final orientation of alignement self.ik(self.limb, pos, final_quat, True) # Aligns the viewer print "Aligned" def pixel_translation(self, uv): # Converts the pixel coordinates to spatial coordinates WRT the camera's frame pose = self.arm_viewer.endpoint_pose() xy = [pose['position'].x, pose['position'].y, pose['position'].z] height, width, depth = self.frame #camera frame dimensions print "\nx-pixel: ", uv[0], "\ty-pixel: ", uv[1] cx = uv[0] # x pixel of hand cy = uv[1] # y pixel of hand pix_size = 0.0025 # Camera calibration (meter/pixels); Pixel size at 1 meter height of camera h = self.z_camera #Height from hand to camera, when at vision place x0b = xy[ 0] # x position of camera in baxter's base frame, when at vision place y0b = xy[ 1] # y position of camera in baxter's base frame, when at vision place x_camera_offset = .045 #x camera offset from center of the gripper y_camera_offset = -.01 #y camera offset from center of the gripper #Position of the object in the baxter's stationary base frame x_camera = (cy - (height / 2) ) * pix_size * h - 0.045 # x-coordinate in camera's frame y_camera = (cx - (width / 2) ) * pix_size * h + 0.01 # y-coordiante in camera's frame return x_camera, y_camera, h def xy_translation(self): uv = (self.x, self.y) # pixel coordinates if self.x != 0 and self.y != 0 and self.z_camera != None: # If a hand has been detected and a position recorded x_camera, y_camera, h = self.pixel_translation( uv) # Obtains the spatial x,y coordinates self.depth_detection(float(x_camera), float(y_camera), h) # proceeds to coordinates translation else: self.depth_detection( 0, 0, 0 ) #If a depth hasn't been detected, proceed to the depth detection def depth_detection(self, x, y, z): pose = self.arm_viewer.endpoint_pose() pos = [pose['position'].x, pose['position'].y, pose['position'].z] quat = [ pose['orientation'].x, pose['orientation'].y, pose['orientation'].z, pose['orientation'].w ] height, width, depth = self.frame #camera frame dimensions __matrix = self.tf.fromTranslationRotation(pos, quat) #self.orientation_cam # If a depth has already been detected # Proceed to coordinates translation, and use inverse kinematics to move to position if self.z_camera != None: print "depth detected" z = self.z_camera xyz = np.dot(__matrix, np.matrix([[x], [y], [z], [1]])) print "\nx-coordinate obtained: ", xyz[ 0], "\ny-coordinate obtained: ", xyz[ 1], "\nz-coordinate obtained: ", xyz[2] pos = [xyz[0], xyz[1], xyz[2]] self.ik(self.limb, pos, self.orientation_shaker) # Else, move the arm towards unit vector until a depth has been detected else: print "...Moving arm..." __matrix = __matrix[:3, :3] # Aligns the end effector to the detected hand before moving it n = self.unit_vector_to_point(self.x, self.y) if self.x > width / 2: # If self.x > widht/2, hand is above the camera up = True # Will pass this as an argument to generate the unit vector, indicating that the hand is above else: up = False u_vector_gripper = self.unit_vector_gripper_frame(n) u_vector = np.dot( __matrix, np.matrix([[u_vector_gripper[0]], [u_vector_gripper[1]], [u_vector_gripper[2]]])) print "...Aligning arm..." self.align(u_vector) #self.with_check(__matrix,up) self.without_check(__matrix, up) def without_check(self, __matrix, up=False): height, width, depth = self.frame #camera frame dimensions while self.z_camera == None and self.torque < 20: # While no depth is detcted and no torque is detected n = self.unit_vector_to_point( width / 2, height / 2) # Unit vector to the center of the camera # the unit vector to the center of the camera is generated, since after alignement, the hand should be almost in line with the center u_vector_gripper = self.unit_vector_gripper_frame( n) # converts the unit vector to the gripper's frame u_vector = np.dot(__matrix, np.matrix([[u_vector_gripper[0]], [u_vector_gripper[1]], [u_vector_gripper[2]] ])) # converts to the base frame if self.limb == 'left': if up == True: # Accounts for the position of the hand, above or below the camera # Unit vector seems to always have a negative z-component, hence if the hand is detected above the camera, the z-component is negated u_vector[2] = -u_vector[2] # negating the z-component elif self.limb == 'right': if up != True: # The inverse is true for the right arm u_vector[2] = -u_vector[2] pose = self.arm_viewer.endpoint_pose() pos = [ pose['position'].x + (u_vector[0] / 20), pose['position'].y + (u_vector[1] / 20), pose['position'].z + (u_vector[2] / 20) ] # Move the arm by increments quat = [ pose['orientation'].x, pose['orientation'].y, pose['orientation'].z, pose['orientation'].w ] self.ik(self.limb, pos, quat) # Once depth has been detected, change orientation of gripper to that of the shaker, defined when initialized pose = self.arm_viewer.endpoint_pose() pos = [pose['position'].x, pose['position'].y, pose['position'].z] if self.limb == 'left': self.ik('left', pos, self.left_orientation_shaker, True) # Hand-shaking position for left arm else: self.ik('right', pos, self.right_orientation_shaker, True) # Hand-shaking position for right arm print "Position reached...Moving to Instructor position" self.follow_up() # Follow_up instructions def with_check(self, __matrix, up): height, width, depth = self.frame #camera frame dimensions print self.hand_area while self.z_camera == None and self.torque < 20 and self.hand_area > 5000: # As long as hand area is detected if self.hand_area > 5000: n = self.unit_vector_to_point(width / 2, height / 2) u_vector_gripper = self.unit_vector_gripper_frame(n) u_vector = np.dot( __matrix, np.matrix([[u_vector_gripper[0]], [u_vector_gripper[1]], [u_vector_gripper[2]]])) if self.limb == 'left': if up == True: u_vector[2] = -u_vector[2] elif self.limb == 'right': print self.limb if up != True: u_vector[2] = -u_vector[2] pose = self.arm_viewer.endpoint_pose() pos = [ pose['position'].x + (u_vector[0] / 20), pose['position'].y + (u_vector[1] / 20), pose['position'].z + (u_vector[2] / 20) ] quat = [ pose['orientation'].x, pose['orientation'].y, pose['orientation'].z, pose['orientation'].w ] self.ik(self.limb, pos, quat) else: print "No hands detected..." return 0 if self.hand_area > 5000: pose = self.arm_viewer.endpoint_pose() pos = [pose['position'].x, pose['position'].y, pose['position'].z] self.ik('left', pos, self.left_orientation_shaker, True) print "Position reached...Moving to Instructor position" #self.follow_up() else: print "No hands detected for final alignment" def ik(self, limb, pos, quat, block=None, arm=None): ''' This function uses inverse kinematics to calculate the joint states given a certain pose. It also applies the joint states to the specified arms, i.e moves the arm. Arguments: - limb : the limb which is to be moved. If not specified, limb is viewer limb - pos,quat : pose for which joint solutions are desired - block - if block is None, the motion of the joints will be done by set_joint_positions. Else, it will be done by move_to_joint_positions. set_joint_positions allows the operation to be interupted (Used when moving the arm towards the hand) move_to_joint_positions cannot be interupted, and is used when aligning the end effector ******* CORE OF FUNCTION IS FROM /BAXTER_EXAMPLES/IK_SERVICE_CLIENT.PY******** ''' if arm == None: arm = self.arm_viewer ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService" iksvc = rospy.ServiceProxy(ns, SolvePositionIK) ikreq = SolvePositionIKRequest() hdr = Header(stamp=rospy.Time.now(), frame_id='base') poses = { 'left': PoseStamped( header=hdr, pose=Pose( position=Point( x=pos[0], y=pos[1], z=pos[2], ), orientation=Quaternion( x=quat[0], y=quat[1], z=quat[2], w=quat[3], ), ), ), 'right': PoseStamped( header=hdr, pose=Pose( position=Point( x=pos[0], y=pos[1], z=pos[2], ), orientation=Quaternion( x=quat[0], y=quat[1], z=quat[2], w=quat[3], ), ), ), } ikreq.pose_stamp.append(poses[limb]) try: rospy.wait_for_service(ns, 5.0) resp = iksvc(ikreq) except (rospy.ServiceException, rospy.ROSException), e: rospy.logerr("Service call failed: %s" % (e, )) return 1 # Check if result valid, and type of seed ultimately used to get solution # convert rospy's string representation of uint8[]'s to int's resp_seeds = struct.unpack('<%dB' % len(resp.result_type), resp.result_type) if (resp_seeds[0] != resp.RESULT_INVALID): seed_str = { ikreq.SEED_USER: '******', ikreq.SEED_CURRENT: 'Current Joint Angles', ikreq.SEED_NS_MAP: 'Nullspace Setpoints', }.get(resp_seeds[0], 'None') #print("SUCCESS - Valid Joint Solution Found from Seed Type: %s" % # (seed_str,)) # Format solution into Limb API-compatible dictionary limb_joints = dict( zip(resp.joints[0].name, resp.joints[0].position)) # reformat the solution arrays into a dictionary joint_solution = dict( zip(resp.joints[0].name, resp.joints[0].position)) if block == None: arm.set_joint_positions(joint_solution) else: arm.move_to_joint_positions(joint_solution) else: print("INVALID POSE - No Valid Joint Solution Found.") return 0
'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 } right.move_to_joint_positions(neutral_right) left.move_to_joint_positions(neutral_left) ####PART 1 ("what would you like today") #can we track the person with Baxter's head? ###PART 2 (COOKIE) Head.set_pan(head, 0.45, speed=1.0, timeout=0.0) #looks at cookie section #lifts arm above cookies left.move_to_joint_positions({ 'left_w0': -0.177941771394708, 'left_w1': 1.131694326262464, 'left_w2': 3.0161897241796947, 'left_e0': 0.24965537322835107, 'left_e1': 0.8038059328519568,
def main(): """ Main Script """ while not rospy.is_shutdown(): planner = PathPlanner("right_arm") limb = Limb("right") joint_angles = limb.joint_angles() print(joint_angles) camera_angle = { 'right_j6': 1.72249609375, 'right_j5': 0.31765625, 'right_j4': -0.069416015625, 'right_j3': 1.1111962890625, 'right_j2': 0.0664150390625, 'right_j1': -1.357775390625, 'right_j0': -0.0880478515625 } limb.set_joint_positions(camera_angle) limb.move_to_joint_positions(camera_angle, timeout=15.0, threshold=0.008726646, test=None) #drawing_angles = {'right_j6': -2.00561328125, 'right_j5': -1.9730205078125, 'right_j4': 1.5130146484375, 'right_j3': -1.0272568359375, 'right_j2': 1.24968359375, 'right_j1': -0.239498046875, 'right_j0': 0.4667275390625} #print(joint_angles) #drawing_angles = {'right_j6': -1.0133310546875, 'right_j5': -1.5432158203125, 'right_j4': 1.4599892578125, 'right_j3': -0.04361328125, 'right_j2': 1.6773486328125, 'right_j1': 0.019876953125, 'right_j0': 0.4214736328125} drawing_angles = { 'right_j6': 1.9668515625, 'right_j5': 1.07505859375, 'right_j4': -0.2511611328125, 'right_j3': 0.782650390625, 'right_j2': 0.25496875, 'right_j1': -0.3268349609375, 'right_j0': 0.201146484375 } raw_input("Press <Enter> to take picture: ") waypoints_abstract = vision() print(waypoints_abstract) #ar coordinate : x = 0.461067 y = -0.235305 #first get the solution of the maze #solutionpoints = [(0,0),(-0.66,0.16), (-0.7, 0.4)] # Make sure that you've looked at and understand path_planner.py before starting tfBuffer = tf2_ros.Buffer() tfListener = tf2_ros.TransformListener(tfBuffer) r = rospy.Rate(10) #find trans from while not rospy.is_shutdown(): try: trans = tfBuffer.lookup_transform('base', 'ar_marker_0', rospy.Time()).transform break except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): if tf2_ros.LookupException: print("lookup") elif tf2_ros.ConnectivityException: print("connect") elif tf2_ros.ExtrapolationException: print("extra") # print("not found") pass r.sleep() mat = as_transformation_matrix(trans) point_spaces = [] for point in waypoints_abstract: # for point in solutionpoints: point = np.array([point[0], point[1], 0, 1]) point_space = np.dot(mat, point) point_spaces.append(point_space) print(point_spaces) length_of_points = len(point_spaces) raw_input("Press <Enter> to move the right arm to drawing position: ") limb.set_joint_positions(drawing_angles) limb.move_to_joint_positions(drawing_angles, timeout=15.0, threshold=0.008726646, test=None) ## ## Add the obstacle to the planning scene here ## #add obstacle size = [0.78, 1.0, 0.05] name = "box" pose = PoseStamped() pose.header.frame_id = "base" pose.pose.position.x = 0.77 pose.pose.position.y = 0.0 pose.pose.position.z = -0.18 #Orientation as a quaternion pose.pose.orientation.x = 0.0 pose.pose.orientation.y = 0.0 pose.pose.orientation.z = 0.0 pose.pose.orientation.w = 1.0 planner.add_box_obstacle(size, name, pose) #limb.set_joint_positions( drawing_angles) #limb.move_to_joint_positions( drawing_angles, timeout=15.0, threshold=0.008726646, test=None) #starting position x, y, z = 0.67, 0.31, -0.107343 goal_1 = PoseStamped() goal_1.header.frame_id = "base" #x, y, and z position goal_1.pose.position.x = x goal_1.pose.position.y = y goal_1.pose.position.z = z #Orientation as a quaternion goal_1.pose.orientation.x = 0.0 goal_1.pose.orientation.y = -1.0 goal_1.pose.orientation.z = 0.0 goal_1.pose.orientation.w = 0.0 while not rospy.is_shutdown(): try: waypoint = [] for point in point_spaces: goal_1.pose.position.x = point[0] goal_1.pose.position.y = point[1] goal_1.pose.position.z = -0.113343 #set this value when put different marker waypoint.append(copy.deepcopy(goal_1.pose)) plan, fraction = planner.plan_straight(waypoint, []) print(fraction) raw_input("Press <Enter> to move the right arm to draw: ") if not planner.execute_plan(plan): raise Exception("Execution failed") except Exception as e: print e traceback.print_exc() else: break raw_input("Press <Enter> to start again: ")
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) # 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) def wait(): raw_input("Press Enter to continue...")
class BaxterController(): def __init__(self, limb_name, trajectory, offset, radius): # crate a limb instance control the baxter arm self.limb = Limb(limb_name) # numpy joint angle trajectory of nx7 shape self.trajectory = trajectory # index variable to keep track of current row in numpy array self.trajectory_index = 0 # store the joint names since # it will be used later while commanding the limb self.joint_names = self.limb.joint_names() # define a service called 'move_arm_to_waypoint' self.move_arm_to_waypoint_service = rospy.Service( 'move_arm_to_waypoint', Trigger, self.handle_move_arm_to_waypoint) # define another service called 'get_ee_pose' self.get_ee_position_service = rospy.Service( 'get_ee_position', GetEEPosition, self.handle_get_ee_position_service) # flag to set when trajectory is finished self.trajectory_finished = False # define 4x4 transformation for marker wrt end-effector # considering no rotation in 'marker_wrt_ee' transformation matrix self.marker_wrt_ee = np.eye(4) self.marker_wrt_ee[2, -1] = offset + radius # in z direction only def spin(self): # let the ros stay awake and serve the request rate = rospy.Rate(10) while not rospy.is_shutdown() and not self.trajectory_finished: rate.sleep() # wait some time before stopping the node so that the service request returns if any rospy.sleep(1) self.get_ee_position_service.shutdown() self.move_arm_to_waypoint_service.shutdown() rospy.logdebug('Shutting down the baxter controller node') rospy.signal_shutdown('User requested') def handle_get_ee_position_service(self, request): # create a response object for the 'GetEEPose' service ee_pose = self.limb.endpoint_pose() # get rotation matrix from quaternion. 'quaternion_matrix' # returns 4x4 HTM with translation set to sero ee_wrt_robot = quaternion_matrix(ee_pose['orientation']) ee_wrt_robot[:-1, -1] = ee_pose['position'] # update the translation # marker wrt robot = marker_wrt_ee * ee_wrt_robot marker_wrt_robot = ee_wrt_robot.dot(self.marker_wrt_ee) response = GetEEPositionResponse() response.position.x = marker_wrt_robot[0, -1] response.position.y = marker_wrt_robot[1, -1] response.position.z = marker_wrt_robot[2, -1] return response def handle_move_arm_to_waypoint(self, request): # create a response object for the trigger service response = TriggerResponse() # check if the trajectory finished trajectory_finished = self.trajectory_index >= self.trajectory.shape[0] # if trajectory isn't finished if not trajectory_finished: # get the latest joint values from given trajectory joint_values = trajectory[self.trajectory_index, :] # create command, i.e., a dictionary of joint names and values command = dict(zip(self.joint_names, joint_values)) # move the limb to given joint angle try: self.limb.move_to_joint_positions(command) response.message = 'Successfully moved arm to the following waypoint %s' % command except rospy.exceptions.ROSException: response.message = 'Error while moving arm to the following waypoint %s' % command finally: # increment the counter self.trajectory_index += 1 else: response.message = 'Arm trajectory is finished already' # set the success parameter of the response object response.success = trajectory_finished # set the flag just before returning from the function so that it is # almost certain that the service request is returned successfully self.trajectory_finished = trajectory_finished return response
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} right.move_to_joint_positions(neutral_right) left.move_to_joint_positions(neutral_left) ####PART 1 ("what would you like today") #can we track the person with Baxter's head? ###PART 2 (COOKIE) Head.set_pan(head, 0.45, speed = 1.0, timeout = 0.0) #looks at cookie section #lifts arm above cookies left.move_to_joint_positions({'left_w0': -0.177941771394708, 'left_w1': 1.131694326262464, 'left_w2': 3.0161897241796947, 'left_e0': 0.24965537322835107, 'left_e1': 0.8038059328519568, 'left_s0': -0.7320923310183137, 'left_s1': -0.4571262747898533} )
'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) def wait(): raw_input("Press Enter to continue...") def send_voice(msg): print "Baxter: %s" % index_to_string[msg] voice_pub.publish(data=msg)
try: tucker.supervised_tuck() except Exception, e: rospy.logerr(e.strerror) rospy.logerr("Failed to return arms to untucked position.") return False rospy.loginfo("Arms in untucked position.") rospy.loginfo("Moving arms to neutral position.") left_arm_neutral = rospy.get_param(LEFT_ARM_NEUTRAL) right_arm_neutral = rospy.get_param(RIGHT_ARM_NEUTRAL) arm_l = Limb("left") arm_r = Limb("right") arm_l.move_to_joint_positions(left_arm_neutral) arm_r.move_to_joint_positions(right_arm_neutral) return 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