def startup_swordfight(host_att, host_def):
print("=== connecting to poppies")
poppy_def = from_remote('{}.local'.format(host_def), 4242) # def = flogo4
poppy_att = from_remote('{}.local'.format(host_att), 4242) # att = flogo2
print("=== connection established")
controller_def = SwordFightController(poppy_def, mode="def")
controller_att = SwordFightController(poppy_att, mode="att")
controller_att.compliant(False)
controller_def.compliant(False)
controller_att.set_max_speed(50)
controller_def.set_max_speed(150)
controller_def.rest()
controller_att.rest()
print("=== resetting robots")
time.sleep(2)
return controller_att, controller_def
import time
from pypot.creatures import PoppyErgoJr
from poppy_helpers.controller import SwordFightController
from pypot.robot import from_remote
robot_sim = PoppyErgoJr(simulator='vrep')
poppy_att = from_remote('flogo2.local', 4242)
controller_att = SwordFightController(poppy_att, mode="att")
controller_att.rest()
poses = [
[45, 0, 0, 0, 0, 0],
[0, 45, 0, 0, 0, 0],
[0, 0, 45, 0, 0, 0],
[0, 0, 0, 45, 0, 0],
[0, 0, 0, 0, 45, 0],
[0, 0, 0, 0, 0, 45],
]
for pose in poses:
for i, m in enumerate(robot_sim.motors):
m.goal_position = pose[i]
controller_att.goto_pos(pose)
time.sleep(5)
import time
from poppy_helpers.constants import MOVES
from poppy_helpers.controller import SwordFightController
from pypot.robot import from_remote
poppy_def = from_remote('flogo4.local', 4242)
controller_def = SwordFightController(poppy_def, mode="def")
controller_def.rest()
controller_def.compliant(False)
controller_def.set_max_speed(150)
#
# while True:
# print (controller_def.get_pos())
# time.sleep(2)
#
#
# # [-14.52, -20.09, 65.54, 98.97, -21.85, -53.81]
# # [8.36, -27.13, 65.84, -75.51, -62.32, -25.37]
# # [-39.15, -10.7, 101.91, 96.92, 20.97, -70.53]
# # [-13.64, 2.49, 41.79, 82.55, -85.19, 27.71]
for i in range(4):
controller_def.goto_pos(MOVES["def{}".format(i)])
time.sleep(2)
controller_def.rest()
import time
from pypot.robot import from_remote
import numpy as np
from gym_ergojr.sim.single_robot import SingleRobot
robot_sim = SingleRobot(debug=True)
robot_real = from_remote('flogo2.local', 4242)
poses = [
[45, 0, 0, 0, 0, 0],
[0, 45, 0, 0, 0, 0],
[0, 0, 45, 0, 0, 0],
[0, 0, 0, 45, 0, 0],
[0, 0, 0, 0, 45, 0],
[0, 0, 0, 0, 0, 45],
]
for pose in poses:
pose_sim = np.deg2rad(pose + [0] * 6) / (np.pi / 2)
robot_sim.set(pose_sim)
robot_sim.act2(pose_sim[:6])
# robot_sim.step()
for i in range(100):
robot_sim.step()
print(robot_sim.observe().round(1))
for i, m in enumerate(robot_real.motors):
m.goal_position = pose[i]
time.sleep(2)