def test_draw_advance(self):
with open('../test_folder/single_draw_1000.p', 'rb') as f:
draws_dict = p.load(f)
my_arm = Arm.Arm(mu=0,
draws_in_advance=draws_dict['draws_in_advance'][0])
my_arm.idx = 0
cur_t = 0
reward = my_arm.draw(cur_t)
assert reward == 0 or reward == 1
def __init__(self):
self.A = bot.Arm()
self.M0 = mag.Magnet(12,25)
self.M5 = mag.Magnet(33,25)
self.M0.TURN_ON()
self.Walkmode=Walk.Walk(A,M0,M5)
self.Forkmode=Fork.Fork(A,M0,M5)
self.Cam=Camera.Camera(Walkmode.max_distance)
self.way=[]
def test_unreachable(self): arm = Arm.Arm(link1=link1, link2=link2) end_effector = position(1+link1+link2, 0) with self.assertRaises(ValueError): joints = arm.inverse_kinematics(end_effector) end_effector = position(0, 0) with self.assertRaises(ValueError): joints = arm.inverse_kinematics(end_effector)
def test_case3(self): arm = Arm.Arm(link1=link1, link2=link2, origin=makeVector(1,1)) end_effector = position(1+link2, 1-link1) joints = arm.inverse_kinematics(end_effector) self.assertAlmostEqual(joints.theta1, -pi / 2) self.assertAlmostEqual(joints.theta2, pi / 2) end_effector = position(1, 1+link1+link2) joints = arm.inverse_kinematics(end_effector) self.assertAlmostEqual(joints.theta1, pi / 2) self.assertAlmostEqual(joints.theta2, 0.0)
def test_case2(self): arm = Arm.Arm(link1=link1, link2=link2) end_effector = position(link2, -link1) joints = arm.inverse_kinematics(end_effector) self.assertAlmostEqual(joints.theta1, -pi / 2) self.assertAlmostEqual(joints.theta2, pi / 2) end_effector = position(0, link1+link2) joints = arm.inverse_kinematics(end_effector) self.assertAlmostEqual(joints.theta1, pi / 2) self.assertAlmostEqual(joints.theta2, 0.0)
def test_case3(self): arm = Arm.Arm(link1=link1, link2=link2, origin=makeVector(1,1)) joints = jointangle(pi/2, pi/2) end_effector = arm.forward_kinematics(joints) self.assertAlmostEqual(end_effector.x, 1 - link2) self.assertAlmostEqual(end_effector.y, 1 + link1) joints = jointangle(-pi/2, pi/2) end_effector = arm.forward_kinematics(joints) self.assertAlmostEqual(end_effector.x, 1 + link2) self.assertAlmostEqual(end_effector.y, 1 - link1)
def test_case2(self): arm = Arm.Arm(link1=link1, link2=link2) joints = jointangle(pi/2, pi/2) end_effector = arm.forward_kinematics(joints) self.assertAlmostEqual(end_effector.x, -link2) self.assertAlmostEqual(end_effector.y, link1) joints = jointangle(-pi/2, pi/2) end_effector = arm.forward_kinematics(joints) self.assertAlmostEqual(end_effector.x, link2) self.assertAlmostEqual(end_effector.y, -link1)
def test_draw_not_advance(self):
mu = 0.3
my_arm = Arm.Arm(mu=mu)
nb_times_first_draws = 5000
for t in range(nb_times_first_draws):
reward_t = my_arm.draw(t)
assert reward_t == 0 or reward_t == 1
assert len(my_arm.mu_hat_history) == nb_times_first_draws
assert my_arm.nb_times_drawn == nb_times_first_draws
assert abs(my_arm.mu_hat - mu) < 0.02
second_t = nb_times_first_draws + 100
my_arm.draw(second_t)
assert my_arm.nb_times_drawn == nb_times_first_draws + 1
for i in range(100):
assert my_arm.mu_hat_history[nb_times_first_draws - 1 + i] == my_arm.mu_hat_history[nb_times_first_draws - 1]
def test_case1(self): arm = Arm.Arm(link1=link1, link2=link2) end_effector = arm.compute_end_effector() self.assertAlmostEqual(end_effector.x, link1 + link2) self.assertAlmostEqual(end_effector.y, 0.0) joints = jointangle(0, -pi/2) end_effector = arm.forward_kinematics(joints) self.assertAlmostEqual(end_effector.x, link1 + link2 * cos(joints.theta2)) self.assertAlmostEqual(end_effector.y, link2 * sin(joints.theta2)) joints = jointangle(0, pi/4) end_effector = arm.forward_kinematics(joints) self.assertAlmostEqual(end_effector.x, link1 + link2 * cos(joints.theta2)) self.assertAlmostEqual(end_effector.y, link2 * sin(joints.theta2))
def __init__(self):
import sys
import os
if sys.platform == 'linux':
port_handler = PortHandler('/dev/ttyACM0')
else:
port_handler = PortHandler('/dev/' + os.listdir('/dev')[-2])
packet_handler = PacketHandler(1.0)
try:
port_handler.openPort()
port_handler.setBaudRate(1000000)
except OSError:
_ = None
self.arm = Arm(port_handler, packet_handler)
self.apply_pressure = False
self.pid = PID(self.arm.get_xy())
def main():
print(cv2.__version__)
# start
print("start noby eye")
# 係り受け解析をする
sendText4Noby("リモコンをとって")
# リモコンを探す
#
# end
searchObjectAndOrder("リモコン", "とって")
print("finish noby eye")
eyes = NobyEyes.NobyEyes()
locationXyz = eyes.meatureDistance()
arm = Arm.Arm()
arm.move(locationXyz)
print(" start arm")
# Arm.move()
# arm.grusp()
#moveArm(1, 2, 3, 4, 5, 6)
# finish arm
print("finish arm")
angles = get_triangle_angles(r.lengths[1], r.lengths[2] + r.lengths[4],
distance)
alpha = atan(destination_point[1] / destination_point[0])
base_pitch = angles[1] + alpha
second_joint = angles[2]
a = sqrt((distance**2) - (destination_point[1]**2))
base_yaw = asin(destination_point[2] / a)
#for making parallel
r.joint_angles[1] = base_pitch
r.joint_angles[2] = second_joint
r.joint_angles[0] = base_yaw
if __name__ == '__main__':
rospy.init_node('control_Deneme', anonymous=True)
arm = Arm()
pub1 = rospy.Publisher(
'/rover_arm_eksen1_joint_position_controller/command',
F64,
queue_size=1)
pub2 = rospy.Publisher(
'/rover_arm_eksen2_joint_position_controller/command',
F64,
queue_size=1)
pub3 = rospy.Publisher(
'/rover_arm_eksen3_joint_position_controller/command',
F64,
queue_size=1)
pub4 = rospy.Publisher(
'/rover_arm_eksen4_joint_position_controller/command',
F64,
def test_calcTargetDirection(self):
arm = Arm.Arm()
targetXyz= [-1,0,1]
self.assertAlmostEqual(math.pi/4,arm.calcDirectionFromTarget(targetXyz))
def test_calcLocationFromThetas(self):
arm = Arm.Arm();
thetas = [0,0,0,0,0,3.14/4]
arm.calcPositionFromAngles(thetas);
def test_calc(self):
arm = Arm.Arm();
arm.calc()
self.assertFalse(False);
def test_hogehoge(self):
arm = Arm.Arm();
locationXyz = [1,2,3]
self.assertEqual("move x:" + str(1) +" y: "+str(2)+" z: "+ str(3),arm.move(locationXyz));
import viz
from Arm import *
# set size of application window
viz.window.setSize(600, 600)
viz.window.setName("Robot Arm Simulation")
# set properties of MainWindow
window = viz.MainWindow
window.ortho(-100, +100, -100, +100, -1, 1)
window.clearcolor(viz.BLACK)
# do not translate view up to eye height
viz.eyeheight(0)
# turn mouse navigation off
viz.mouse.setOverride(viz.ON)
Arm()
viz.go()
HOST = '192.168.43.210'
PORT = 3000
socket.setdefaulttimeout(3)
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
s.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
s.bind((HOST, PORT))
s.listen(12)
pygame.init()
display = pygame.display.set_mode((1100,600))
pygame.display.set_caption('IRA @Work')
clock = pygame.time.Clock()
robot = Robot(s, 500, 300, display)
arm = Arm(s)
robot_img = pygame.image.load('robot.png')
robot.image = pygame.transform.rotozoom(robot_img, 0, 0.25)
background_color = (50,50,50)
crashed = False
mouse_x = 0
mouse_y = 0
key_pressed = False
doScan = False
def run():
pass
def showDesks():
for i in range(len(desk)):
desk[i].show(display)
"""
Program entry point
"""
import argparse, os, sys
import X86, Arm, Power, Linker, Globals
from Arch import Arch
from Globals import er
# Instantiate one object representing each architecture and put them
# in a dictionary indexed by the Arch "enum" (see Arch.py)
x86_obj = X86.X86()
arm_obj = Arm.Arm()
power_obj = Power.Power()
archs = {Arch.X86: x86_obj, Arch.ARM: arm_obj, Arch.POWER: power_obj}
considered_archs = [] # filled by setConsideredArchs
considered_sections = [".text", ".data", ".bss", ".rodata", ".tdata", ".tbss"]
def buildArgParser():
""" Construct the command line argument parser object """
res = argparse.ArgumentParser(
description="Align symbols in binaries from" + " multiple ISAs")
res.add_argument("--compiler-inst",
help="Path to the compiler installation",
required=True)
res.add_argument("--x86-bin", help="Path to the input x86 executable")
res.add_argument("--arm-bin", help="Path to the input ARM executable")
def test_idx_arm(self):
my_arm = Arm.Arm(mu=0)
my_arm.set_idx(3)
self.assertEqual(my_arm.idx,3)
# open COM3, baudrate 1000000
PORT_NAME = 'COM3'
PORT_NAME = None
BAUDRATE = 1000000
TIME_OUT = 0.05
#motorsAPI = DXSerialAPI(PORT_NAME, BAUDRATE, TIMEOUT=TIME_OUT)
# -------------------------------------------------------------------------------------------------
# ----------------------------------- ROBOT PARAMETERS SETTING ------------------------------------
#Creation of the Arm API class
joint_number = 6
motor_number = 7
motors_id = [0, 1, 2, 3, 4, 5, 6]
ArmAPI = Arm(PORT_NAME,
BAUDRATE,
TIME_OUT=TIME_OUT,
joint_number=joint_number,
motor_number=motor_number,
motors_id=motors_id)
PORT_NAME = 'COM3'
ArmAPI.open_port(PORT_NAME)
# -------------------------------------------------------------------------------------------------
# --------------------------------------- MOUVEMENTS TESTS ----------------------------------------
# UI
#ArmAPI.initialize_position()
QApp = QtWidgets.QApplication(sys.argv)
app = robot_cam_interface(ArmAPI)
app.show()
sys.exit(QApp.exec_())
# -------------------------------------------------------------------------------------------------
def loadLevel(self, levelNumber):
f = open("rsc/levels/level"+str(levelNumber)+".lvl", 'r')
lines = f.readlines()
f.close()
"""
print lines
print "________________________"
for line in lines:
print line
print "________________________"
"""
newlines = []
for line in lines:
newline = ""
for c in line:
if c != '\n':
newline += c
newlines += [newline]
lines = newlines
for line in lines:
print line
print "________________________"
for y,line in enumerate(lines):
for x,c in enumerate(line):
if c == '#':
Wall([x*self.tileSize + self.tileSize/2,
y*self.tileSize + self.tileSize/2],
self.tileSize)
if c == 'G':
Ground([x*self.tileSize + self.tileSize/2,
y*self.tileSize + self.tileSize/2],
self.tileSize)
if c == 'P':
player = PlayerMeme(self.size, 7,
[x*self.tileSize + self.tileSize/2,
y*self.tileSize + self.tileSize/2])
Arm(player)
if c == 'm':
Meme(self.size, 1,
[random.randint(7, 10), random.randint(7, 10)],
[x*self.tileSize + self.tileSize/2,
y*self.tileSize + self.tileSize/2],
random.randint(20, 100))
if c == 'Q':
Wall_5x5([x*self.tileSize + self.tileSize/2,
y*self.tileSize + self.tileSize/2],
self.tileSize)
if c == '1':
GunPickup("health",
[x*self.tileSize + self.tileSize/2,
y*self.tileSize + self.tileSize/2],
self.tileSize)
if c == "$":
self.goal = Goal([x*self.tileSize + self.tileSize/2,
y*self.tileSize + self.tileSize/2],
self.tileSize)
if c == '2':
BossMeme(self.size,
[x*self.tileSize + self.tileSize/2,
y*self.tileSize + self.tileSize/2],
self.tileSize)
if c == '3':
EndFlag([x*self.tileSize + self.tileSize/2,
y*self.tileSize + self.tileSize/2],
self.tileSize)
# -*- coding: UTF-8 -*-
import pygame
from pygame.locals import *
import sys
sys.path.append(".")
import math
import time
import threading
import Arm
arm = Arm.Arm()
arm.setDaemon(True)
arm.start()
# ボタンは 0から11まで
# アナログボタンを押すと joyaixisモーションがアナログで動く
# 軸0 左アナログコントローラ左右(左が-1から1)
# 軸1 左アナログコントローラ上下(上が-1から下が1)
# 軸2 右アナログコントローラ左右(左が-1から下が1)
# 軸3 右アナログコントローラ上下(上が-1から下が1)
# プログラム(e.button) ジョイスティックの番号 位置
# 0 1ボタン
RIGHT_ONE_BUTTON = 0
# if e.button == RIGHT_ONEBUTTON :
# print("1 button pushed");
# 1 2ボタン
RIGHT_TWO_BUTTON = 1
# 2 3ボタン
RIGHT_THREE_BUTTON = 2
# 3 4ボタン
from controller import Robot, Supervisor, Field, Node
from Arm import *
from Gripper import *
from Base import *
import numpy as np
import math
robot = Supervisor()
timestep = int(robot.getBasicTimeStep())
# Initialize the base, arm and gripper of the youbot robot
base = Base(robot)
arm = Arm(robot)
gripper = Gripper(robot)
# Enable compass/gps modules
compass = robot.getDevice('compass')
compass.enable(timestep)
gps = robot.getDevice('gps')
gps.enable(timestep)
# Initialize waypoints and state machine initial state
waypoints = [(22.26, 24.61), (22.2, 24.6), (22.03, 26.06), (26.0, 26.4),
(28.7, 25.0)] #(25.5, 25.0),
current_waypoint = waypoints.pop(0)
state = 'lower arm'
# Establish the gains for feedback controls
x_gain = 1.5
theta_gain = 2.0
def __init__(self, numberOfArms, armMean, variance):
self.arms = []
for arm in range(numberOfArms):
armRealMean = normal(armMean, variance)
arm = Arm(armRealMean, variance)
self.arms.append(arm)
