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)
