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()
class TestIK(unittest.TestCase): def test_lowerlimit_correctly_setup(self): self.jr = PoppyErgoJr(simulator='poppy-simu') self.jr.close()
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()
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 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()
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))()