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('/nips2016/ergo/reset', Reset, self._cb_reset) rospy.loginfo('Ergo is ready and starts joystick servoing...') self.force_speeds() 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 run(self, simulator=None): rospy.loginfo("Controller is connecting to {}...".format( self.robot_name)) port = rospy.get_param('vrep/port', 19997) try: config = rospy.get_param('description') config_file = rospy.get_param('description_file') except KeyError: rospy.logerr( "Expected robot description in parameter 'description' and file path in 'description_file', please use the launch file to load them" ) exit(-1) try: self.ergo = PoppyErgoJr(config=config_file, use_http=True, simulator=simulator, scene="keep-existing", port=port) except IOError as e: rospy.logerr("{} failed to init: {}".format(self.robot_name, e)) exit(-1) else: self.ergo.power_up() self.ergo.compliant = False self.reset_max_speed() ########################## Setting up services self.srv_robot_execute = rospy.Service('execute', ExecuteTrajectory, self._cb_execute) self.srv_robot_target = rospy.Service('reach', ReachTarget, self._cb_reach) self.srv_robot_set_compliant = rospy.Service( 'set_compliant', SetCompliant, self._cb_set_compliant) rospy.loginfo("{} controllers are up!".format(self.robot_name)) while not rospy.is_shutdown(): self.publish_goals() self.publish_eef(self.ergo.chain.end_effector) self.publish_js() self.publish_rate.sleep() finally: if self.ergo is not None: self.ergo.close()
def setUp(self): port = get_open_port() self.jr = PoppyErgoJr(simulator='poppy-simu', use_ws=True, ws_port=port) self.ws_url = 'ws://127.0.0.1:{}'.format(port) while True: try: self.ws = websocket.WebSocket() self.ws.connect(self.ws_url) break except ConnectionError: time.sleep(1.0)
class TestDummy(unittest.TestCase): def setUp(self): self.jr = PoppyErgoJr(simulator='poppy-simu') def test_dummy_controller(self): for m in self.jr.motors: m.moving_speed = 10000 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 test_empty_primitive(self): p = EmptyPrim(self.jr, 50.0) p.start() p.stop() def tearDown(self): self.jr.close()
def setUp(self): port = get_open_port() self.jr = PoppyErgoJr(simulator='poppy-simu', use_snap=True, snap_port=port) self.base_url = 'http://127.0.0.1:{}'.format(port) # Make sure the Snap API is running before actually testing. while True: try: self.get('/') break except requests.exceptions.ConnectionError: time.sleep(1)
def make_jr_or_die_trying(): while True: try: jr = PoppyErgoJr(config='config_new.json') 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
def __init__(self): self.robot = PoppyErgoJr(simulator="poppy-simu") # print("Open the simulator here:") # print(" http://simu.poppy-project.org/poppy-ergo-jr/") self.nn = None self.nb_joints = len(self.robot.motors) self.size = 0 #On ingore la taille de la base qui est à 1, mais on somme le reste des links for link in self.robot.chain.links[1:]: self.size += link.length self.motor_range = [] self.motor_limit = [] for m in self.robot.motors: mi, ma = m.angle_limit if ma < mi: t = ma ma = mi mi = t self.motor_range.append(ma - mi) self.motor_limit.append((mi, ma))
class TestIK(unittest.TestCase): def test_lowerlimit_correctly_setup(self): self.jr = PoppyErgoJr(simulator='poppy-simu') self.jr.close()
import time from pypot.creatures import PoppyErgoJr poppy = PoppyErgoJr(simulator='vrep', scene='poppy_ergo_jr_holder.ttt', camera='dummy') #Routine imposée 1 : le balai-brosse 6 secondes def balai_brosse(): poppy.m5.goto_position(-90, 2) poppy.m2.goto_position(70, 2) poppy.m3.goto_position(0, 2) time.sleep(2) poppy.m1.goto_position(180, 2, wait=True) poppy.m1.goto_position(0, 2, wait=True) #Routine imposée 2 : le shaker 12 secondes def shaker(): poppy.m3.goto_position(0, 2, wait=True) poppy.m2.goto_position(0, 2, wait=True) poppy.m5.goto_position(0, 2, wait=True) poppy.m1.goto_position(1440, 6) for i in range(3): poppy.m5.goto_position(90, 1, wait=True) poppy.m5.goto_position(0, 1, wait=True) # Position imposée 1 : Reverse 3 secondes def Reverse():
from contextlib import closing from numpy import mean, std from pypot.primitive.utils import PositionWatcher from pypot.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:
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)
from pypot.creatures import PoppyErgoJr import time poppy = PoppyErgoJr() poppy.dance.start() time.sleep(5) poppy.dance.stop() time.sleep(5)
class Ergo(object): def __init__(self): self.rospack = RosPack() with open( join(self.rospack.get_path('nips2016'), '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('/nips2016/ergo/end_effector_pose', PoseStamped, queue_size=1) self.state_pub = rospy.Publisher('/nips2016/ergo/state', CircularState, queue_size=1) self.button_pub = rospy.Publisher('/nips2016/ergo/button', Bool, queue_size=1) self.joy_pub = rospy.Publisher('/nips2016/ergo/joysticks/1', Joy, queue_size=1) self.joy_pub2 = rospy.Publisher('/nips2016/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() if self.params['useless_joystick_id'] != int( self.joystick2.get_name()[-1]): useless_joy = self.joystick self.joystick = self.joystick2 self.joystick2 = useless_joy rospy.loginfo('Initialized Joystick 1: {}'.format( self.joystick.get_name())) rospy.loginfo('Initialized Joystick 2: {}'.format( self.joystick2.get_name())) def force_speeds(self): for m in self.ergo.motors: m.moving_speed = 100 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('/nips2016/ergo/reset', Reset, self._cb_reset) rospy.loginfo('Ergo is ready and starts joystick servoing...') self.force_speeds() 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()
def setUp(self): self.jr = PoppyErgoJr(simulator='poppy-simu')
def setUp(self): self.jr = PoppyErgoJr(simulator='dummy')
import cv2 from pypot.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)
def test_lowerlimit_correctly_setup(self): self.jr = PoppyErgoJr(simulator='poppy-simu') self.jr.close()
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 self.switched = Event() 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 self.switched.set() 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.switched.clear() p.current_state = not p.current_state p.switched.wait() 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))()
class ErgoJrControllers(object): def __init__(self, robot_name): """ :param robot_name: Robot name and ROS topics/services namespace """ self.rospack = RosPack() with open( join(self.rospack.get_path('poppy_ergo_jr_controllers'), 'config', 'ergo_jr.json')) as f: self.params = json.load(f) self.publish_rate = rospy.Rate(self.params['publish_rate']) self.robot_name = robot_name self.eef_pub = rospy.Publisher('end_effector_pose', PoseStamped, queue_size=1) self.js_pub = rospy.Publisher('joint_state', JointState, queue_size=1) self.goal_pub = rospy.Publisher('joint_goals', JointState, queue_size=1) # Services self.srv_robot_target = None self.srv_robot_execute = None self.srv_robot_set_compliant = None # Protected resources self.ergo = None self.robot_lock = RLock() def reset_max_speed(self): # Reseting max speed to 100% for m in self.ergo.motors: m.moving_speed = 100 def run(self, simulator=None): rospy.loginfo("Controller is connecting to {}...".format( self.robot_name)) port = rospy.get_param('vrep/port', 19997) try: config = rospy.get_param('description') config_file = rospy.get_param('description_file') except KeyError: rospy.logerr( "Expected robot description in parameter 'description' and file path in 'description_file', please use the launch file to load them" ) exit(-1) try: self.ergo = PoppyErgoJr(config=config_file, use_http=True, simulator=simulator, scene="keep-existing", port=port) except IOError as e: rospy.logerr("{} failed to init: {}".format(self.robot_name, e)) exit(-1) else: self.ergo.power_up() self.ergo.compliant = False self.reset_max_speed() ########################## Setting up services self.srv_robot_execute = rospy.Service('execute', ExecuteTrajectory, self._cb_execute) self.srv_robot_target = rospy.Service('reach', ReachTarget, self._cb_reach) self.srv_robot_set_compliant = rospy.Service( 'set_compliant', SetCompliant, self._cb_set_compliant) rospy.loginfo("{} controllers are up!".format(self.robot_name)) while not rospy.is_shutdown(): self.publish_goals() self.publish_eef(self.ergo.chain.end_effector) self.publish_js() self.publish_rate.sleep() finally: if self.ergo is not None: self.ergo.close() def publish_eef(self, eef_pose): pose = PoseStamped() pose.header.stamp = rospy.Time.now() pose.header.frame_id = 'ergo_jr_base' 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_js(self): js = JointState() js.header.stamp = rospy.Time.now() js.name = [m.name for m in self.ergo.motors] js.position = [m.present_position for m in self.ergo.motors] js.velocity = [m.present_speed for m in self.ergo.motors] js.effort = [m.present_load for m in self.ergo.motors] self.js_pub.publish(js) def publish_goals(self): js = JointState() js.header.stamp = rospy.Time.now() js.name = [m.name for m in self.ergo.motors] js.position = [m.goal_position for m in self.ergo.motors] js.velocity = [m.goal_speed for m in self.ergo.motors] self.goal_pub.publish(js) def _cb_execute(self, request): # TODO Action server thread = Thread(target=self.execute, args=[request.trajectory]) thread.daemon = True thread.start() return ExecuteTrajectoryResponse() def _cb_reach(self, request): target = dict(zip(request.target.name, request.target.position)) duration = request.duration.to_sec() if duration <= 0.: rospy.loginfo("Teleporting {}...".format(str(target))) else: rospy.loginfo("Reaching {} in {} sec...".format( str(target), duration)) with self.robot_lock: if duration <= 0.: # Duration = 0 teleports the joint or goes at full speed for motor in request.target.name: try: index = ['m1', 'm2', 'm3', 'm4', 'm5', 'm6'].index(motor) except ValueError: pass else: self.ergo.motors[index].goal_position = target[motor] else: # Other durations will trigger a threaded control self.ergo.goto_position(target, request.duration.to_sec()) self.reset_max_speed() return ReachTargetResponse() def execute(self, trajectory): with self.robot_lock: rospy.loginfo( "Executing Ergo Jr trajectory with {} points...".format( len(trajectory.points))) time = 0. try: for point_id, point in enumerate(trajectory.points): if rospy.is_shutdown(): break time_from_start = point.time_from_start.to_sec() duration = time_from_start - time if duration < 0.: rospy.logwarn( "Skipping invalid point {}/{} with incoherent time_from_start", point_id + 1, len(trajectory.points)) continue self.ergo.goto_position( dict(zip(trajectory.joint_names, point.positions)), self.params['time_margin'] + duration) # Time margin trick to smooth trajectory rospy.sleep(duration - 0.001) time = time_from_start except rospy.exceptions.ROSInterruptException: rospy.logwarn("Trajectory aborted!") else: rospy.loginfo("Trajectory ended!") def _cb_set_compliant(self, request): rospy.loginfo("{} now {}".format( self.robot_name, 'compliant' if request.compliant else 'rigid')) with self.robot_lock: for m in self.ergo.motors: m.compliant = request.compliant return SetCompliantResponse()
from pypot.creatures import PoppyErgoJr from pypot.primitive import LoopPrimitive #from poppy_ergo_jr.postures import IdleBreathing from poppy_ergo_jr.primitives.postures import CuriousPosture from poppy_ergo_jr.primitives.postures import IdleBreathing from poppy_ergo_jr.primitives.dance import Dance #robot = PoppyErgoJr(simulator='poppy-simu') robot = PoppyErgoJr(config='config_new.json') # dance = Dance(robot) # dance.start() breathing = IdleBreathing(robot, 1.) c = CuriousPosture(robot, 1.) for i in [1]: breathing.start() breathing.stop() c.start() c.stop() for i in [1, 2, 3, 4, 5, 6]: breathing.start() breathing.stop()
class TestPrimLifeCycle(unittest.TestCase): def setUp(self): self.jr = PoppyErgoJr(simulator='dummy') 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 self.switched = Event() 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 self.switched.set() 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.switched.clear() p.current_state = not p.current_state p.switched.wait() 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))()