def __init__(self): ''' ''' self.rightArm = arm.Arm('right') self.leftArm = arm.Arm('left') self.rightArm.sim.env.SetViewer('qtcoin') self.valid = True
def __init__(self): self.rightArm = arm.Arm((450, 300), pygame.mouse.get_pos(), 100) self.leftArm = arm.Arm((350, 300), pygame.mouse.get_pos(), 100) # Stores the velocity at which the arms are lifting self.velocity = 0 # Height lifted in pixels self.height = self.rightArm.anchor_point[1]
def __init__(self): ''' ''' self.rightArm = arm.Arm('right') self.leftArm = arm.Arm('left') self.rightArm.sim.env.SetViewer('qtcoin') self.valid = True self.graspPlanner = planner.Planner('right') rightArmPose = self.rightArm.get_pose() self.currentGoal = rightArmPose.position.array # Set goal as default to current starting position
def __init__(self): rospy.init_node("xbox_ctl", anonymous=True) rospy.Subscriber("/joy", Joy, self.callback) self.pub = rospy.Publisher("/motor_ctl", MotorCMD, queue_size=10) self.last_type = 0 self.arm = arm.Arm(20, math.pi / 20) rospy.spin()
def test_inverse_kinematics(self): a = arm.Arm(5, 4, 1) a.move_hand(5, 3, 0.1, 0) pos = a.get_hand_position() self.assertAlmostEqual(pos[0], 5) self.assertAlmostEqual(pos[1], 3) self.assertAlmostEqual(pos[2], 0.1)
def test_move_hand(self): a = arm.Arm(5, 4, 1) a.move_hand(5, 4, 0.1, 0) joint = a.get_actuators() self.assertAlmostEqual(joint['shoulder'], 0) self.assertAlmostEqual(joint['elbow'], pi / 2) self.assertAlmostEqual(joint['z'], 0.1) self.assertAlmostEqual(joint['wrist'], -pi / 2)
def test_get_tcp_5_offset(self): a = arm.Arm(1, 1, 1, 1, 2, 1) theta = pi / 2 x, y, z, theta_hand = a.get_tcp_offset(tool=5, theta=theta) self.assertAlmostEqual(0, theta_hand) self.assertAlmostEqual(2 * math.cos(theta), x) self.assertAlmostEqual(2 * math.sin(theta), y) self.assertAlmostEqual(1, z)
def rung_kutta4t(obj, x1, u, dt): obj = arm.Arm() k1 = obj.xdot(x1, u) k2 = obj.xdot(x1 + dt / 2 * k1, u) k3 = obj.xdot(x1 + dt / 2 * k2, u) k4 = obj.xdot(x1 + dt * k3, u) x2 = x1 + dt / 6 * (k1 + 2 * k2 + 2 * k3 + k4) return x2
def test_move_tcp(self): a = arm.Arm(5, 4, 1) a.move_tcp(tool=1, x=5, y=3, z=0, theta=0) hand = a.get_hand_position() hand_orientation = a.get_hand_orientation() tcp_offset = a.get_tcp_offset(tool=1, theta=0) self.assertAlmostEqual(5, hand[0] + tcp_offset[0]) self.assertAlmostEqual(3, hand[1] + tcp_offset[1]) self.assertAlmostEqual(0, hand[2] + tcp_offset[2]) self.assertAlmostEqual(0, hand_orientation + tcp_offset[3])
def main(speed=1, text="Hello world!", test=False, brightness=0.05, angle=0): speedActive = 5 * speed speedInactive = 16 * speed letterSleep = 0.5 / speed tfm = coordinateTransform.CoordinateTransform(9.7, angle=math.pi * angle / 180) lineDrawer = arm.Arm(tfm, speedActive, speedInactive, letterSleep, brightness) if test: lineDrawer.setStraight() time.sleep(2) else: lineDrawer.drawText(str(text))
def __init__(self): #self.grasp_traj = None #self.return_grasp_traj = None #self.grasp_traj_sub = rospy.Subscriber('/check_handle_grasps/grasp_trajectory', gm.PoseArray, self._grasp_traj_callback) #self.return_grasp_traj_sub = rospy.Subscriber('/check_handle_grasps/return_grasp_trajectory', gm.PoseArray, self._return_grasp_traj_callback) self.grasp_joint_traj = None self.return_grasp_joint_traj = None self.grasp_joint_traj_sub = rospy.Subscriber( '/check_handle_grasp/grasp_joint_trajectory', tm.JointTrajectory, self._grasp_joint_traj_callback) self.return_grasp_joint_traj_sub = rospy.Subscriber( '/check_handle_grasp/return_grasp_joint_trajectory', tm.JointTrajectory, self._return_grasp_joint_traj_callback) self.sim = simulator.Simulator(view=True) self.arm = arm.Arm('right', sim=self.sim) self.speed = 0.06
def write_framerate(frame, frame_rate, position=(500, 90), font=cv2.FONT_HERSHEY_SIMPLEX, color=(0, 50, 100)): #TODO add other params """Writes framerate in place (rewrites frame argument)""" printed_text = 'The framerate is {}.'.format(frame_rate) write_text(frame, printed_text, position, font, color) # ROBOT # Connect the arm robot_arm = arm.Arm() robot_arm.connect() if robot_arm.is_connected(): print(robot_arm.get_info()) else: raise Exception('Failed to connect') command = None # Position the arm to READY position above the dot while True: command = input('Please write the command for the Robot: ') if command == 'q': robot_arm.disconnect() elif command == 'track': break else:
b1 = sock1.connect() # connection to 2nd brick "pie" #ID2 = '00:16:53:0A:4B:2B' # MAC address #sock2 = BlueSock(ID2) #b2 = sock2.connect() mx = nxt.Motor(b1, nxt.PORT_A) # left-side my = nxt.Motor(b1, nxt.PORT_B) # right-side d = drive.Drive(mx,my) marm = nxt.Motor(b1, nxt.PORT_C) # arm motor a = arm.Arm(marm) marm.turn(power=-100,tacho_units=200,brake=True) marm.turn(power=100,tacho_units=200,brake=True) # this waved the arm around turn_arm = lambda x: marm.turn(power=-x,tacho_units=180,brake=True) [ turn_arm(_x) for _x in power ] power = [100,-100,100,-100] touch = nxt.Touch(b1,nxt.PORT_1) ultrasonic = nxt.Ultrasonic(b1,nxt.PORT_2) b1.close() b2.close()
from microWebSrv import MicroWebSrv import arm robotArm = arm.Arm() # robotArm.setData('10|20|30|40|open') print('WS ACCEPT') def _acceptWebSocketCallback(webSocket, httpClient): print("WS ACCEPT") webSocket.RecvTextCallback = _recvTextCallback webSocket.RecvBinaryCallback = _recvBinaryCallback webSocket.ClosedCallback = _closedCallback def _recvTextCallback(webSocket, msg): print('Rcv message: %s' % msg) if 'arm:setData:' in msg: armData = msg.split(':')[2] robotArm.setData(armData) webSocket.SendText('armData:%s' % robotArm.getData()) elif 'arm:getData' in msg: webSocket.SendText('armData:%s' % robotArm.getData()) elif 'arm:leftRightBy' in msg: robotArm.incrementPosition(0, msg.split(':')[2]) webSocket.SendText('armData:%s' % robotArm.getData()) elif 'arm:upDownBy' in msg: robotArm.incrementPosition(1, msg.split(':')[2]) webSocket.SendText('armData:%s' % robotArm.getData())
#!/usr/bin/python3 #-*- coding: utf-8 -*- # SSC-32 Python implementation # for CapTechU robot arm # Adapted from https://bitbucket.org/vooon/pyssc32 import arm, script from time import sleep test = "/home/techno/scriptV2.csv" #c=c.controller(None,115200) s = None robot = arm.Arm(arm.c) home = False joypad = False def call(servo=None, pos=None, pwm=None, **kwargs): print(servo, pos, pwm) for key, val in kwargs.items(): if key == "home": print("Homing arm...", val) pass def init(): global s if arm.c.board.is_open == False: arm.c.connect() #Port will be opened if successful robot.highArm.home = 2500 s = script.Script(test, call)
queue_size=1) gripper_pub = rospy.Publisher('/' + robot_name + '/gripper', Bool, queue_size=1) cmdvel_pub = rospy.Publisher('/' + robot_name + '/cmd_vel', Twist, queue_size=1) # time delay to register the subscribers time.sleep(0.5) # object that holds the base movement functions robot_control = controller.Controller(cmdvel_pub) # object that holds the arm movement functions arm_control = arm.Arm(gripper_pub, joints_pub, urdf) # states machine for the task def timerCallBack(event): global state global x, y, z, quat global task_time global trans, rotat position = odom.pose.pose.position # getting position and orientation for the arm's tip current pose try: # get the current transform between the base_link and the tip of the tool (trans, r) = listener.lookupTransform('/base_link', '/tool',
# end the game if there's any winner if board.complete(): break # draw opponent's and computer's move on the screen drawComputerMoves(image) drawOpponentMoves(image) drawRegions(image) cv2.imshow('TICTACTOE',image) if cv2.waitKey(1) & 0xFF == ord('q'): break print ("winner is", board.winner()) pygame.mixer.music.load("beep2.wav") pygame.mixer.music.play() time.sleep(1) videoCapture.release() cv2.destroyAllWindows() if __name__ == "__main__": arm = arm.Arm(SERIALPORT) arm.initialize() regions = regions.Regions(10,480,480,10,3,3) videoCapture = cv2.VideoCapture(VIDEPORT) board = tictactoe.Tic() image = None main()
def test_init(self): a = arm.Arm(10, 1) self.assertEqual(list(a.get_actuators().values()), [0, 0, 0, 0])
def test_inexistent_tcp(self): a = arm.Arm() with self.assertRaises(ValueError): a.move_tcp(42, 0, 0, 0, 0)
import numpy as np from time import sleep import arm import hebi from hebi.util import create_mobile_io # Set up arm family_name = "Example Arm" module_names = [ "J1_base", "J2_shoulder", "J3_elbow", "J4_wrist1", "J5_wrist2", "J6_wrist3" ] hrdf = "hrdf/A-2085-06.hrdf" p = arm.ArmParams(family_name, module_names, hrdf) a = arm.Arm(p) # Mobile device setup phone_family = 'HEBI' phone_name = "mobileIO" lookup = hebi.Lookup() sleep(2) print('Waiting for Mobile IO device to come online...') m = create_mobile_io(lookup, phone_family, phone_name) if m is None: raise RuntimeError("Could not find Mobile IO device") m.update() abort_flag = False
max_episodes = 5000 max_it = 200 t = 0.5 dt = t / max_it batch_size = 1 num_inputs = 3 input_bound = 5 single_actions = np.linspace(-input_bound, input_bound, num_inputs) actions = [] for action in itertools.combinations_with_replacement(single_actions, m): actions.append(np.array(action)) num_actions = len(actions) Q = nn.Qnetwork(n, hidden_layers, num_actions) myarm = arm.Arm() mean_rewards = [] success_freq = [] successes = [] for ep in range(max_episodes): x = np.zeros([n, max_it]) x[:, 0] = np.array([pi / 2, pi / 2, 0, 0]) y = np.zeros([num_actions, max_it]) mean_reward = 0 success = False for i in range(1, max_it - 1): state = x[:, i] qval = Q.predict(state.reshape([1, n])) if (random.random() < rand_eps): action = np.random.randint(0, num_actions) else:
# You may need to import some classes of the controller module. Ex: # from controller import Robot, Motor, DistanceSensor from controller import Robot import lift as lift import arm as arm import time # create the Robot instance. robot = Robot() # get the time step of the current world. timestep = int(robot.getBasicTimeStep()) armMotor = robot.getDevice('l5') arm = arm.Arm(armMotor) # arm.out() grabConnector = robot.getDevice('connector') # grab = grab.Grab(grabConnector ) liftMotors = [] liftMotorsNames = ['l1', 'l2', 'l3', 'l4'] for i in range(len(liftMotorsNames)): liftMotors.append(robot.getDevice(liftMotorsNames[i])) liftMotors[i].setPosition(0) lift = lift.Lift(liftMotors) level3 = 0.183
def __init__(self, origin, i_angles, g_angles, obstacles): self.arm = arm.Arm(origin, i_angles, g_angles, obstacles) self.query = None
import gripper import time driver = Driver(port='/dev/tty.usbserial-AR0JW21B') # Import AX-12 register constants # Get "moving" register for servo with ID 1 #is_moving = driver.getReg(1, P_MOVING, 1) # Set the "moving speed" register for servo with ID 1 #speed = 50 # A number between 0 and 1023 #driver.setReg(1, P_GOAL_SPEED_L, [speed%256, speed>>8]) #driver.setReg(2,P_GOAL_SPEED_L, [speed%256, speed>>8]) 761 496 a = arm.Arm() time.sleep(0.2) print(a.current_position()) a.recenter() a.move((383, 467)) time.sleep(1) driver.setReg(2, 24, [0, 0]) driver.setReg(1, 24, [0, 0]) time.sleep(1) a.recenter() '''a.move((761, 496)) a.move((761,480)) ## over piece a.move((761, 512)) a.move((740,512))
def test_zeros(self): a = arm.Arm() a.set_zeros({'z': 1, 'shoulder': 2, 'elbow': 3, 'wrist': 4}) self.assertEqual(list(a.get_actuators().values()), [1, 2, 3, 4])
#!/usr/bin/python3 #-*- coding: utf-8 -*- # SSC-32 Python implementation # for CapTechU robot arm # Adapted from https://bitbucket.org/vooon/pyssc32 import arm import itertools from time import sleep #c=c.controller(None,115200) robot = arm.Arm(arm.c) robot2 = arm.Arm(arm.d) home = False joypad = False def init(): if arm.c.board.is_open == False: arm.c.connect() #Port will be opened if successful if arm.d.board.is_open == False: arm.d.connect() robot.highArm.home = 2500 robot2.highArm.home = 2500 pass def main(): print("Welcome to Chinese Controller for SSC-32") init() center()
def test_arm_length(self): a = arm.Arm(10, 1) pos = a.get_hand_position() self.assertEqual(pos[0], 11) self.assertEqual(pos[1], 0)