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))()