def main(): print('sup') rospy.init_node('sawyer_kinematics') print('init') # rospy.sleep(10) # init node???? # step 1 determine initial point # step 2 determine desired endpoint # in loop now # step 3 measure force and position # step 4 determine force we need to apply to push towards desired endpoint # step 5 convert force to joint torques with Jacobian # step 6 command joint torques # end of loop r = rospy.Rate(200) limb = Limb() kin = sawyer_kinematics('right') #step 1 pose = limb.endpoint_pose() point = pose.get('position') quaternion = pose.get('orientation') # both are 3x #step 2 path = np.array([.1, 0, 0]) des_point = point + path des_quaternion = quaternion path_dir = normalize(path) #step 3 #stiffness alpha = 1 #multiplier while not rospy.is_shutdown() and error > tol: #step 3 force = get_end_force(limb) # print("force is:", force) f_mag = force_mag(force) f_dir = force_dir(force) #step 4 f_right = proj(path, force) * path_dir f_wrong = force - f_right force_apply = f_right * alpha - f_wrong * K force_apply = np.hstack((force_apply, np.array([0, 0, 0]))) # print('force_apply is:', force_apply) #step 5 Jt = kin.jacobian_transpose() joint_torques = np.dot(Jt, force_apply) #make sure right dims joint_torques = np.asarray(joint_torques) joint_torques = np.array([[0, 0, 0, 0, 0, 0, 0]]) # print('set joints as:', joint_torques[0]) set_torques = joint_array_to_dict(joint_torques[0], limb) print(set_torques) print() #step 6 # limb.set_joint_torques(set_torques) cur_point = limb.endpoint_pose().get('position') error = np.linalg.norm(des_point - cur_point) r.sleep()
def move_position(xs, ys, zs): limb = intera_interface.Limb('right') kinematics = sawyer_kinematics('right') xyz_pos = [xs, ys, zs] x = dict(zip(limb._joint_names, kinematics.inverse_kinematics(xyz_pos))) limb.move_to_joint_positions(x) rospy.sleep(1.0)
def attach_springs(self): """ Switches to joint torque mode and attached joint springs to current joint positions. """ # record initial joint angles self._start_angles = self._limb.joint_angles() # set control rate control_rate = rospy.Rate(self._rate) # for safety purposes, set the control rate command timeout. # if the specified number of command cycles are missed, the robot # will timeout and return to Position Control Mode self._limb.set_command_timeout((1.0 / self._rate) * self._missed_cmds) limb = Limb() kin = sawyer_kinematics('right') pose = limb.endpoint_pose() point = pose.get('position') quaternion = pose.get('orientation') # loop at specified rate commanding new joint torques while not rospy.is_shutdown(): if not self._rs.state().enabled: rospy.logerr("Joint torque example failed to meet " "specified control rate timeout.") break self._update_forces(limb, kin, point, quaternion) control_rate.sleep()
def __init__(self, reconfig_server, limb = "right"): self._dyn = reconfig_server # control parameters self._rate = 1000.0 # Hz self._missed_cmds = 20.0 # Missed cycles before triggering timeout # create our limb instance self._limb = intera_interface.Limb(limb) # initialize parameters self._springs = dict() self._damping = dict() self._start_angles = dict() kinematics = sawyer_kinematics("right") xyz_pos = [4, 0, 0] x = dict(zip(self._limb._joint_names, kinematics.inverse_kinematics(xyz_pos))) self._limb.set_joint_positions(x) # create cuff disable publisher cuff_ns = 'robot/limb/' + limb + '/suppress_cuff_interaction' self._pub_cuff_disable = rospy.Publisher(cuff_ns, Empty, queue_size=1) # verify robot is enabled print("Getting robot state... ") self._rs = intera_interface.RobotEnable(CHECK_VERSION) self._init_state = self._rs.state().enabled print("Enabling robot... ") self._rs.enable() print("Running. Ctrl-c to quit")
def __init__(self): global LEGO_HEIGHT self.lego_height = LEGO_HEIGHT self.currentPos = [None, None, None] #length must be 3, [x,y,z] self.gripper = intera_interface.Gripper() self.limb = intera_interface.Limb('right') self.kinematics = sawyer_kinematics('right') self.currentLegoPos = [None, None, None]
def __init__(self): self.arm = "right" self.limb = intera_interface.Limb( self.arm) # Create arm & limb objects self.kin = sawyer_kinematics(self.arm) # Create kinematics construct self.rs = intera_interface.RobotEnable() # Enable Saywer self.rs.enable()
def __init__(self): # Instance Variables self.P_gain = 2e-3 self.I_gain = 0 self.D_gain = 0 self.int_err_x = 0 self.int_err_y = 0 self.x_bnd = [-0.27, 0.38] self.y_bnd = [0.35, 0.85] self.last_err_x = 0 self.last_err_y = 0 # Initialize Sawyer rospy.init_node('sawyer_comm', anonymous=True) self.limb = intera_interface.Limb('right') self.kin = sawyer_kinematics('right') joint_positions = go_to.joint_angle_arg(joint_angles=[ math.pi / 2, -math.pi / 3, 0, 2 * math.pi / 3, 0, -math.pi / 3, 0.57 ], speed_ratio=.225, accel_ratio=0.1) go_to.joint_angles(joint_positions) self.start_pose = self.limb.endpoint_pose() if VERBOSE: self.lastTime = rospy.get_time() ''' # Debug: move in a square s = 0.1 self.limb.set_command_timeout(2) rate = rospy.Rate(100) x = np.array([s,0,0,0,0,0]); while self.limb.endpoint_pose()['position'][0]<0.25: self.set_endpoint_velocity(x) rate.sleep() x = np.array([0,-s,0,0,0,0]); while self.limb.endpoint_pose()['position'][1]>0.46: self.set_endpoint_velocity(x) rate.sleep() x = np.array([-s,0,0,0,0,0]); while self.limb.endpoint_pose()['position'][0]>-0.1658: self.set_endpoint_velocity(x) rate.sleep() x = np.array([0,s,0,0,0,0]); while self.limb.endpoint_pose()['position'][1]<0.7206: self.set_endpoint_velocity(x) rate.sleep() x = np.array([0,0,0,0,0,0]); self.set_endpoint_velocity(x) ''' # Subscribing topics rospy.Subscriber('/ball_error', Int32MultiArray, callback=self.rx_pos_error, queue_size=1)
def main(): rospy.init_node('sawyer_kinematics') print '*** Sawyer PyKDL Kinematics ***\n' kin = sawyer_kinematics('right') print '\n*** Sawyer Description ***\n' kin.print_robot_description() print '\n*** Sawyer KDL Chain ***\n' kin.print_kdl_chain() # FK Position print '\n*** Saywer Position FK ***\n' print kin.forward_position_kinematics() # FK Velocity # print '\n*** Sawyer Velocity FK ***\n' # kin.forward_velocity_kinematics() # IK print '\n*** Sawyer Position IK ***\n' pos = [0.582583, -0.180819, 0.216003] rot = [0.03085, 0.9945, 0.0561, 0.0829] print kin.inverse_kinematics(pos) # position, don't care orientation print '\n*** Sawyer Pose IK ***\n' print kin.inverse_kinematics(pos, rot) # position & orientation # Jacobian print '\n*** Sawyer Jacobian ***\n' print kin.jacobian() # Jacobian Transpose print '\n*** Sawyer Jacobian Tranpose***\n' print kin.jacobian_transpose() # Jacobian Pseudo-Inverse (Moore-Penrose) print '\n*** Sawyer Jacobian Pseudo-Inverse (Moore-Penrose)***\n' print kin.jacobian_pseudo_inverse() # Joint space mass matrix print '\n*** Sawyer Joint Inertia ***\n' print kin.inertia() # Cartesian space mass matrix print '\n*** Sawyer Cartesian Inertia ***\n' print kin.cart_inertia() # Joint space coriolis matrix print '\n*** Sawyer Joint Coriolis ***\n' print kin.coriolis() # Cartesian space coriolis matrix # print '\n*** Sawyer Cartesian Coriolis ***\n' # print kin.cart_coriolis() # Joint space gravity matrix print '\n*** Sawyer Joint Gravity ***\n' print kin.gravity()
def __init__(self): self.arm = "right" self.limb = intera_interface.Limb(self.arm) self.joint_names = self.limb.joint_names() self.kin = sawyer_kinematics(self.arm) self.limb.set_joint_position_speed(0.25) self.rs = intera_interface.RobotEnable() self.rs.enable() self._state = "LEFT2RIGHT" neutral = [ -1.186115234375, 0.805931640625, 2.448353515625, 1.99850390625, -2.3551396484375, -0.67713671875, 3.3021533203125, 0.0 ] self.neutral = dict(zip(self.joint_names, neutral))
def main(): rospy.init_node('testing_node') limb = Limb('right') print('yay') r = rospy.Rate(200) kin = sawyer_kinematics('right') Jt = kin.jacobian_transpose() F = np.array([0, 0, 0, 0, 0, 10]) tau = np.dot(Jt, F) # print(tau) # rospy.sleep(20) # limb.set_command_timeout(.1) # wrench = limb.endpoint_effort() # force = wrench['force'] # print("force type: ", force) # print(wrench) while not rospy.is_shutdown(): # joint_torques = np.array([[0,0,0,0,0,0,0]]) Jt = kin.jacobian_transpose() mcart = kin.cart_inertia() f = np.array([0, 0, -9.8, 0, 0, 0]) gravity = np.dot(np.dot(Jt, mcart), f) joint_torques = np.array([ 1.5474950095623006, -16.78679653980678, -2.8277487768926406, -0.1771867794616826, 0.1073210015442511, -0.5216893350253217, -0.00607477942479895 ]) # cur = limb.joint_efforts() # print('set joints as:', joint_torques[0]) # set_torques = joint_array_to_dict(joint_torques[0], limb) # print(set_torques) # print(set_torques.get('right_j1')) # torque = np.array([gravity.item(i) for i in range(7)]) torque = joint_array_to_dict(joint_torques, limb) # cur['right_j1'] = 0 # cur['right_j2'] = 0 print('joint torques', torque) limb.set_joint_torques(torque) r.sleep()
def __init__(self, limb="right", tip_name="right_gripper_tip", hover_distance=0.1): self._limb_name = limb self._tip_name = tip_name self._limb = intera_interface.Limb(limb) # verify robot is enabled print("Getting robot state... ") self._rs = intera_interface.RobotEnable(intera_interface.CHECK_VERSION) self._init_state = self._rs.state().enabled print("Enabling robot... ") self._rs.enable() self._joint_names = [ 'right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4', 'right_j5', 'right_j6' ] self._kin = sawyer_kinematics(limb) self.prev_pos = np.zeros(3) self.prev_time = None
# Sample program to solve for the Inverse Kinematics of a particular hand position and send the robot arm there import rospy import intera_interface from sawyer_pykdl import sawyer_kinematics rospy.init_node("test_ik") limb = intera_interface.Limb('right') kinematics = sawyer_kinematics('right') xyz_pos = [0.58, -0.48, 0.216] x = dict(zip(limb._joint_names, kinematics.inverse_kinematics(xyz_pos))) limb.move_to_joint_positions(x)
def __init__(self): print "Initializing Node" rospy.init_node('poirot_rrhost') print "Enabling Robot" rs = intera_interface.RobotEnable() rs.enable() self.rs1 = intera_interface.RobotEnable() rospy.Subscriber("spacenav/joy", Joy, self.SpaceNavJoyCallback) rospy.Subscriber("/robot/digital_io/right_lower_cuff/state", DigitalIOState, self.CuffCallback) rospy.Subscriber("/robot/digital_io/right_button_ok/state", DigitalIOState, self.NavWheelCallback) rospy.Subscriber("/robot/digital_io/right_button_triangle/state", DigitalIOState, self.XCallback) self.DIOpub = rospy.Publisher('/io/comms/io/command', IOComponentCommand, queue_size=10) self.h = IOComponentCommand() self.h.time = rospy.Time.now() self.h.op = "set" self.h.args = "{ \"signals\" : { \"port_sink_0\" : { \"format\" : { \"type\" : \"int\", \"dimensions\" : [ 1] }, \"data\" : [ 0] } } }" self.nav = intera_interface.Navigator() #self.nav.register_callback(self.ok_pressed, '_'.join([nav_name, 'button_ok'])) #intera_interface/robot/digital_io/right_lower_cuff/state # self._valid_limb_names = {'left': 'left', # 'l': 'left', # 'right': 'right', # 'r': 'right'} self._valid_limb_names = {'right': 'right', 'r': 'right'} # get information from the SDK # self._left = intera_interface.Limb('left') self._right = intera_interface.Limb('right') #self._l_jnames = self._left.joint_names() self._r_jnames = self._right.joint_names() self.kin = sawyer_kinematics('right') # data initializations self._jointpos = [0] * 7 self._jointvel = [0] * 7 self._jointtor = [0] * 7 self._ee_pos = [0] * 3 self._ee_or = [0] * 4 self._ee_tw = [0] * 6 self._ee_wr = [0] * 6 self._ee_vel = [0] * 6 self._pijac = [ ] #numpy.matrix('0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0') self._jac = [ ] #numpy.matrix('0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0') self._inertia_mat = [ ] #numpy.matrix('0,0,0,0,0,0,0;0,0,0,0,0,0,0;0,0,0,0,0,0,0;0,0,0,0,0,0,0;0,0,0,0,0,0,0;0,0,0,0,0,0,0;0,0,0,0,0,0,0') self.Jxl_raw = 0 self.Jyl_raw = 0 self.Jzl_raw = 0 self.Jxa_raw = 0 self.Jya_raw = 0 self.Jza_raw = 0 self.leftbutton = 0 self.rightbutton = 0 self.spacenav = [] #self._l_joint_command = dict(zip(self._l_jnames,[0.0]*7)) self._r_joint_command = dict(zip(self._r_jnames, [0.0] * 7)) self.MODE_POSITION = 0 self.MODE_VELOCITY = 1 self.MODE_TORQUE = 2 self._mode = self.MODE_POSITION self.RMH_delay = .01 # initial joint command is current pose self.readJointPositions() #self.setJointCommand('left',self._jointpos[0:7]) self.setJointCommand('right', self._jointpos[0:7]) self.head_display = intera_interface.HeadDisplay() print('head display!') self.image = "/home/rachel/ros_ws/src/sawyer_simulator/sawyer_sim_examples/models/cafe_table/materials/textures/Wood_Floor_Dark.jpg" self.head_display.display_image(self.image, display_rate=100) print('finished first head display') # Start background threads self._running = True self._t_joints = threading.Thread(target=self.jointspace_worker) self._t_joints.daemon = True self._t_joints.start() self._t_effector = threading.Thread(target=self.endeffector_worker) self._t_effector.daemon = True self._t_effector.start() self._t_command = threading.Thread(target=self.command_worker) self._t_command.daemon = True self._t_command.start() self._t_display = threading.Thread(target=self.display_worker) self._t_display.daemon = True self._t_display.start() self._t_dio = threading.Thread(target=self.dio_worker) self._t_dio.daemon = True self._t_dio.start()
import rospy import intera_interface from sawyer_pykdl import sawyer_kinematics rospy.init_node("ik") limb = intera_interface.Limb("right") kinematics = sawyer_kinematics("right") import argparse parser = argparse.ArgumentParser(description="A tutorial of argparse!") parser.add_argument("pos", default=[0, 0, 0, 0, 0, 0]) args = parser.parse_args() a = args.a joints = kinematics.inverse_kinematics(pos) if (joints != None): dest = zip(limb._joint_names, joints) limb.move_to_joint_positions(dict(dest)) else: print("not a valid location")
def __init__(self): print "Initializing Node" rospy.init_node('poirot_rrhost') print "Enabling Robot" rs = intera_interface.RobotEnable() rs.enable() self.rs1 = intera_interface.RobotEnable() rospy.Subscriber("spacenav/joy", Joy, self.SpaceNavJoyCallback) rospy.Subscriber("/robot/digital_io/right_lower_cuff/state", DigitalIOState, self.CuffCallback) rospy.Subscriber("/robot/digital_io/right_button_ok/state", DigitalIOState, self.NavWheelCallback) rospy.Subscriber("/robot/digital_io/right_button_triangle/state", DigitalIOState, self.XCallback) rospy.Subscriber("/robot/digital_io/right_lower_button/state", DigitalIOState, self.GripperCloseCallback) rospy.Subscriber("/robot/digital_io/right_upper_button/state", DigitalIOState, self.GripperOpenCallback) rospy.Subscriber("/pushed1", Bool, self.pushed1callback) rospy.Subscriber("/pushed2", Bool, self.pushed2callback) rospy.Subscriber("/pushed3", Bool, self.pushed3callback) rospy.Subscriber("/pushed4", Bool, self.pushed4callback) self.DIOpub = rospy.Publisher('/io/comms/io/command', IOComponentCommand, queue_size=10) self.h = IOComponentCommand() self.h.time = rospy.Time.now() self.h.op = "set" self.h.args = "{ \"signals\" : { \"port_sink_0\" : { \"format\" : { \"type\" : \"int\", \"dimensions\" : [ 1] }, \"data\" : [ 0] } } }" rospy.Subscriber("Robotiq2FGripperRobotInput", inputMsg.Robotiq2FGripper_robot_input, self.GripperStatusCallback) self.gripperpub = rospy.Publisher( 'Robotiq2FGripperRobotOutput', outputMsg.Robotiq2FGripper_robot_output) self.grippercommand = outputMsg.Robotiq2FGripper_robot_output() self.gripperstatus = inputMsg.Robotiq2FGripper_robot_input() self.grip_des_pos = 444 #self.des_gripper_pos=0 #activate-reset-activate gripper self.grippercommand.rACT = 1 self.grippercommand.rGTO = 1 self.grippercommand.rSP = 255 self.grippercommand.rFR = 150 self.gripperpub.publish(self.grippercommand) time.sleep(.5) self.grippercommand = outputMsg.Robotiq2FGripper_robot_output() self.grippercommand.rACT = 0 self.gripperpub.publish(self.grippercommand) time.sleep(.5) self.grippercommand = outputMsg.Robotiq2FGripper_robot_output() self.grippercommand.rACT = 1 self.grippercommand.rGTO = 1 self.grippercommand.rSP = 255 self.grippercommand.rFR = 150 self.pb1 = 1 self.pb2 = 1 self.pb3 = 1 self.pb4 = 1 time.sleep(.5) self.nav = intera_interface.Navigator() self.head = intera_interface.Head() self.headangle = 0 self._valid_limb_names = {'right': 'right', 'r': 'right'} # get information from the SDK # self._left = intera_interface.Limb('left') self._right = intera_interface.Limb('right') #self._l_jnames = self._left.joint_names() self._r_jnames = self._right.joint_names() self.kin = sawyer_kinematics('right') # data initializations self._jointpos = [0] * 7 self._jointvel = [0] * 7 self._jointtor = [0] * 7 self._ee_pos = [0] * 3 self._ee_or = [0] * 4 self._ee_tw = [0] * 6 self._ee_wr = [0] * 6 self._ee_vel = [0] * 6 self._pijac = [ ] #numpy.matrix('0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0') self._jac = [ ] #numpy.matrix('0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0') self._inertia_mat = [ ] #numpy.matrix('0,0,0,0,0,0,0;0,0,0,0,0,0,0;0,0,0,0,0,0,0;0,0,0,0,0,0,0;0,0,0,0,0,0,0;0,0,0,0,0,0,0;0,0,0,0,0,0,0') self.Jxl_raw = 0 self.Jyl_raw = 0 self.Jzl_raw = 0 self.Jxa_raw = 0 self.Jya_raw = 0 self.Jza_raw = 0 self.leftbutton = 0 self.rightbutton = 0 self.spacenav = [] #self._l_joint_command = dict(zip(self._l_jnames,[0.0]*7)) self._r_joint_command = dict(zip(self._r_jnames, [0.0] * 7)) self.MODE_POSITION = 0 self.MODE_VELOCITY = 1 self.MODE_TORQUE = 2 self._mode = self.MODE_POSITION self.RMH_delay = .01 # initial joint command is current pose self.readJointPositions() #self.setJointCommand('left',self._jointpos[0:7]) self.setJointCommand('right', self._jointpos[0:7]) self.hascollided = 0 self.head_display = intera_interface.HeadDisplay() print('head display!') #self.image="/home/rachel/rawhide/rawhide_ws/src/sawyer_simulator/sawyer_sim_examples/models/cafe_table/materials/textures/Wood_Floor_Dark.jpg" self.image = "/home/rachel/rawhide/rmh_code/puppy.png" self.head_display.display_image(self.image, display_rate=100) print('finished first head display') # Start background threads self._running = True self._t_joints = threading.Thread(target=self.jointspace_worker) self._t_joints.daemon = True self._t_joints.start() self._t_effector = threading.Thread(target=self.endeffector_worker) self._t_effector.daemon = True self._t_effector.start() self._t_command = threading.Thread(target=self.command_worker) self._t_command.daemon = True self._t_command.start() self._t_display = threading.Thread(target=self.display_worker) self._t_display.daemon = True self._t_display.start() self._t_dio = threading.Thread(target=self.dio_worker) self._t_dio.daemon = True self._t_dio.start()
def __init__(self): print "Initializing Node" rospy.init_node('sawyer_jointstates') print "Enabling Robot" rs = intera_interface.RobotEnable() rs.enable() # self._valid_limb_names = {'left': 'left', # 'l': 'left', # 'right': 'right', # 'r': 'right'} self._valid_limb_names = {'right': 'right', 'r': 'right'} # get information from the SDK # self._left = intera_interface.Limb('left') self._right = intera_interface.Limb('right') #self._l_jnames = self._left.joint_names() self._r_jnames = self._right.joint_names() self.kin = sawyer_kinematics('right') # data initializations self._jointpos = [0] * 7 self._jointvel = [0] * 7 self._jointtor = [0] * 7 self._ee_pos = [0] * 3 self._ee_or = [0] * 4 self._ee_tw = [0] * 6 self._ee_wr = [0] * 6 self._ee_vel = [0] * 6 self._pijac = [ ] #numpy.matrix('0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0') self._jac = [ ] #numpy.matrix('0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0') self._inertia_mat = [ ] #numpy.matrix('0,0,0,0,0,0,0;0,0,0,0,0,0,0;0,0,0,0,0,0,0;0,0,0,0,0,0,0;0,0,0,0,0,0,0;0,0,0,0,0,0,0;0,0,0,0,0,0,0') #self._l_joint_command = dict(zip(self._l_jnames,[0.0]*7)) self._r_joint_command = dict(zip(self._r_jnames, [0.0] * 7)) self.MODE_POSITION = 0 self.MODE_VELOCITY = 1 self.MODE_TORQUE = 2 self._mode = self.MODE_POSITION self.RMH_delay = .01 # initial joint command is current pose self.readJointPositions() #self.setJointCommand('left',self._jointpos[0:7]) self.setJointCommand('right', self._jointpos[0:7]) # Start background threads self._running = True self._t_joints = threading.Thread(target=self.jointspace_worker) self._t_joints.daemon = True self._t_joints.start() self._t_effector = threading.Thread(target=self.endeffector_worker) self._t_effector.daemon = True self._t_effector.start() self._t_command = threading.Thread(target=self.command_worker) self._t_command.daemon = True self._t_command.start()