def main(argv):
robot = Robot()
robot.vision.color = Color.Green
robot.vision.features = Feature.Ball
robot.start()
time.sleep(1)
robot.motors.left.setSpeed(0)
robot.motors.right.setSpeed(0)
robot.pid.start(0.7, 0.0005, 50)
# robot.motors.left.setSpeed(100)
# robot.motors.right.setSpeed(100)
# robot.pid.start(0.4, 0.0001, 100)
while robot.time.elapsed() < 20:
detections = robot.vision.detections
if detections[Feature.Ball] != None:
# print detections[Feature.Ball]
relPos = 2.0 * detections[Feature.Ball][0] / robot.vision.width - 1
# print relPos
robot.pid.setError(int(126 * relPos))
# else:
# robot.motors.left.setSpeed(0)
# robot.motors.right.setSpeed(0)
# robot.pid.reset(0, 0, 0)
# robot.motors.left.setSpeed(0)
# robot.motors.right.setSpeed(0)
# robot.motors.tower.setSpeed(0)
# robot.pid.stop()
robot.stop()
def main(argv):
try:
opts, args = getopt.getopt(argv, 'ad', ['analog', 'distance', 'duration='])
except getopt.GetoptError:
print "Arguments: -a -d --analog --distance --duration=#"
duration = None
analog = False
for opt, arg in opts:
if opt == '-a':
analog = True
if opt == '--analog':
analog = True
if opt == '--duration':
duration = int(arg)
try:
robot = Robot()
robot.start()
while robot.ready == False: pass
marker = time.time()
if analog == True:
while duration == None or time.time() - marker < duration:
print "Analog: {0:6.1f} {1:6.1f} {2:6.1f}".format(robot.ir.nirLeft.ain.getValue(), robot.ir.nirRight.ain.getValue(), robot.ir.firLeft.ain.getValue())
else:
while duration == None or time.time() - marker < duration:
print "Distance: {0:6.1f} {1:6.1f} {2:6.1f}".format(robot.ir.nirLeft.getDist(), robot.ir.nirRight.getDist(), robot.ir.firLeft.getDist())
except:
pass
robot.stop()
def handle_post(request):
print request.body
message = TextMessage.from_xml(request.body)
bot = Robot()
bot.add_handler(cacti.handler)
reply = bot.get_reply(message)
return HttpResponse(reply, content_type="application/xml")
def do_POST(self):
"""Respond to a POST request."""
content_len = int(self.headers.getheader("content-length"))
post_body = self.rfile.read(content_len)
print post_body
robot = Robot()
robot.from_json(post_body)
print
self.send_response(200)
self.send_header("Content-type", "text/html")
self.end_headers()
if robot.getOwnPosition() in robot.destination:
self.wfile.write('{ "move": [ %s, %s ], "speed" : 0, "velocity" : [1, 1] }' % robot.getOwnPosition())
else:
move = physics_a_star(
(robot.position, robot.velocity, robot.speed),
a_star(robot.position, robot.destination[0], robot.robots),
)
log = '{"robotID" : %s, "move": [ %s, %s ], "speed" : %s, "velocity" : [%s, %s] }' % (
robot.id,
move[0][0],
move[0][1],
move[2],
move[1][0],
move[1][1],
)
print log
self.wfile.write(log)
class RobotMoveTests(unittest.TestCase): def setUp(self): self._robot = Robot(0, None, None) self._target = Point(0,4) def testEndLocation(self): # TODO: expose these? r = self._robot r._speed = 0.1 r._updateDelay = 0.1 r._moveDuration = 1 r.moveTowards(self._target) l = r.location # Round to something sensible l = Point(round(l.x, 5), round(l.y, 5)) # Moved at 0.1 m/s for 1s util.assertEqual(Point(0, 0.4), l) def testDuration(self): start = datetime.now() self._robot.moveTowards(self._target) end = datetime.now() dur = end - start self.assertAlmostEqual(1.0, dur.seconds)
def test_age():
world = World()
robot = Robot(0,0,world)
robot.move(20)
assert robot.x != 0
robot = robot.age()
assert robot.x == 0
def do_POST(self):
"""Respond to a POST request."""
content_len = int(self.headers.getheader('content-length'))
post_body = self.rfile.read(content_len)
robot = Robot()
robot.from_json(post_body)
# robotsPaths = {}
if(not robotsPaths.has_key(robot.getId())):
robots[robot.getId()] = robot
self.calculatePaths(board, robot)
self.send_response(200)
self.send_header("Content-type", "text/html")
self.end_headers()
return
# print 'id: {0}, path: {1}, position: {2}'.format(robot.getId(), robotsPaths[robot.getId()], robot.position)
self.send_response(200)
self.send_header("Content-type", "text/html")
self.end_headers()
if robot.getOwnPosition() in robot.destination:
self.wfile.write('{ "move": [ %s, %s ] }' % robot.getOwnPosition())
else:
allowedMoves = robot.allowedMoves
desired = moves[robot.getId()][0]
print desired
if desired in allowedMoves:
del moves[robot.getId()][0]
self.wfile.write('{ "move": [ %s, %s ] }' % desired)
else:
self.wfile.write('{ "move": [ %s, %s ] }' % robot.getOwnPosition())
class FiniteStateMachine:
def __init__(self, color = Color.Red):
# Store match properties
self.color = color
self.state = None
# Store robot properties
self.robot = Robot()
self.robot.start()
time.sleep(1)
def start(self):
# self.state = StartState(self.robot)
self.state = ScanState(self.robot)
while self.robot.time.elapsed() < 180:
try:
print self.robot.time.string() + " " + self.state.__class__.__name__
self.state = self.state.next_state()
except KeyboardInterrupt:
self.stop()
self.stop()
def stop(self):
# Kill everything
self.robot.stop()
time.sleep(0.1)
def time_elapsed(self):
return time.time() - self.start_time
def main(argv):
try:
opts, args = getopt.getopt(argv, 'cod:', ['duration='])
except getopt.GetoptError:
print "Arguments: -c -o -d"
try:
robot = Robot()
robot.start()
while robot.ready == False: pass
marker = time.time()
duration = None
delay = None
for opt, arg in opts:
if opt == '-c':
robot.servoGate.setAngle(45)
robot.servoBridge.setAngle(5)
if opt == '-o':
robot.servoGate.setAngle(15)
robot.servoBridge.setAngle(150)
if opt == '-d':
robot.servoGate.setAngle(45)
robot.servoBridge.setAngle(150)
delay = int(arg)
if opt == '--duration':
duration = int(arg)
robot.motors.roller.setSpeed(126)
robot.motors.tower.setSpeed(-100)
while duration == None or time.time() - marker < duration:
time.sleep(0.1)
if delay != None and time.time() - marker > delay:
robot.servoGate.setAngle(15)
delay = None
except:
pass
robot.stop()
def start_robot(size):
"""
Sets up the world and robot and enters endless loop of guessing.
:param size:
"""
# create grid that contains world state
grid = Grid(size.width, size.height)
# create sensor, which queries the grid
sensor = Sensor(grid)
# create robot, which guesses location based on sensor
hmm = HMM(size.width, size.height)
robot = Robot(sensor, hmm)
moves = 0
guessed_right = 0
while True:
# move robot
grid.move_robot()
moves += 1
print "\nRobot is in: ", grid.robot_location
guessed_move, probability = robot.guess_move()
if guessed_move == grid.robot_location:
guessed_right += 1
man_distance = abs(guessed_move[0] - grid.robot_location[0]) + abs(guessed_move[1] - grid.robot_location[1])
print "Manhattan distance: ", man_distance
print "Robot has been correct:", float(guessed_right) / moves, "of the time."
sleep(1)
def run():
log("Starting robot")
initial_data = read_initial_data()
log("Initial data: %s"%initial_data)
(player_id, player_count, max_turns, width, height) = initial_data
robot = Robot(player_id, max_turns)
state = read_state(height, player_count)
game.PLAYER_ID = player_id
game.ME = get_player_with_id(player_id, state[1])
counter = 0
longest_calc = (0,0)
while state:
start = time.time()
game.CURRENT_ROUND = game.Round(state, counter)
me = get_player_with_id(player_id, state[1])
game.ME = me if me else game.ME
command = robot.play_round(state)
# if dead robot returns 'break'
if command == "break":
break
exectime = time.time() - start
longest_calc = [longest_calc,(exectime,counter)][exectime>longest_calc[0]]
log("Execution time: %s s"%exectime)
if exectime > 1:
log("Timed out.")
# break
write_command(command)
counter += 1
log(("--"*8+"Round %s"+"--"*8)%counter)
state = read_state(height, player_count)
log("Longest exec: %s"%str(longest_calc))
def test_rest_name(self):
robot = Robot()
name = robot.name
robot.reset()
name2 = robot.name
self.assertNotEqual(name, name2)
self.assertRegexpMatches(name2, self.name_re)
def main(argv):
try:
opts, args = getopt.getopt(argv, 'lrot', ['left=', 'right=', 'roller=', 'tower=', 'duration='])
robot = Robot()
robot.start()
while robot.ready == False: pass
speed = 50
duration = 5
for opt, arg in opts:
if opt == '-l':
robot.motors.left.setSpeed(speed)
if opt == '-r':
robot.motors.right.setSpeed(speed)
if opt == '-o':
robot.motors.roller.setSpeed(speed)
if opt == '-t':
robot.motors.tower.setSpeed(speed)
if opt == '--left':
robot.motors.left.setSpeed(int(arg))
if opt == '--right':
robot.motors.right.setSpeed(int(arg))
if opt == '--roller':
robot.motors.roller.setSpeed(int(arg))
if opt == '--tower':
robot.motors.tower.setSpeed(int(arg))
if opt == '--duration':
duration = int(arg)
time.sleep(duration)
robot.stop()
except getopt.GetoptError:
print "Arguments: -l -r -o -t --left=# --right=# --roller=# --tower=# --duration=#"
def cal_distances():
r = Robot()
r.calibrate_distances()
for i in range(8):
se = 'sensor'+str(i)
print se
print r.data.thresholds[se]
def __init__( self, can=None, maxAcc = 0.5, replyLog=None, metaLog=None):
Robot.__init__( self, can, maxAcc, replyLog, metaLog=metaLog )
self.WHEEL_DISTANCE = 0.315
#wd = 0.26/4.0 # with gear 1:4
wd = 427.0 / 445.0 * 0.26/4.0 # with gear 1:4
delta = 0.00015
self._WHEEL_DIAMETER = {SIDE_LEFT: wd+delta, SIDE_RIGHT: wd-delta} #TODO: different wheel diameters
self.fnSpeedLimitController = []
def situate(self
, some_map
, this_pose
, some_goal
, some_ens):
Robot.situate(self, some_map, this_pose, some_goal)
self.ensemble = some_ens
self.ensemble_artist = self.ax.quiver([], [], [], [], color='b', alpha='0.1')
def testdefault(self): """ default robot calculates force """ morph = SpringMorphology() control = SineControl(morph) robot = Robot(morph, control); f = robot.computeForces() assert all(np.isfinite(f.x)) assert all(np.isfinite(f.y))
def main(argv):
try:
opts, args = getopt.getopt(argv, 'p:i:d:s:g:t:lr', ['goal=', 'side=', 'duration='])
except getopt.GetoptError:
print "Arguments: -p# -i# -d# -s# -t# --duration=#"
kp = 0
ki = 0
kd = 0
speed = 50
goal = 30
side = 'left'
duration = None
for opt, arg in opts:
if opt == '-p':
kp = float(arg)
if opt == '-i':
ki = float(arg)
if opt == '-d':
kd = float(arg)
if opt == '-s':
speed = int(arg)
if opt == '-g':
goal = int(arg)
if opt == '-l':
side = 'left'
if opt == '-r':
side = 'right'
if opt == '-t':
duration = int(arg)
if opt == '--goal':
goal = int(arg)
if opt == '--side':
if arg.lower() == 'left':
side = 'left'
if arg.lower() == 'right':
side = 'right'
if opt == '--duration':
duration = int(arg)
try:
robot = Robot()
robot.start()
while robot.ready == False: pass
while robot.ir.nirLeftValue == None or robot.ir.nirRightValue == None: pass
marker = time.time()
robot.motors.left.setSpeed(speed)
robot.motors.right.setSpeed(speed)
robot.pid.start(kp, ki, kd)
while duration == None or time.time() - marker < duration:
error = 0
if side == 'left':
error = goal - robot.ir.nirLeftValue
if side == 'right':
error = robot.ir.nirRightValue - goal
robot.pid.setError(int(error))
except:
pass
robot.stop()
def test_get_ancestors():
world = World()
robot = Robot(0,0, world)
robot2 = Robot(0,0,world, robot)
robot3 = Robot(0,0,world, robot2)
ancestors = robot3.get_ancestors()
assert ancestors == [robot.id] + [robot2.id]
def main(argv):
if len(argv) > 0:
class_name = argv[0]
if class_name in globals():
robot = Robot()
robot.start()
time.sleep(3)
print "TESTING:", class_name
globals()[class_name](robot).next_state()
robot.stop()
def raycasting():
raw_input('place the robot at the home position and press enter.')
print 'starting a raycasting simulation'
r = Robot()
while True:
r.update_data()
real_readings = np.array(r.data.sensor_values)
ray_readings = raycasting.exp_reading_for_pose(r.pose, r.distance_thresholds)
ray_readings = np.array(ray_readings)
print 'ray readings: %s real readings: %s errors: %s' % (ray_readings, real_readings, abs(real_readings-ray_readings))
def __init__(self, pose):
Robot.__init__(self,pose)
# create shape
self._p1 = np.array([[-3.1, 4.3, 1],
[-3.1, -4.3, 1],
[ 3.3, -4.3, 1],
[ 5.2, -2.1, 1],
[ 5.7, 0 , 1],
[ 5.2, 2.1, 1],
[ 3.3, 4.3, 1]])
self._p2 = np.array([[-2.4, 6.4, 1],
[ 3.3, 6.4, 1],
[ 5.7, 4.3, 1],
[ 7.4, 1.0, 1],
[ 7.4, -1.0, 1],
[ 5.7, -4.3, 1],
[ 3.3, -6.4, 1],
[-2.5, -6.4, 1],
[-4.2, -4.3, 1],
[-4.8, -1.0, 1],
[-4.8, 1.0, 1],
[-4.2, 4.3, 1]])
# create IR sensors
self.ir_sensors = []
ir_sensor_poses = [
Pose( 1.9, 6.4, np.radians(75)),
Pose( 5.0, 5.0, np.radians(42)),
Pose( 7.0, 1.7, np.radians(13)),
Pose( 7.0, -1.7, np.radians(-13)),
Pose( 5.0, -5.0, np.radians(-42)),
Pose( 1.9, -6.4, np.radians(-75)),
Pose(-3.8, -4.8, np.radians(-128)),
Pose(-4.8, 0.0, np.radians(180))
]
for pose in ir_sensor_poses:
self.ir_sensors.append(Khepera3_IRSensor(pose,self))
# initialize motion
self.ang_velocity = (0.0,0.0)
# these were the original parameters
#self.wheel_radius = 21.0
#self.wheel_base_length = 88.5
self.wheel_radius = 3.0
self.wheel_base_length = 12.8
# #I'm not sure we need those
# self.ticks_per_rev = 2765
# self.speed_factor = 6.2953e-6
self.integrator = ode(motion_f,motion_jac)
self.integrator.set_integrator('dopri5',atol=1e-8,rtol=1e-8)
def testInit(self):
r = Robot('bob')
self.assertEqual(r.name, 'bob')
self.assertEqual(r.neighbors, [])
self.assertEqual(r.position, (0, 0))
r.addNeighbor(3)
self.assert_(3 in r.neighbors)
r.setWorld('boo')
self.assertEqual(r.world, 'boo')
def execute_robot(robot_name, max_moves):
robot = Robot(
robot_name,
ServerProxy(UrlGenerator("http://188.165.135.37:81/game?")),
NextCellCalculator(0, 100))
print("Robot start!")
robot.start(max_moves)
print("Robot finish!")
print("Result: " + robot.status)
print("Score.: " + str(robot.score))
def __init__(self, this_parameters, sonar):
Robot.__init__(self, this_parameters, sonar)
self.navigator = Navigator()
self.guard_fatness = 5
self.fixed_params = {'omega_max': self.parameters.omega_max
, 'vel_max': self.parameters.vel_max
, 'slowdown_radius': self.parameters.displacement_slowdown
, 'guard_fatness': self.guard_fatness
, 'flee_threshold': self.parameters.avoid_threshold
}
def distance_estimate():
sensor_num = raw_input('which sensor do you want to test?') or '3'
r = Robot()
while True:
r.update_data()
try:
reading = r.data.sensor_values[int(sensor_num)]
except IndexError:
print 'no data at that index'
continue
threshold = r.data.thresholds['sensor'+sensor_num]
dist = utils.estimated_distance(reading, threshold)
print '%.2f mm away on sensor %s' % (dist, sensor_num)
def command(self, control_x, control_v):
Robot.command(self, control_x, control_v)
forward_obstacle_distance = self.this_map.ray_trace(self.pose, 0, self.vel_max)
random_move = np.random.normal(0, self.control_std, 3)
random_dist = np.linalg.norm(random_move[0:2])
if forward_obstacle_distance < random_dist:
self.crashed = True
elif self.this_map.collision(*self.pose[0:2]):
self.crashed = True
else:
self.pose += random_move
self.ensemble.pf_update(control_x, control_v)
def main():
# use your own robot to run a simulation!
sim = Sim2d(SIM, ROBOT) # create a simulator according to specification
patch_robot(sim)
robot = Robot()
# choose from one of the robot's algorithms:
# sim.update = robot.simple_algorithm
# sim.update = robot.p_algorithm
# sim.update = robot.pd_algorithm
sim.update = robot.pid_algorithm(SIM['velocity'], ROBOT['PID'])
sim.run()
def main(argv):
try:
opts, args = getopt.getopt(argv, 'p:i:d:s:t:rg', ['color=', 'duration='])
except getopt.GetoptError:
print "Arguments: -p# -i# -d# -s# -t# -r -g --duration=#"
kp = 0
ki = 0
kd = 0
speed = 0
color = Color.Red
duration = None
for opt, arg in opts:
if opt == '-p':
kp = float(arg)
if opt == '-i':
ki = float(arg)
if opt == '-d':
kd = float(arg)
if opt == '-s':
speed = int(arg)
if opt == '-t':
duration = int(arg)
if opt == '-r':
color = Color.Red
if opt == '-g':
color = Color.Green
if opt == '--color':
if arg.lower() == 'red':
color = Color.Red
if arg.lower() == 'green':
color = Color.Green
if opt == '--duration':
duration = int(arg)
try:
robot = Robot()
robot.start()
while robot.ready == False: pass
marker = time.time()
robot.vision.color = color
robot.vision.features = Feature.Ball
robot.motors.left.setSpeed(speed)
robot.motors.right.setSpeed(speed)
robot.pid.start(kp, ki, kd)
while duration == None or time.time() - marker < duration:
detections = robot.vision.detections
if detections[Feature.Ball] != None:
relPos = 2.0 * detections[Feature.Ball][0] / robot.vision.width - 1
robot.pid.setError(int(126 * relPos))
except:
pass
robot.stop()
class RobotTestCase(unittest.TestCase):
def setUp(self):
self.position = Position('3', '2', 'N')
self.position.rotateClockwise = MagicMock()
self.position.rotateAnticlockwise = MagicMock()
self.position.moveForward = MagicMock()
self.robot = Robot(self.position)
def test_initRobot(self):
self.assertEqual(self.robot.position, self.position)
def test_rotation(self):
self.robot.processInstruction('R', [])
self.robot.processInstruction('R', [])
self.robot.processInstruction('L', [])
self.position.rotateClockwise.assert_called()
self.position.rotateAnticlockwise.assert_called()
self.assertEqual(self.position.rotateClockwise.call_count, 2)
def test_movement(self):
self.robot.processInstruction('F', [])
self.position.moveForward.assert_called()
def test_lostPosition(self):
self.robot.processInstruction('F', [self.position])
self.assertFalse(self.robot.position.moveForward.called)
def main(args):
"""Main entry point for robot."""
robot = Robot(target=args.target, verbose=args.verbose)
robot.start()
primary_encoder = bot.left_encoder
set_secondary = bot.set_right
secondary_encoder = bot.right_encoder
controller = PIController(proportional_constant=5, integral_constant=0.2)
# start the motors, and start the loop
set_primary(speed)
set_secondary(speed)
while primary_encoder.pulse_count < distance or secondary_encoder.pulse_count < distance:
# Sleep a bit before calculating
time.sleep(0.05)
# How far off are we?
error = primary_encoder.pulse_count - secondary_encoder.pulse_count
adjustment = controller.get_value(error)
# How fast should the motor move to get there?
set_secondary(int(speed + adjustment))
# Some debug
print("Primary c:{} ({} mm)\tSecondary c:{} ({} mm) e:{} adjustment: {:.2f}".format(
primary_encoder.pulse_count, primary_encoder.distance_in_mm(),
secondary_encoder.pulse_count, secondary_encoder.distance_in_mm(),
error,
adjustment
))
bot = Robot()
distance_to_drive = 1000 # in mm - this is a meter
distance_in_ticks = EncoderCounter.mm_to_ticks(distance_to_drive)
drive_distance(bot, distance_in_ticks, distance_in_ticks)
def __init__(self):
self.fleet = [
Robot("Optimus Prime", 100, Weapon("sword"), 15),
Robot("Bumble Bee", 100, Weapon("Laser Canon"), 20),
Robot("Night Wing", 100, Weapon("Rockets"), 20)
]
from pybricks.tools import wait, StopWatch from robot import Robot import sandbox import test import basketball import bench import step_counter import attachment_runner import mario_music import slide brick = EV3Brick() robot = None # while True: # # Create a new robot instance robot = Robot() # # Check if robot is ok # if robot.is_ok(): # # Robot is ok, check drift # if not robot.drift_check(): robot.beep() # break # # Robot not ok, wait for user to fix # robot.beep(True) # # Wait for button to be pressed and released # while not any(brick.buttons.pressed()): # wait(10)
def main():
no_rows = 10
no_cols = 9
no_obs = 10
no_matrix = 1
total_elapsed_bfs = 0
total_steps_bfs = 0
total_turns_bfs = 0
total_elapsed_dfs = 0
total_steps_dfs = 0
total_turns_dfs = 0
import time
for i in range(no_matrix):
matrix, start_position = random_matrix(no_rows, no_cols, no_obs)
start_direction = random.randint(0, 3)
# run with dfs
robot = Robot(matrix, start_position, start_direction)
# robot.log()
sweeper = DFSSweeper(robot)
sweeper.loggable = False
robot.loggable = True
start = time.time()
sweeper.sweep(callback_a)
elapsed = time.time() - start
total_elapsed_dfs += elapsed
total_steps_dfs += robot.move_count
total_turns_dfs += robot.turn_count
print('steps taken by dfs: %d, turns taken: %d, time taken: %.2fms' %
(robot.move_count, robot.turn_count, elapsed * 1000))
# run with bfs
robot = Robot(matrix, start_position, start_direction)
sweeper = Sweeper(robot)
# sweeper.loggable = False
# robot.loggable = True
# start = time.time()
# sweeper.sweep()
# elapsed = time.time() - start
total_elapsed_bfs += elapsed
total_steps_bfs += robot.move_count
total_turns_bfs += robot.turn_count
print(
'steps taken by planned bfs: %d, turns taken: %d, time taken: %.2fms'
% (robot.move_count, robot.turn_count, elapsed * 1000))
# sweeper.print_map()
# robot.log()
print('DFS: average steps taken: %d, turns taken: %d, time taken: %.2fms' %
(int(total_steps_dfs / no_matrix), int(total_turns_dfs / no_matrix),
total_elapsed_dfs * 1000 / no_matrix))
print(
'Planned BFS: average steps taken: %d, turns taken: %d, time taken: %.2fms'
% (int(total_steps_bfs / no_matrix), int(total_turns_bfs / no_matrix),
total_elapsed_bfs * 1000 / no_matrix))
print(lst)
def setUp(self):
self.robot = Robot()
#!/usr/bin/env python3 # Reset the robot position. from robot import Robot robot = Robot() robot.resetPosition() robot.disable()
def create_robot(self, event):
xc, yc = int(event.x / self.boxSide), int(event.y / self.boxSide)
cello = self.grille.getCell(xc, yc)
if cello.typeC == "?":
self.danger(
self.can,
"Vous avez séléctioné une position sur un obstacle, ce n'est pas possible"
)
self.window.wait_window(self.attention)
else:
if (self.click == 0):
self.start = cello
self.click = 1
self.grille.chgCell(xc, yc, "S")
self.reset()
elif (self.click == 1):
self.can.unbind("<ButtonPress>")
self.end = cello
self.grille.chgCell(xc, yc, "G")
self.reset()
if self.Rname == None:
self.popup()
self.window.wait_window(self.top)
self.directionStart = self.var.get()
self.directionEnd = self.var2.get()
print("direction de depart:", self.directionStart)
print("direction d arivee:", self.directionEnd)
robotStart = Node(self.start, self.directionStart)
robotEnd = Node(self.end, self.directionEnd)
print("Robot End", repr(robotEnd))
newRobot = Robot(self.Rname, robotStart, robotEnd)
self.robots.append(newRobot)
self.grille.setRobots(self.robots)
newRobot.path = self.astar.findPath(robotStart, robotEnd)
last = len(newRobot.path)
if newRobot.path[last - 1].dir != robotEnd.dir:
newRobot.path.append(robotEnd)
coor = Coordination(self.robots)
check, node = coor.validatePath(newRobot)
if node:
node.changeType("?")
self.grille.setCell(node)
newRobot.path = self.astar.findPath(
robotStart, robotEnd)
node.changeType(".")
self.grille.setCell(node)
check, node = coor.validatePath(newRobot)
self.afficher_chemin(newRobot, self.nbR)
self.nbR = self.nbR + 1
newRobot.setTime()
self.click = 0
coor.coordinateRobots()
self.paths.append(newRobot.path)
self.reset()
self.buttonactive = False
self.baddObstacle.config(state="normal")
self.bdeleteObstacle.config(state="normal")
self.msgButton.config(state="normal")
self.brobot.config(state="normal")
self.bdelete.config(state="normal")
from utilidades import cargar_mapa, cargar_instrucciones
from mapa import Mapa
from robot import Robot
import time
nombre_mapa = "mapas/" + (input("Ingrese el nombre del mapa: ")) + ".txt"
el_mapa = cargar_mapa(nombre_mapa)
nombre_instrucciones = "instrucciones/" + (
input("Ingrese el nombre de las instrucciones: ")) + ".txt"
instrucciones = cargar_instrucciones(nombre_instrucciones)
objeto_mapa = Mapa(el_mapa) #Crea el objeto Mapa
objeto_robot = Robot() #Crea el objeto Robot
objeto_mapa.asignar_robot(objeto_robot) #Asigna robot a mapa
objeto_robot.asignar_mapa(objeto_mapa) #Asigna mapa a robot
objeto_robot.buscar_robot() #Busca el robot
print()
print("Robot: ", objeto_robot.x, ",", objeto_robot.y,
objeto_robot.direccion) #Imprime posicion y direccion del robot
print(objeto_mapa.imprimir_mapa())
print()
for instruccion in instrucciones:
print(instruccion)
if instruccion == "MOVE":
objeto_robot.MOVE()
#print(objeto_mapa.imprimir_mapa())
objeto_mapa.fichas_posicion_robot()
if objeto_mapa.fichas_posicion_actual > 0:
print("Fichas en el lugar: ", objeto_mapa.fichas_posicion_actual)
def main(args):
# --------------- Setup options ---------------
is_sim = args.is_sim # Run in simulation?
obj_mesh_dir = os.path.abspath(args.obj_mesh_dir) if is_sim else None # Directory containing 3D mesh files (.obj) of objects to be added to simulation
num_obj = args.num_obj if is_sim else None # Number of objects to add to simulation
tcp_host_ip = args.tcp_host_ip if not is_sim else None # IP and port to robot arm as TCP client (UR5)
tcp_port = args.tcp_port if not is_sim else None
rtc_host_ip = args.rtc_host_ip if not is_sim else None # IP and port to robot arm as real-time client (UR5)
rtc_port = args.rtc_port if not is_sim else None
if is_sim:
#workspace_limits = np.asarray([[-0.724, -0.276], [-0.224, 0.224], [-0.0001, 0.4]]) # Cols: min max, Rows: x y z (define workspace limits in robot coordinates)
workspace_limits = np.asarray([[-0.724, -0.276], [-0.3, 0.148], [-0.0001, 0.4]])
else:
#workspace_limits = np.asarray([[-0.5, -0.25], [-0.35, 0.35], [0.3, 0.40]]) # Cols: min max, Rows: x y z (define workspace limits in robot coordinates)
#workspace_limits = np.asarray([[-0.724, -0.276], [-0.224, 0.224], [0.3, 0.50]]) # Cols: min max, Rows: x y z (define workspace limits in robot coordinates)
workspace_limits = np.asarray([[-0.724, -0.276], [-0.3, 0.148], [0.2, 0.35]])
heightmap_resolution = args.heightmap_resolution # Meters per pixel of heightmap
random_seed = args.random_seed
force_cpu = args.force_cpu
# ------------- Algorithm options -------------
method = args.method # 'reactive' (supervised learning) or 'reinforcement' (reinforcement learning ie Q-learning)
push_rewards = args.push_rewards if method == 'reinforcement' else None # Use immediate rewards (from change detection) for pushing?
future_reward_discount = args.future_reward_discount
experience_replay = args.experience_replay # Use prioritized experience replay?
heuristic_bootstrap = args.heuristic_bootstrap # Use handcrafted grasping algorithm when grasping fails too many times in a row?
explore_rate_decay = args.explore_rate_decay
grasp_only = args.grasp_only
# -------------- Testing options --------------
is_testing = args.is_testing
max_test_trials = args.max_test_trials # Maximum number of test runs per case/scenario
test_preset_cases = args.test_preset_cases
test_preset_file = os.path.abspath(args.test_preset_file) if test_preset_cases else None
# ------ Pre-loading and logging options ------
load_snapshot = args.load_snapshot # Load pre-trained snapshot of model?
snapshot_file = os.path.abspath(args.snapshot_file) if load_snapshot else None
continue_logging = args.continue_logging # Continue logging from previous session
logging_directory = os.path.abspath(args.logging_directory) if continue_logging else os.path.abspath('logs')
save_visualizations = args.save_visualizations # Save visualizations of FCN predictions? Takes 0.6s per training step if set to True
# Set random seed
np.random.seed(random_seed)
# Initialize pick-and-place system (camera and robot)
robot = Robot(is_sim, obj_mesh_dir, num_obj, workspace_limits,
tcp_host_ip, tcp_port, rtc_host_ip, rtc_port,
is_testing, test_preset_cases, test_preset_file)
# Initialize trainer
trainer = Trainer(method, push_rewards, future_reward_discount,
is_testing, load_snapshot, snapshot_file)
# Initialize data logger
logger = Logger(continue_logging, logging_directory)
logger.save_camera_info(robot.cam_intrinsics, robot.cam_pose, robot.cam_depth_scale) # Save camera intrinsics and pose
logger.save_heightmap_info(workspace_limits, heightmap_resolution) # Save heightmap parameters
# Find last executed iteration of pre-loaded log, and load execution info and RL variables
if continue_logging:
trainer.preload(logger.transitions_directory)
# Initialize variables for heuristic bootstrapping and exploration probability
no_change_count = [2, 2] if not is_testing else [0, 0] # heuristic_bootstrap, training = [2, 2], test = [0, 0], no_change_count[0]=push, [1]=grasp
explore_prob = 0.5 if not is_testing else 0.0
# Quick hack for nonlocal memory between threads in Python 2
nonlocal_variables = {'executing_action' : False,
'primitive_action' : None,
'best_pix_ind' : None,
'push_success' : False,
'grasp_success' : False}
# Parallel thread to process network output and execute actions
# -------------------------------------------------------------
def process_actions():
while True:
if nonlocal_variables['executing_action']:
print('>>>>>>> executing_action start >>>>>>>>>>')
# Determine whether grasping or pushing should be executed based on network predictions
best_push_conf = np.max(push_predictions)
best_grasp_conf = np.max(grasp_predictions)
print('> Primitive confidence scores: %f (push), %f (grasp)' % (best_push_conf, best_grasp_conf))
nonlocal_variables['primitive_action'] = 'grasp'
explore_actions = False
if not grasp_only:
if is_testing and method == 'reactive':
if best_push_conf > 2*best_grasp_conf:
nonlocal_variables['primitive_action'] = 'push'
else:
if best_push_conf > best_grasp_conf:
nonlocal_variables['primitive_action'] = 'push'
explore_actions = np.random.uniform() < explore_prob
if explore_actions: # Exploitation (do best action) vs exploration (do other action)
print('> Strategy: explore (exploration probability: %f)' % (explore_prob))
nonlocal_variables['primitive_action'] = 'push' if np.random.randint(0,2) == 0 else 'grasp'
else:
print('> Strategy: exploit (exploration probability: %f)' % (explore_prob))
trainer.is_exploit_log.append([0 if explore_actions else 1])
logger.write_to_log('is-exploit', trainer.is_exploit_log)
# If heuristic bootstrapping is enabled: if change has not been detected more than 2 times, execute heuristic algorithm to detect grasps/pushes
# NOTE: typically not necessary and can reduce final performance.
if heuristic_bootstrap and nonlocal_variables['primitive_action'] == 'push' and no_change_count[0] >= 2:
print('> Change not detected for more than two pushes. Running heuristic pushing.')
nonlocal_variables['best_pix_ind'] = trainer.push_heuristic(valid_depth_heightmap)
no_change_count[0] = 0
predicted_value = push_predictions[nonlocal_variables['best_pix_ind']]
use_heuristic = True
elif heuristic_bootstrap and nonlocal_variables['primitive_action'] == 'grasp' and no_change_count[1] >= 2:
print('> Change not detected for more than two grasps. Running heuristic grasping.')
nonlocal_variables['best_pix_ind'] = trainer.grasp_heuristic(valid_depth_heightmap)
no_change_count[1] = 0
predicted_value = grasp_predictions[nonlocal_variables['best_pix_ind']]
use_heuristic = True
else:
print('> Running not heuristic action.')
use_heuristic = False
# Get pixel location and rotation with highest affordance prediction from heuristic algorithms (rotation, y, x)
if nonlocal_variables['primitive_action'] == 'push':
nonlocal_variables['best_pix_ind'] = np.unravel_index(np.argmax(push_predictions), push_predictions.shape) # https://stackoverflow.com/questions/48135736/what-is-an-intuitive-explanation-of-np-unravel-index/48136499
predicted_value = np.max(push_predictions)
elif nonlocal_variables['primitive_action'] == 'grasp':
nonlocal_variables['best_pix_ind'] = np.unravel_index(np.argmax(grasp_predictions), grasp_predictions.shape)
predicted_value = np.max(grasp_predictions)
trainer.use_heuristic_log.append([1 if use_heuristic else 0])
logger.write_to_log('use-heuristic', trainer.use_heuristic_log)
# Save predicted confidence value
trainer.predicted_value_log.append([predicted_value])
logger.write_to_log('predicted-value', trainer.predicted_value_log)
# Compute 3D position of pixel
print('> Action: %s at (best_pix_ind, best_pix_y, best_pix_x) = (%d, %d, %d) of pixel' % (nonlocal_variables['primitive_action'], nonlocal_variables['best_pix_ind'][0], nonlocal_variables['best_pix_ind'][1], nonlocal_variables['best_pix_ind'][2]))
best_rotation_angle = np.deg2rad(nonlocal_variables['best_pix_ind'][0]*(360.0/trainer.model.num_rotations))
best_pix_x = nonlocal_variables['best_pix_ind'][2]
best_pix_y = nonlocal_variables['best_pix_ind'][1]
# 3D position [x, y, depth] of cartesian coordinate, conveted from pixel
primitive_position = [best_pix_x * heightmap_resolution + workspace_limits[0][0], \
best_pix_y * heightmap_resolution + workspace_limits[1][0], \
valid_depth_heightmap[best_pix_y][best_pix_x] + workspace_limits[2][0]]
# If pushing, adjust start position, and make sure z value is safe and not too low
if nonlocal_variables['primitive_action'] == 'push': # or nonlocal_variables['primitive_action'] == 'place':
finger_width = 0.144
safe_kernel_width = int(np.round((finger_width/2)/heightmap_resolution))
local_region = valid_depth_heightmap[max(best_pix_y - safe_kernel_width, 0):min(best_pix_y + safe_kernel_width + 1, valid_depth_heightmap.shape[0]), max(best_pix_x - safe_kernel_width, 0):min(best_pix_x + safe_kernel_width + 1, valid_depth_heightmap.shape[1])]
if local_region.size == 0:
safe_z_position = workspace_limits[2][0]
else:
safe_z_position = np.max(local_region) + workspace_limits[2][0]
primitive_position[2] = safe_z_position
# Save executed primitive
if nonlocal_variables['primitive_action'] == 'push':
trainer.executed_action_log.append([0, nonlocal_variables['best_pix_ind'][0], nonlocal_variables['best_pix_ind'][1], nonlocal_variables['best_pix_ind'][2]]) # 0 - push
elif nonlocal_variables['primitive_action'] == 'grasp':
trainer.executed_action_log.append([1, nonlocal_variables['best_pix_ind'][0], nonlocal_variables['best_pix_ind'][1], nonlocal_variables['best_pix_ind'][2]]) # 1 - grasp
logger.write_to_log('executed-action', trainer.executed_action_log)
# Visualize executed primitive, and affordances
if save_visualizations:
push_pred_vis = trainer.get_prediction_vis(push_predictions, color_heightmap, nonlocal_variables['best_pix_ind'])
logger.save_visualizations(trainer.iteration, push_pred_vis, 'push')
cv2.imwrite('visualization.push.png', push_pred_vis)
grasp_pred_vis = trainer.get_prediction_vis(grasp_predictions, color_heightmap, nonlocal_variables['best_pix_ind'])
logger.save_visualizations(trainer.iteration, grasp_pred_vis, 'grasp')
cv2.imwrite('visualization.grasp.png', grasp_pred_vis)
# Initialize variables that influence reward
nonlocal_variables['push_success'] = False
nonlocal_variables['grasp_success'] = False
change_detected = False
# Execute primitive, robot act!!! 'push' or 'grasp'
print('> Action: %s at (x, y, z) = (%f, %f, %f) of 3D' % (nonlocal_variables['primitive_action'], primitive_position[0], primitive_position[1], primitive_position[2]))
print('> best_rotation_angle: %f of 3D' % (best_rotation_angle))
if nonlocal_variables['primitive_action'] == 'push':
nonlocal_variables['push_success'] = robot.push(primitive_position, best_rotation_angle, workspace_limits)
print('> Push successful: %r' % (nonlocal_variables['push_success']))
elif nonlocal_variables['primitive_action'] == 'grasp':
nonlocal_variables['grasp_success'] = robot.grasp(primitive_position, best_rotation_angle, workspace_limits)
print('> Grasp successful: %r' % (nonlocal_variables['grasp_success']))
nonlocal_variables['executing_action'] = False
print('>>>>>>> executing_action end >>>>>>>>>>')
time.sleep(0.01)
action_thread = threading.Thread(target=process_actions)
action_thread.daemon = True
action_thread.start()
exit_called = False
# -------------------------------------------------------------
# -------------------------------------------------------------
# Start main training/testing loop
while True:
print('\n ##### %s iteration: %d ##### ' % ('Testing' if is_testing else 'Training', trainer.iteration))
iteration_time_0 = time.time()
# Make sure simulation is still stable (if not, reset simulation)
if is_sim: robot.check_sim()
def __init__(self, can=None, maxAcc=1.0, replyLog=None):
Robot.__init__(self, can, maxAcc, replyLog)
def define_problem(outputdir='.'):
'''Define case study setup (robot parameters, workspace, etc.).'''
# define robot diameter (used to compute the expanded workspace)
robotDiameter = 0.02
# define boundary
boundary = BoxBoundary2D([[0, 1], [0, 1]])
# define boundary style
boundary.style = {'color' : 'black'}
# create expanded region
eboundary = BoxBoundary2D(boundary.ranges +
np.array([[1, -1], [1, -1]]) * robotDiameter/2)
eboundary.style = {'color' : 'black'}
# create robot's workspace and expanded workspace
wspace = Workspace(boundary=boundary)
ewspace = Workspace(boundary=eboundary)
# create robot object
robot = Robot('vector', init=Point2D((0.3, 0.3)), wspace=ewspace,
stepsize=0.168)
robot.diameter = robotDiameter
logging.info('"Workspace": (%s, %s)', wspace, boundary.style)
logging.info('"Expanded workspace": (%s, %s)', ewspace, eboundary.style)
logging.info('"Robot name": "%s"', robot.name)
logging.info('"Robot initial configuration": %s', robot.initConf)
logging.info('"Robot step size": %f', robot.controlspace)
logging.info('"Robot diameter": %f', robot.diameter)
# create simulation object
sim = Simulate2D(wspace, robot, ewspace)
sim.config['output-dir'] = outputdir
# regions of interest
R1 = (BoxRegion2D([[0, 0.2], [0, 0.2]], ['r1']), 'green')
R2 = (BoxRegion2D([[0.8,1], [0, 0.2]], ['r2']), 'green')
R3 = (BoxRegion2D([[0.8, 1], [0.8,1]], ['r3']), 'green')
R4 = (BoxRegion2D([[0, 0.2], [0.8,1]], ['r4']), 'green')
# global obstacles
O1 = (BoxRegion2D([[0.4,0.6], [0, 0.1]], ['o1']), 'red')
O3 = (BoxRegion2D([[0, 0.1], [0.4,0.6]], ['o1']), 'gray')
O2 = (BoxRegion2D([[0.9, 1], [0.4,0.6]], ['o2']), 'gray')
O4 = (BoxRegion2D([[0.4,0.6], [0.4,0.6]], ['o4']), 'red')
# add all regions
regions = [R1, R2, R3, R4, O1, O2, O3, O4]
# add regions to workspace
for k, (r, c) in enumerate(regions):
# add styles to region
addStyle(r, style={'facecolor': c})
# add region to workspace
sim.workspace.addRegion(r)
# create expanded region
er = expandRegion(r, robot.diameter/2)
# add style to the expanded region
addStyle(er, style={'facecolor': c})
# add expanded region to the expanded workspace
sim.expandedWorkspace.addRegion(er)
logging.info('("Global region", %d): (%s, %s)', k, r, r.style)
# display workspace
# sim.display()
# display expanded workspace
# sim.display(expanded=True)
ltlSpec = ('[] ( (<> r1) && (<> r2) && (<> r3) && (<> r4) && '
'!(o1 || o2 || o3 || o4))')
logging.info('"Global specification": "%s"', ltlSpec)
return robot, sim, ltlSpec
def generate_robot(self, **kwargs):
self.robots.append(Robot())
self.robots[-1].set_status(**kwargs)
def main():
print('Program Started')
sim.simxFinish(-1)
clientID = sim.simxStart('127.0.0.1', 19999, True, True, 5000, 5)
if clientID != -1:
print('Connected to remote API server')
else:
print('Failed connecting to remote API server')
returnCode, leftMotor = sim.simxGetObjectHandle(
clientID, "Pioneer_p3dx_leftMotor", sim.simx_opmode_oneshot_wait)
returnCode, rightMotor = sim.simxGetObjectHandle(
clientID, "Pioneer_p3dx_rightMotor", sim.simx_opmode_oneshot_wait)
robot = Robot(clientID, 2)
returnCode, robotHandle = sim.simxGetObjectHandle(
clientID, "Pioneer_p3dx", sim.simx_opmode_oneshot_wait)
returnCode, sensorData = sim.simxGetStringSignal(clientID, "scanRanges",
sim.simx_opmode_streaming)
l_rot_prev, r_rot_prev = 0, 0
# Initial state and initial covariance matrix
prevPosition = np.matrix(getPosition(clientID, robotHandle)).T
odPrevPos = np.matrix(getPosition(clientID, robotHandle)).T
prevErrorPosition = np.zeros((3, 3))
odPrevError = np.zeros((3, 3))
a = 0
g = 0.3
count = 0
while sim.simxGetConnectionId(clientID) != -1:
v = np.zeros((2, 1))
# speedMotors = robot.breit_controller(clientID)
# sim.simxSetJointTargetVelocity(clientID, leftMotor, speedMotors[0], sim.simx_opmode_streaming)
# sim.simxSetJointTargetVelocity(clientID, rightMotor, speedMotors[1], sim.simx_opmode_streaming)
if count == 100:
# Dead reckoning
dPhiL, dPhiR, l_rot_prev, r_rot_prev = readOdometry(
clientID, leftMotor, rightMotor, l_rot_prev, r_rot_prev)
# Initialize Kalman
kalman_filter = Kalman(dPhiL, dPhiR)
odometry = Kalman(dPhiL, dPhiR)
predPosition, predError = kalman_filter.prediction(
prevPosition, prevErrorPosition)
truePos = np.matrix(getPosition(clientID, robotHandle)).T
odPosition, odError = odometry.prediction(odPrevPos, odPrevError)
odPosition[2] = truePos[2]
odPrevPos = odPosition
odPrevError = odError
# Observations
observedFeatures = readObservations(clientID)
x, y = lidar.arrangeData(observedFeatures)
lidarInputs, nLidarInputs = lidar.split_and_merge(x, y)
d_ant = 10000000000
S = np.zeros((2, 2))
H = np.zeros((2, 3))
predPosition[2] = truePos[2]
for i in range(nLidarInputs):
for j in range(len(mapInputs)):
y, S, H = kalman_filter.getInnovation(
predPosition, predError, mapInputs[j, :],
lidarInputs[:, i])
d = y.T @ np.linalg.pinv(S) @ y
if d < g**2 and d < d_ant:
# print("=============== MATCH ================\n")
v = y
d_ant = d
estPosition, estError, y, S = kalman_filter.update(
predPosition, predError, v, S, H)
prevPosition = estPosition
prevErrorPosition = estError
print(
f'Odometry position (x) - Estimated position (x) = {float(odPosition[0] - estPosition[0])}\n'
)
print(
f'Odometry position (y) - Estimated position (y) = {float(odPosition[1] - estPosition[1])}\n'
)
count = 0
count += 1
from robot import Robot from .func1 import walk_to_bord r = Robot() walk_to_bord(r, 'o')
def main():
robot = Robot("Andrew")
robot.order(Robot.COMAMND_WALK)
robot.order(Robot.COMAMND_STOP)
robot.order(Robot.COMAMND_JUMP)
def test_set_initial_robot_position(self):
customRobot = Robot(1, 2, 'E')
self.assertEqual((1, 2, 'E'), customRobot.get_current_state())
import numpy as np
import time
from robot import Robot
# User options (change me)
# --------------- Setup options ---------------
tcp_host_ip = '100.127.7.223' # IP and port to robot arm as TCP client (UR5)
tcp_port = 30002
workspace_limits = np.asarray([
[0.3, 0.748], [-0.224, 0.224], [-0.255, -0.1]
]) # Cols: min max, Rows: x y z (define workspace limits in robot coordinates)
# ---------------------------------------------
# Initialize robot and move to home pose
robot = Robot(False, None, None, workspace_limits, tcp_host_ip, tcp_port, None,
None, False, None, None)
# Repeatedly grasp at middle of workspace
grasp_position = np.sum(workspace_limits, axis=1) / 2
grasp_position[2] = -0.25
grasp_position[0] = 86 * 0.002 + workspace_limits[0][0]
grasp_position[1] = 120 * 0.002 + workspace_limits[1][0]
grasp_position[2] = workspace_limits[2][0]
while True:
robot.grasp(grasp_position, 11 * np.pi / 8, workspace_limits)
# robot.push(push_position, 0, workspace_limits)
# robot.restart_real()
time.sleep(1)
| distance[7]['close'], v_right['positive'])
rule_r3 = ctrl.Rule(
distance[0]['away'] & distance[1]['away'] & distance[2]['away']
& distance[3]['away'] & distance[4]['away'] & distance[5]['away']
& distance[6]['away'] & distance[7]['away'], v_right['positive'])
vel_ctrl = ctrl.ControlSystem(
[rule_l1, rule_l2, rule_l3, rule_r1, rule_r2, rule_r3])
self.fuzzy_system = ctrl.ControlSystemSimulation(vel_ctrl)
def get_vel(self, dist):
for i in range(len(dist)):
self.fuzzy_system.input['s' + str(i)] = dist[i]
self.fuzzy_system.compute()
return [self.fuzzy_system.output['vl'], self.fuzzy_system.output['vr']]
robot = Robot()
a = avoid_obstacles()
a.init_fuzzy()
for x in range(350):
ultrassonic = robot.read_ultrassonic_sensors()[0:8]
vel = a.get_vel(ultrassonic)
print("Ultrassonic: ", ultrassonic)
print("vel: ", vel)
robot.set_left_velocity(vel[0]) # rad/s
robot.set_right_velocity(vel[1])
time.sleep(0.2)
from robot import Robot
from time import sleep
from gpiozero import LineSensor
r = Robot()
lsensor = LineSensor(23, pull_up=True)
rsensor = LineSensor(16, pull_up=True)
lsensor.when_line = r.stop_motors
rsensor.when_line = r.stop_motors
r.set_left(60)
r.set_right(60)
while True:
sleep(0.02)
class RobotTest(unittest.TestCase):
def setUp(self):
self.robot = Robot()
def test_set_initial_robot_position(self):
customRobot = Robot(1, 2, 'E')
self.assertEqual((1, 2, 'E'), customRobot.get_current_state())
def test_get_current_state(self):
self.assertEqual(self.robot.get_current_state(), (0, 0, 'N'))
def test_rotate_clockwise(self):
self.assertEqual(self.robot.rotate_clockwise(), (0, 0, 'E'))
def test_rotate_clockwise_at_west(self):
self.robot.rotate_clockwise()
self.robot.rotate_clockwise()
self.robot.rotate_clockwise()
self.assertEqual(self.robot.rotate_clockwise(), (0, 0, 'N'))
def test_rotate_anticlockwise(self):
self.assertEqual(self.robot.rotate_anticlockwise(), (0, 0, 'W'))
def test_move_forward_when_N(self):
self.assertEqual((0, 1, 'N'), self.robot.move_forward())
def test_move_forward_when_E(self):
self.robot.rotate_clockwise()
self.assertEqual((1, 0, 'E'), self.robot.move_forward())
def test_move_forward_when_S(self):
self.robot.rotate_clockwise()
self.robot.rotate_clockwise()
self.assertEqual((0, -1, 'S'), self.robot.move_forward())
def test_move_forward_when_W(self):
self.robot.rotate_anticlockwise()
self.assertEqual((-1, 0, 'W'), self.robot.move_forward())
from simulator import Simulator
from robot import Robot
from controller import Controller
import numpy as np
import time
if __name__ == '__main__':
start_state = np.array([.2, -.2, 0, -1, 0, 1, .04, .04, .01, .1, -.3, 0])
desired_state = np.array([0, 0, 0, 0, 0, 0, 0, 0, .08, 0, 0, 0])
time = 10
dt = 1 / 10000
write_data = True
robofly = Robot()
simple_controller = Controller(dt)
dsim = Simulator(robofly, simple_controller, write_data)
dsim.equatin_solver(start_state, desired_state, time, dt)
# User options (change me)
# --------------- Setup options ---------------
obj_mesh_dir = os.path.abspath('objects/blocks')
num_obj = 10
random_seed = 1234
workspace_limits = np.asarray([
[-0.724, -0.276], [-0.224, 0.224], [-0.0001, 0.4]
]) # Cols: min max, Rows: x y z (define workspace limits in robot coordinates)
# ---------------------------------------------
# Set random seed
np.random.seed(random_seed)
# Initialize robot simulation
robot = Robot('push_grasp', obj_mesh_dir, num_obj, workspace_limits, True,
False, None, True, True)
test_case_file_name = input(
"Enter the name of the file: ") # test-10-obj-00.txt
# Fetch object poses
obj_positions, obj_orientations = robot.get_obj_positions_and_orientations()
# Save object information to file
file = open(test_case_file_name, 'w')
for object_idx in range(robot.num_obj):
# curr_mesh_file = os.path.join(robot.obj_mesh_dir, robot.mesh_list[robot.obj_mesh_ind[object_idx]]) # Use absolute paths
curr_mesh_file = os.path.join(
robot.mesh_list[robot.obj_mesh_ind[object_idx]])
file.write(
'%s %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e %.18e\n' %
def setUp(self):
# create the class before each test
self.mega_man = Robot("Mega Man", battery=50)
def plan(robot=Robot(), plot=False):
N = 200 # Number of control intervals
OBSTACLES = create_obstacles("barrel-racing")
FINISH_LINE_BUFFER = 0.1
# Setup Optimization
opti = ca.Opti()
# State variables
X = opti.variable(len(StateVars), N + 1)
xpos = X[StateVars.xIdx.value, :] # X position
ypos = X[StateVars.yIdx.value, :] # Y-position
theta = X[StateVars.thetaIdx.value, :] # Theta
vl = X[StateVars.vlIdx.value, :] # Left wheel velocity
vr = X[StateVars.vrIdx.value, :] # Right wheel velocity
al = X[StateVars.alIdx.value, :] # Left wheel acceleration
ar = X[StateVars.arIdx.value, :] # Right wheel acceleration
# Control variables
U = opti.variable(len(ControlVars), N)
jl = U[ControlVars.jlIdx.value, :] # Left wheel jerk
jr = U[ControlVars.jrIdx.value, :] # Right wheel jerk
# Total time variable
T = opti.variable()
dt = T / N # length of one control interval
# Minimize time
opti.minimize(T)
# Apply dynamic constriants
for k in range(N):
x_next = X[:, k] + robot.dynamics_model(X[:, k], U[:, k]) * dt
opti.subject_to(X[:, k + 1] == x_next)
# Wheel constraints
robot.apply_wheel_constraints(opti, vl, vr, al, ar, jl, jr)
# Boundary conditions
# Start
opti.subject_to(xpos[0] == in2m(60) - robot.LENGTH / 2)
opti.subject_to(ypos[0] == in2m(90))
opti.subject_to(theta[0] == 0)
opti.subject_to(vl[0] == 0)
opti.subject_to(vr[0] == 0)
opti.subject_to(al[0] == 0)
opti.subject_to(ar[0] == 0)
opti.subject_to(jl[0] == 0)
opti.subject_to(jr[0] == 0)
# End
robot.apply_finish_line_constraints(
opti,
xpos[-1],
ypos[-1],
theta[-1],
(
(in2m(60) - FINISH_LINE_BUFFER, in2m(60)),
(in2m(60) - FINISH_LINE_BUFFER, in2m(120)),
),
"left",
)
# Obstacles
robot.apply_obstacle_constraints(opti, xpos, ypos, theta, OBSTACLES)
# Time constraints
opti.subject_to(T >= 0)
# Compute initial guess from init traj
x_init, y_init, theta_init = load_init_json("init_traj/barrel_racing.json",
(in2m(30), in2m(90), 0.0), N)
# Initial guess
opti.set_initial(xpos, x_init)
opti.set_initial(ypos, y_init)
opti.set_initial(theta, theta_init)
opti.set_initial(vl, 0)
opti.set_initial(vr, 0)
opti.set_initial(al, 0)
opti.set_initial(ar, 0)
opti.set_initial(jl, 0)
opti.set_initial(jr, 0)
opti.set_initial(T, 10)
if plot:
# Plot initialization
plot_traj(
"Initial Trajectory",
x_init,
y_init,
theta_init,
OBSTACLES,
robot.GEOMETRY,
robot.AXIS_SIZE,
)
# Solve non-linear program
opti.solver("ipopt", {}, {"mu_init": 1e-3}) # set numerical backend
sol = opti.solve()
if plot:
# Plot result without wheel force limits
plot_traj(
"Before Wheel Force Limits",
sol.value(xpos),
sol.value(ypos),
sol.value(theta),
OBSTACLES,
robot.GEOMETRY,
robot.AXIS_SIZE,
)
# Solve the problem again, but this time with wheel force & friction limit constraints
robot.apply_wheel_force_constraints(opti, al, ar)
robot.apply_wheel_friction_constraints(opti, vl, vr, al, ar)
# Copy over X, U, and T to initialize
opti.set_initial(X, sol.value(X))
opti.set_initial(U, sol.value(U))
opti.set_initial(T, sol.value(T))
sol = opti.solve()
times = np.linspace(0, sol.value(T), N)
if plot:
# Plot final result
plot_traj(
"Final Result",
sol.value(xpos),
sol.value(ypos),
sol.value(theta),
OBSTACLES,
robot.GEOMETRY,
robot.AXIS_SIZE,
)
plt.figure()
plot_wheel_vel_accel_jerk(
times,
sol.value(vl)[:-1],
sol.value(vr)[:-1],
sol.value(al)[:-1],
sol.value(ar)[:-1],
sol.value(jl),
sol.value(jr),
)
lon_fl, lon_fr = robot.get_longitudinal_wheel_forces(al, ar)
lat_f = robot.get_lateral_wheel_force(vl, vr)
plt.figure()
plot_wheel_forces(
times,
sol.value(lon_fl)[:-1],
sol.value(lon_fr)[:-1],
sol.value(lat_f)[:-1],
)
plt.figure()
plot_total_force(
times,
np.sqrt(sol.value(lon_fl)**2 + sol.value(lat_f)**2)[:-1],
np.sqrt(sol.value(lon_fr)**2 + sol.value(lat_f)**2)[:-1],
)
interp_time = 0.02 # seconds
interp_x = interp_state_vector(times, sol.value(xpos), interp_time)
interp_y = interp_state_vector(times, sol.value(ypos), interp_time)
interp_theta = interp_state_vector(times, sol.value(theta),
interp_time)
plot_traj(
"Interp",
interp_x,
interp_y,
interp_theta,
OBSTACLES,
robot.GEOMETRY,
robot.AXIS_SIZE,
save_png=True,
)
anim = anim_traj(
"Final Result",
interp_x,
interp_y,
interp_theta,
OBSTACLES,
robot.GEOMETRY,
robot.AXIS_SIZE,
20, # milliseconds
save_gif=False,
)
plt.show()
print(f"Trajectory time: {sol.value(T)} seconds")
def robot():
return Robot()
from robot import Robot
# Initialize robot arm (requires user input)
RobotArm = Robot()
def main():
""" Current temporary routine for picking up a screwdriver """
# Move to HOME
RobotArm.move_home()
# Switch to JOINT
RobotArm.to_joint()
# Level arm and hand
RobotArm.level_hand()
# Switch to CARTESIAN
RobotArm.to_cartesian()
# Move down
RobotArm.give_command('0 0 -1500 MOVE')
# Rotate hand
RobotArm.give_command('TELL WRIST 5000 MOVE')
# Switch to CARTESIAN
RobotArm.to_cartesian()
# Move up
def test_robot_name(self):
rob = Robot('R2D2')
n = rob.get_name()
self.assertTrue(n == 'R2D2')
maxDetectionDist = 0.2
def braitenberg(dist, vel):
"""
Control the robot movement by the distances read with the ultrassonic sensors. More info: https://en.wikipedia.org/wiki/Braitenberg_vehicle
Args:
dist: Ultrassonic distances list
vel: Max wheel velocities
"""
vLeft = vRight = vel
for i in range(len(dist)):
if (dist[i] < noDetectionDist):
detect[i] = 1 - ((dist[i] - maxDetectionDist) /
(noDetectionDist - maxDetectionDist))
else:
detect[i] = 0
for i in range(8):
vLeft = vLeft + braitenbergL[i] * detect[i]
vRight = vRight + braitenbergR[i] * detect[i]
return [vLeft, vRight]
robot = Robot()
while (robot.get_connection_status() != -1):
us_distances = robot.read_ultrassonic_sensors()
vel = braitenberg(us_distances[:8], 3) #Using only the 8 frontal sensors
robot.set_left_velocity(vel[0])
robot.set_right_velocity(vel[1])
import sys
import pygame
sys.path.append('simulator')
sys.path.append('ai')
from robot import Robot
from ball import Ball
from simulation import Simulation
from fake_ai import FakeAI
from soccer_goal import SoccerGoals
from window import run_sim
c1 = FakeAI(pygame.K_LEFT, pygame.K_RIGHT, pygame.K_UP, pygame.K_DOWN)
c2 = FakeAI(pygame.K_a, pygame.K_d, pygame.K_w, pygame.K_s)
ai_arr = [c1, c2]
r1 = Robot((100, 100), 0, (255, 0, 0), 40, 5, 10, 5, c1)
r2 = Robot((100, 100), 0, (0, 0, 255), 40, 5, 10, 5, c2)
robots = [r1, r2]
goals = SoccerGoals((0, 0, 0), 100, 4)
updaters = [goals]
ball = Ball((150, 100), 3, (0, 255, 255), 0.999, 2)
dim = (640, 480)
sim = Simulation(robots, ball, updaters, dim)
run_sim(sim, ai_arr, dim)
