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)
def listener(): global watch_timesteps global firt_pack_sync seq_len = 25 rospy.init_node('auto_grasping', anonymous=True, disable_signals=True) model = load_model(pkg_dir + '/model/my_model25-94.h5') zscore_data = np.loadtxt(pkg_dir + '/model/mean_std_zscore', delimiter=',', ndmin=2) left_arm = Limb('left') left_gripper = Gripper('left') left_gripper.calibrate() rate = rospy.Rate(50) # rate rospy.Subscriber('/imu_giver', ImuPackage, watchCallback) with firt_pack_sync: firt_pack_sync.wait() opened_timeout = 0 pre_res = 0 watch_buffer = [] bax_timesteps = [] # bax_timesteps and watch_buffer are two buffers to manage sequences while not rospy.is_shutdown(): rate.sleep() l_ang = list(left_arm.joint_angles().values()) l_vel = list(left_arm.joint_velocities().values()) l_eff = list(left_arm.joint_efforts().values()) bax_step = l_ang + l_vel + l_eff bax_timesteps.append(bax_step) if (watch_timesteps[1]): watch_buffer.extend(watch_timesteps[0]) watch_timesteps[1] = 0 if (len(bax_timesteps) >= seq_len and len(watch_buffer) >= seq_len): watch_buffer = watch_buffer[len(watch_buffer) - (seq_len):] bax_timesteps = bax_timesteps[len(bax_timesteps) - (seq_len):] sequence = [] for i in range(0, math.floor(seq_len * 0.3)): step = watch_buffer.pop(0) + bax_timesteps.pop(0) sequence.append(step) for i in range(0, math.ceil(seq_len * 0.7)): step = watch_buffer[i] + bax_timesteps[i] sequence.append(step) sequence = np.array(sequence) sequence = sequence - zscore_data[0, :] sequence = sequence / zscore_data[1, :] seq = np.ndarray((1, seq_len, sequence.shape[1])) seq[0] = sequence res = model.predict(seq) res = res[0][0] rospy.loginfo(left_gripper.position()) if (left_gripper.position() > 94.0): opened_timeout = opened_timeout + 1 if (res > 0.7 and pre_res > 0.7 and opened_timeout > 25): left_gripper.command_position(0.0) opened_timeout = 0 pre_res = res
def listener(): rospy.init_node('record_data', anonymous=True, disable_signals=True) global BAX_COLUMNS global WATCH_COLUMNS global NANOS_TO_MILLIS global bax_file_name global bax_row global watch_rows global command global sequence_counter resetNode() rospy.loginfo("Commands :\ns to stop\nr to remove the last file\nn to start new sequence\nc TWICE to shutdown the node\n") rate = rospy.Rate(120) # rate watch_sub = rospy.Subscriber('/imu_giver', ImuPackage, watchCallback) rospy.loginfo("START RECORDING SEQUENCE " + str(sequence_counter)) getkey_thread = Thread(target = getKey) getkey_thread.start() left_arm = Limb('left') left_gripper = Gripper('left') while not rospy.is_shutdown(): rate.sleep() t = round(rospy.get_rostime().to_nsec()*NANOS_TO_MILLIS) l_ang = list(left_arm.joint_angles().values()) l_vel = list(left_arm.joint_velocities().values()) l_eff = list(left_arm.joint_efforts().values()) l_grip_pos = str(left_gripper.position()) bax_row = l_ang + l_vel + l_eff bax_row.append(l_grip_pos) bax_row.append(str(t)) with open(bax_file_name + str(sequence_counter), 'a') as writeFile: writer = csv.writer(writeFile) writer.writerow(bax_row) writeFile.close() if (watch_rows[1]==1): with open(watch_file_name + str(sequence_counter), 'a') as writeFile: writer = csv.writer(writeFile) writer.writerows(watch_rows[0]) writeFile.close() watch_rows[1]=0 # s to stop # r to remove the last file # n to start new sequence # c TWICE to shutdown the node shutdown = False if (command == 's') : watch_sub.unregister() rospy.loginfo("FINISH RECORDING SEQUENCE " + str(sequence_counter)) rospy.loginfo("NODE STOPPED!") while True : rospy.Rate(2).sleep() if (command == 'r') : os.remove(bax_file_name + str(sequence_counter)) os.remove(watch_file_name + str(sequence_counter)) sequence_counter = sequence_counter - 1 rospy.loginfo("FILE REMOVED!") command = '' if (command == 'n') : rospy.loginfo("RESET NODE!") sequence_counter = sequence_counter + 1 resetNode() watch_sub = rospy.Subscriber('/imu_giver', ImuPackage, watchCallback) rospy.loginfo("START RECORDING SEQUENCE " + str(sequence_counter)) break if (command == 'c') : rospy.loginfo("Enter 'c' to shutdown... ") shutdown = True break if (shutdown) : rospy.signal_shutdown("reason...") getkey_thread.join()
def main(): rospy.init_node('baxter_kinematics') kin = baxter_kinematics('left') pos = [0.582583, -0.180819, 0.216003] rot = [0.03085, 0.9945, 0.0561, 0.0829] fl = fcntl.fcntl(sys.stdin.fileno(), fcntl.F_GETFL) fcntl.fcntl(sys.stdin.fileno(), fcntl.F_SETFL, fl | os.O_NONBLOCK) # Read initial positions: from sensor_msgs.msg import JointState from baxter_interface import Limb right_arm = Limb('right') left_arm = Limb('left') joints = left_arm.joint_names() positions = left_arm.joint_angles() velocities = left_arm.joint_velocities() force = convert_dict_to_list('left', left_arm.joint_efforts()) # Initial states q_previous = positions # Starting joint angles q_dot_previous = velocities # Starting joint velocities x_previous = kin.forward_position_kinematics( ) # Starting end effector position x_dot_previous = np.zeros((6, 1)) # Set model parameters: m = 1 K = 0.2 C = 15 B = 12 while True: ''' Following code breaks loop upon user input (enter key) ''' try: stdin = sys.stdin.read() if "\n" in stdin or "\r" in stdin: return_to_init(joints, 'left') break except IOError: pass # Gather Jacobian information: J = kin.jacobian() J_T = kin.jacobian_transpose() J_PI = kin.jacobian_pseudo_inverse() J_T_PI = np.linalg.pinv(J_T) # Extract sensor data: from sensor_msgs.msg import JointState from baxter_interface import Limb right_arm = Limb('right') left_arm = Limb('left') # Information is published at 100Hz by default (every 10ms=.01sec) time_step = 0.01 joints = left_arm.joint_names() positions = convert_dict_to_list('left', left_arm.joint_angles()) velocities = convert_dict_to_list('left', left_arm.joint_velocities()) force = convert_dict_to_list('left', left_arm.joint_efforts()) force_mag = np.linalg.norm(force) print(force_mag) if (force_mag < 28): # Add deadzone continue else: from sensor_msgs.msg import JointState from baxter_interface import Limb right_arm = Limb('right') left_arm = Limb('left') joints = left_arm.joint_names() positions = convert_dict_to_list('left', left_arm.joint_angles()) velocities = convert_dict_to_list('left', left_arm.joint_velocities()) force = convert_dict_to_list('left', left_arm.joint_efforts()) positions = np.reshape(positions, [7, 1]) # Converts to matrix velocities = np.reshape(velocities, [7, 1]) # Converts to matrix force = np.reshape(force, [7, 1]) # Converts to matrix x = kin.forward_position_kinematics() x = x[:-1] x_ref = np.reshape(x, [6, 1]) # Reference position x_ref_dot = J * velocities # Reference velocities t_stop = 10 t_end = time.time() + t_stop # Loop timer (in seconds) # Initial plot parameters time_plot_cum = 0 time_vec = [time_plot_cum] pos_vec = [x_ref[1][0]] # For integral control x_ctrl_cum = 0 time_cum = 0.00001 # Prevent divide by zero x_ctrl_prev = 0 # Initial for derivative ctrl while time.time() < t_end: from sensor_msgs.msg import JointState from baxter_interface import Limb right_arm = Limb('right') left_arm = Limb('left') J = kin.jacobian() J_T = kin.jacobian_transpose() J_PI = kin.jacobian_pseudo_inverse() J_T_PI = np.linalg.pinv(J_T) joints = left_arm.joint_names() positions = convert_dict_to_list('left', left_arm.joint_angles()) velocities = convert_dict_to_list('left', left_arm.joint_velocities()) force = convert_dict_to_list('left', left_arm.joint_efforts()) positions = np.reshape(positions, [7, 1]) # Converts to matrix velocities = np.reshape(velocities, [7, 1]) # Converts to matrix force = np.reshape(force, [7, 1]) # Converts to matrix x = kin.forward_position_kinematics() x = x[:-1] x_current = np.reshape(x, [6, 1]) x_dot_current = J * velocities # Only interested in y-axis: x_dot_current[0] = 0 #x_dot_current[1]=0 x_dot_current[2] = 0 x_dot_current[3] = 0 x_dot_current[4] = 0 x_dot_current[5] = 0 x_ddot = derivative(x_dot_previous, x_dot_current, time_step) # Model goes here f = J_T_PI * force # spacial force # Only interested in y-axis: f[0] = [0] #f[1]=[0] f[2] = [0] f[3] = [0] f[4] = [0] f[5] = [0] x_des = ((B * f - m * x_ddot - C * (x_dot_current - x_ref_dot)) / K) + x_ref # Spring with damper # Control robot Kp = 0.0004 Ki = 0.00000022 Kd = 0.0000005 x_ctrl = x_current - x_des # Only interested in y-axis: x_ctrl[0] = 0 #x_ctrl[1]=0 x_ctrl[2] = 0 x_ctrl[3] = 0 x_ctrl[4] = 0 x_ctrl[5] = 0 q_dot_ctrl = J_T * np.linalg.inv(J * J_T + np.identity(6)) * ( -Kp * x_ctrl - Ki * (x_ctrl_cum) - Kd * (x_ctrl_prev - x_ctrl) / time_step) q_dot_ctrl_list = convert_mat_to_list(q_dot_ctrl) q_dot_ctrl_dict = convert_list_to_dict(joints, q_dot_ctrl_list) left_arm.set_joint_velocities( q_dot_ctrl_dict) # Velocity control function # Update plot info time_cum += .05 time_vec.append(time_cum) pos_vec.append(x_current[1][0]) # Update integral control parameters x_ctrl_cum += x_ctrl * time_step # Update derivative control parameters x_ctrl_prev = x_ctrl # Update values before next loop x_previous = x_current # Updates previous position for next loop iteration x_dot_previous = x_dot_current # Updates previous velocity for next loop iteration print(time_vec) print(pos_vec) plt.plot(time_vec, pos_vec) plt.title('Position over time') plt.xlabel('Time (sec)') plt.ylabel('Position') plt.show() stop_joints(q_dot_ctrl_dict) left_arm.set_joint_velocities(q_dot_ctrl_dict) time.sleep(1) break
def main(): rospy.init_node('baxter_kinematics') kin = baxter_kinematics('left') pos = [0.582583, -0.180819, 0.216003] rot = [0.03085, 0.9945, 0.0561, 0.0829] fl = fcntl.fcntl(sys.stdin.fileno(), fcntl.F_GETFL) fcntl.fcntl(sys.stdin.fileno(), fcntl.F_SETFL, fl | os.O_NONBLOCK) # Read initial positions: from sensor_msgs.msg import JointState from baxter_interface import Limb right_arm=Limb('right') left_arm=Limb('left') joints=left_arm.joint_names() positions=convert_dict_to_list('left',left_arm.joint_angles()) velocities=convert_dict_to_list('left',left_arm.joint_velocities()) force=convert_dict_to_list('left',left_arm.joint_efforts()) positions=np.reshape(positions,[7,1]) # Converts to matrix velocities=np.reshape(velocities,[7,1]) # Converts to matrix force=np.reshape(force,[7,1]) # Converts to matrix # Initial states q_previous=positions # Starting joint angles q_dot_previous=velocities # Starting joint velocities x_previous=kin.forward_position_kinematics() # Starting end effector position x_dot_previous=np.zeros((6,1)) # Set model parameters: C=50 B=1 f_des=np.reshape(np.array([0,15,0,0,0,0]),[6,1]) J=kin.jacobian() J_T=kin.jacobian_transpose() J_PI=kin.jacobian_pseudo_inverse() J_T_PI=np.linalg.pinv(J_T) x=kin.forward_position_kinematics() x=x[:-1] x_ref=np.reshape(x,[6,1]) # Reference position x_ref_dot=J*velocities # Reference velocities t_stop=15 t_end=time.time()+t_stop # Loop timer (in seconds) # Initial plot parameters time_cum=0 time_vec=[time_cum] f=J_T_PI*force force_vec=[convert_mat_to_list(f[1])[0]] while time.time()<t_end: from sensor_msgs.msg import JointState from baxter_interface import Limb right_arm=Limb('right') left_arm=Limb('left') J=kin.jacobian() J_T=kin.jacobian_transpose() J_PI=kin.jacobian_pseudo_inverse() J_T_PI=np.linalg.pinv(J_T) joints=left_arm.joint_names() positions=convert_dict_to_list('left',left_arm.joint_angles()) velocities=convert_dict_to_list('left',left_arm.joint_velocities()) force=convert_dict_to_list('left',left_arm.joint_efforts()) positions=np.reshape(positions,[7,1]) # Converts to matrix velocities=np.reshape(velocities,[7,1]) # Converts to matrix force=np.reshape(force,[7,1]) # Converts to matrix x=kin.forward_position_kinematics() x=x[:-1] x_current=np.reshape(x,[6,1]) x_dot_current=J*velocities # Only interested in y-axis: x_dot_current[0]=0 #x_dot_current[1]=0 x_dot_current[2]=0 x_dot_current[3]=0 x_dot_current[4]=0 x_dot_current[5]=0 # Model goes here f=J_T_PI*force # spacial force # Only interested in y-axis: f[0]=[0] #f[1]=[0] f[2]=[0] f[3]=[0] f[4]=[0] f[5]=[0] x_dot_des=-B*(f_des+f)/C # Impedance control # Control robot q_dot_ctrl=J_PI*x_dot_des # Use this for damper system q_dot_ctrl=np.multiply(q_dot_ctrl,np.reshape(np.array([1,0,0,0,0,0,0]),[7,1])) # y-axis translation corresponds to first shoulder joint rotation q_dot_ctrl_list=convert_mat_to_list(q_dot_ctrl) q_dot_ctrl_dict=convert_list_to_dict(joints,q_dot_ctrl_list) left_arm.set_joint_velocities(q_dot_ctrl_dict) # Velocity control function # Update values before next loop x_previous=x_current # Updates previous position for next loop iteration x_dot_previous=x_dot_current # Updates previous velocity for next loop iteration # Update plot info time_cum+=.05 time_vec.append(time_cum) force_vec.append(convert_mat_to_list(f[1])[0]) print(time_vec) print(force_vec) plt.plot(time_vec,force_vec) plt.title('Force applied over time') plt.xlabel('Time (sec)') plt.ylabel('Force (N)') plt.show() time.sleep(1)