def run(self, dummy=False):
try:
self.ergo = PoppyErgoJr(use_http=True,
simulator='poppy-simu' if dummy else None,
camera='dummy')
except IOError as e:
rospy.logerr("Ergo hardware failed to init: {}".format(e))
return None
self.ergo.compliant = False
self.go_to_start()
self.last_activity = rospy.Time.now()
self.srv_reset = rospy.Service('/apex_playground/ergo/reset', Reset,
self._cb_reset)
rospy.loginfo('Ergo is ready and starts joystick servoing...')
while not rospy.is_shutdown():
self.go_or_resume_standby()
pygame.event.get()
x = self.joystick.get_axis(0)
y = self.joystick.get_axis(1)
self.servo_robot(y, x)
self.publish_eef()
self.publish_state()
self.publish_button()
# Publishers
self.publish_joy(x, y, self.joy_pub)
x = self.joystick2.get_axis(0)
y = self.joystick2.get_axis(1)
self.publish_joy(x, y, self.joy_pub2)
self.rate.sleep()
self.ergo.compliant = True
self.ergo.close()
class TestIK(unittest.TestCase):
def setUp(self):
self.jr = PoppyErgoJr(simulator='poppy-simu')
def test_dummy_controller(self):
for m in self.jr.motors:
m.goal_position = 25
# Make sure it was synced
self.jr._controllers[0]._updated.clear()
self.jr._controllers[0]._updated.wait()
for m in self.jr.motors:
self.assertEqual(m.goal_position, m.present_position)
def tearDown(self):
self.jr.close()
def make_jr_or_die_trying():
while True:
try:
jr = PoppyErgoJr()
break
except (IndexError, ValueError, OSError):
print('Tries to connect to the ErgoJr failed!'
' Retry in {}s...'.format(RETRY))
time.sleep(RETRY)
# except (EnvironmentError):
# print('You most likely have to unplug/replug your ErgoJr!')
# sys.exit(1)
print('Sucessfully connected to the ErgoJr!')
return jr
from poppy.creatures import PoppyErgoJr jr = PoppyErgoJr(simulator='threejs', use_http=True) # Magie Python pour lancer le server web en background from threading import Thread t = Thread(target=jr.http.run) t.daemon = True t.start() jr.dance.start()
def setUp(self):
self.jr = PoppyErgoJr(simulator='poppy-simu')
def setUp(self):
self.jr = PoppyErgoJr(simulator='poppy-simu')
from contextlib import closing
from numpy import mean, std
from pypot.primitive.utils import PositionWatcher
from poppy.creatures import PoppyErgoJr
from hampy import detect_markers
D = 10
cpu = []
if __name__ == '__main__':
psutil.cpu_percent()
time.sleep(.5)
with closing(PoppyErgoJr()) as jr:
jr.rest_posture.start()
jr.rest_posture.wait_to_stop()
traj_rec = PositionWatcher(jr, 25., jr.motors)
jr.dance.start()
traj_rec.start()
t0 = time.time()
while time.time() - t0 < D:
cpu.append(psutil.cpu_percent())
img = jr.camera.frame
markers = detect_markers(img)
for m in markers:
class Ergo(object):
def __init__(self):
self.rospack = RosPack()
with open(
join(self.rospack.get_path('apex_playground'), 'config',
'ergo.json')) as f:
self.params = json.load(f)
self.button = Button(self.params)
self.rate = rospy.Rate(self.params['publish_rate'])
self.eef_pub = rospy.Publisher(
'/apex_playground/ergo/end_effector_pose',
PoseStamped,
queue_size=1)
self.state_pub = rospy.Publisher('/apex_playground/ergo/state',
CircularState,
queue_size=1)
self.button_pub = rospy.Publisher('/apex_playground/ergo/button',
Bool,
queue_size=1)
self.joy_pub = rospy.Publisher('/apex_playground/ergo/joysticks/1',
Joy,
queue_size=1)
self.joy_pub2 = rospy.Publisher('/apex_playground/ergo/joysticks/2',
Joy,
queue_size=1)
self.srv_reset = None
self.ergo = None
self.extended = False
self.standby = False
self.last_activity = rospy.Time.now()
if pygame.joystick.get_count() < 2:
rospy.logerr(
"Ergo: Expecting 2 joysticks but found only {}, exiting".
format(pygame.joystick.get_count()))
sys.exit(0)
else:
self.joystick = pygame.joystick.Joystick(0)
self.joystick2 = pygame.joystick.Joystick(1)
self.joystick.init()
self.joystick2.init()
rospy.loginfo('Initialized Joystick 1: {}'.format(
self.joystick.get_name()))
rospy.loginfo('Initialized Joystick 2: {}'.format(
self.joystick2.get_name()))
def go_to_start(self, slow=True):
self.go_to([0.0, -15.4, 35.34, -8.06, -15.69, 71.99], 4 if slow else 1)
def go_to_extended(self):
extended = {'m2': 60, 'm3': -37, 'm5': -50, 'm6': 96}
self.ergo.goto_position(extended, 0.5)
self.extended = True
def go_to_rest(self):
rest = {'m2': -26, 'm3': 59, 'm5': -30, 'm6': 78}
self.ergo.goto_position(rest, 0.5)
self.extended = False
def is_controller_running(self):
return len([
node for node in rosnode.get_node_names() if 'controller' in node
]) > 0
def go_or_resume_standby(self):
recent_activity = rospy.Time.now(
) - self.last_activity < rospy.Duration(
self.params['auto_standby_duration'])
if recent_activity and self.standby:
rospy.loginfo("Ergo is resuming from standby")
self.ergo.compliant = False
self.standby = False
elif not self.standby and not recent_activity:
rospy.loginfo("Ergo is entering standby mode")
self.standby = True
self.ergo.compliant = True
if self.is_controller_running():
self.last_activity = rospy.Time.now()
def go_to(self, motors, duration):
self.ergo.goto_position(
dict(zip(['m1', 'm2', 'm3', 'm4', 'm5', 'm6'], motors)), duration)
rospy.sleep(duration)
def run(self, dummy=False):
try:
self.ergo = PoppyErgoJr(use_http=True,
simulator='poppy-simu' if dummy else None,
camera='dummy')
except IOError as e:
rospy.logerr("Ergo hardware failed to init: {}".format(e))
return None
self.ergo.compliant = False
self.go_to_start()
self.last_activity = rospy.Time.now()
self.srv_reset = rospy.Service('/apex_playground/ergo/reset', Reset,
self._cb_reset)
rospy.loginfo('Ergo is ready and starts joystick servoing...')
while not rospy.is_shutdown():
self.go_or_resume_standby()
pygame.event.get()
x = self.joystick.get_axis(0)
y = self.joystick.get_axis(1)
self.servo_robot(y, x)
self.publish_eef()
self.publish_state()
self.publish_button()
# Publishers
self.publish_joy(x, y, self.joy_pub)
x = self.joystick2.get_axis(0)
y = self.joystick2.get_axis(1)
self.publish_joy(x, y, self.joy_pub2)
self.rate.sleep()
self.ergo.compliant = True
self.ergo.close()
def servo_axis_rotation(self, x):
x = x if abs(x) > self.params['sensitivity_joy'] else 0
p = self.ergo.motors[0].goal_position
min_x = self.params['bounds'][0][0] + self.params['bounds'][3][0]
max_x = self.params['bounds'][0][1] + self.params['bounds'][3][1]
new_x = min(
max(min_x,
p + self.params['speed'] * x / self.params['publish_rate']),
max_x)
if new_x > self.params['bounds'][0][1]:
new_x_m3 = new_x - self.params['bounds'][0][1]
elif new_x < self.params['bounds'][0][0]:
new_x_m3 = new_x - self.params['bounds'][0][0]
else:
new_x_m3 = 0
new_x_m3 = max(min(new_x_m3, self.params['bounds'][3][1]),
self.params['bounds'][3][0])
self.ergo.motors[0].goto_position(new_x,
1.1 / self.params['publish_rate'])
self.ergo.motors[3].goto_position(new_x_m3,
1.1 / self.params['publish_rate'])
def servo_axis_elongation(self, x):
if x > self.params['min_joy_elongation']:
self.go_to_extended()
else:
self.go_to_rest()
def servo_axis(self, x, id):
x = x if abs(x) > self.params['sensitivity_joy'] else 0
p = self.ergo.motors[id].goal_position
new_x = p + self.params['speed'] * x / self.params['publish_rate']
if self.params['bounds'][id][0] < new_x < self.params['bounds'][id][1]:
self.ergo.motors[id].goto_position(
new_x, 1.1 / self.params['publish_rate'])
def servo_robot(self, x, y):
self.servo_axis_rotation(-x)
self.servo_axis_elongation(y)
def publish_eef(self):
pose = PoseStamped()
pose.header.frame_id = 'ergo_base'
eef_pose = self.ergo.chain.end_effector
pose.header.stamp = rospy.Time.now()
pose.pose.position.x = eef_pose[0]
pose.pose.position.y = eef_pose[1]
pose.pose.position.z = eef_pose[2]
self.eef_pub.publish(pose)
def publish_button(self):
self.button_pub.publish(Bool(data=self.button.pressed))
def publish_state(self):
# TODO We might want a better state here, get the arena center, get EEF and do the maths as in environment/get_state
angle = self.ergo.motors[0].present_position + self.ergo.motors[
3].present_position
self.state_pub.publish(
CircularState(angle=angle, extended=self.extended))
def publish_joy(self, x, y, publisher):
# Update the alst activity
if abs(x) > self.params['min_joy_activity'] or abs(
y) > self.params['min_joy_activity']:
self.last_activity = rospy.Time.now()
joy = Joy()
joy.header.stamp = rospy.Time.now()
joy.axes.append(x)
joy.axes.append(y)
publisher.publish(joy)
def _cb_reset(self, request):
rospy.loginfo("Resetting Ergo...")
self.go_to_start(request.slow)
return ResetResponse()
import time
from poppy.creatures import PoppyErgoJr
# INIT JOYSTICK
os.environ["SDL_VIDEODRIVER"] = "dummy"
pygame.display.init()
screen = pygame.display.set_mode((1,1))
pygame.joystick.init()
j = pygame.joystick.Joystick(0)
j.init()
print 'Initialized Joystick : %s' % j.get_name()
# INIT POPPY
poppy = PoppyErgoJr(camera="dummy")
poppy.compliant = False
# goto start position
for m,p in zip(poppy.motors, [0.0, -15.4, 35.34, -8.06, -15.69, 71.99]):
m.goto_position(p, 1.)
time.sleep(1.)
print "Poppy started"
def servo_axis0(x, id):
if x <= 1 and x >= -1:
p = poppy.motors[id].goal_position
if -180 < p+x < 180 :
poppy.motors[id].goto_position(p + 2*x, 0.1)
def setUp(self):
self.jr = PoppyErgoJr(simulator='poppy-simu', use_snap=True)
self.base_url = 'http://127.0.0.1:6969'
import cv2
from poppy.creatures import PoppyErgoJr
if __name__ == '__main__':
jr = PoppyErgoJr()
while True:
img = jr.camera.frame
for m in jr.marker_detector.markers:
m.draw_contour(img)
cv2.imshow('live', img)
cv2.waitKey(100)
class TestPrimLifeCycle(unittest.TestCase):
def setUp(self):
self.jr = PoppyErgoJr(simulator='poppy-simu')
def tearDown(self):
self.jr.close()
def test_teardown(self):
self.jr.dance.start()
time.sleep(random.random() * 5)
self.jr.dance.stop()
self.assertEqual({m.led for m in self.jr.motors}, {'off'})
self.assertEqual({m.led for m in self.jr.dance.robot.motors}, {'off'})
def test_set_once(self):
class Switcher(LoopPrimitive):
def setup(self):
self.current_state = False
self.old_state = self.current_state
def update(self):
if self.current_state != self.old_state:
for m in self.robot.motors:
self.affect_once(m, 'led',
'red' if self.current_state else 'off')
self.old_state = self.current_state
p = Switcher(self.jr, 10)
p.start()
for m in self.jr.motors:
m.led = 'off'
self.jr.m3.led = 'pink'
self.assertEqual([m.led for m in self.jr.motors],
['off', 'off', 'pink', 'off', 'off', 'off'])
p.current_state = not p.current_state
time.sleep(.3)
self.assertEqual([m.led for m in self.jr.motors],
['red', 'red', 'red', 'red', 'red', 'red'])
self.jr.m3.led = 'blue'
self.assertEqual([m.led for m in self.jr.motors],
['red', 'red', 'blue', 'red', 'red', 'red'])
p.stop()
def test_start_pause_stop(self):
self.jr.dance.start()
self.jr.dance.pause()
self.jr.dance.stop()
def test_start_stop_pause_resume_random_order(self):
cmd = ['start', 'stop', 'pause', 'resume']
for _ in range(10):
getattr(self.jr.dance, random.choice(cmd))()
def test_lowerlimit_correctly_setup(self):
self.jr = PoppyErgoJr(simulator='poppy-simu')
def test_lowerlimit_correctly_setup(self):
self.jr = PoppyErgoJr(simulator='poppy-simu')
self.jr.close()
class TestIK(unittest.TestCase):
def test_lowerlimit_correctly_setup(self):
self.jr = PoppyErgoJr(simulator='poppy-simu')
self.jr.close()
