Hello everyone,
I’m trying, using a state machine, to make a stimulus pop in different locations of the visual field of my robot, once every 1.0 second (and it takes approximately 10 simulation steps = 0.2 simulation seconds to make the stimulus disapear and appear somewhere else).
At the same time, I send a strong inhibition signal to the robot’s brain (from every N to N+0.2 seconds).
However, I’m having a hard time to synchronize the simulation time (“t” in the transfer functions, which seems to be very well synchronized with the brain simulation time) with the time inside the state machine. The visual stimulus keeps popping later and later, compared to the time it should actually pop in / pop out.
EDIT: I realized that the way I’m using the state machine creates the problem (I modified an already existing one): only once all objects are popped (SM state “CreateStimulusState”), the SM state goes to “RemoveStimulusState”, which sleeps for 1 second before going to the state “CreateStimulusState” again. So the time needed to make the stimulus appear and re-appear in “CreateStimulus” is actually the de-synchronization time.
—> this leads me to another question: is there a way I can access the transfer function time (“t” in the TFs) in the state machine? Is it simple “rospy.get_rostime().to_sec()”? Thank you!
Thanks for the help!
Alban
SM:
import rospy, hbp_nrp_excontrol.nrp_states as states, std_msgs
from smach import StateMachine
from smach.state import State
from gazebo_msgs.srv import ApplyBodyWrench, GetModelState, DeleteModel, SpawnEntity, SpawnEntityRequest
from geometry_msgs.msg import Wrench, Vector3, Point
from rospy import ServiceProxy, wait_for_service
FINISHED = ‘FINISHED’
ERROR = ‘ERROR’
PREEMPTED = ‘PREEMPTED’
sm = StateMachine(outcomes=[FINISHED, ERROR, PREEMPTED])
box_names = [“box_3”, “box_10”, “box_14”, “box_15”]
box_positions = [(-9.587, -1.480, 1.680), (-9.586, -1.480, 1.680), (-9.585, -1.485, 1.698), (-9.585, -1.475, 1.664)]
stim_sdf_xmls = []
stim_sdf_xmls.append(""" some box sdf “”")
stim_sdf_xmls.append(""" another box sdf “”")
stim_sdf_xmls.append(""" another box sdf “”")
stim_sdf_xmls.append(""" another box sdf “”")
class CreateStimulusState(State):
def __init__(self, names, positions, sdf_xmls, outcomes=['success', 'aborted']):
super(CreateStimulusState, self).__init__(outcomes=outcomes) self._iteration = 0 self._names = names self._positions = positions self._spawn_proxy = rospy.ServiceProxy('/gazebo/spawn_sdf_entity', SpawnEntity, persistent=True) self._wrench_proxy = ServiceProxy('/gazebo/apply_body_wrench', ApplyBodyWrench, persistent=True) self._msgs = []
for (name, sdf_xml) in zip(names, sdf_xmls):
thisMsg = SpawnEntityRequest() thisMsg.entity_name = name thisMsg.entity_xml = sdf_xml.format(box_name = name) thisMsg.reference_frame = "world" self._msgs.append(thisMsg)
def execute(self, userdata):
self._iteration += 1 shiftY = 0.1 if self._iteration % 2 is 0 else -0.1 # 0.2 originally
for (pos, msg) in zip(self._positions, self._msgs):
msg.initial_pose.position.x = pos[0] msg.initial_pose.position.y = pos[1] + shiftY msg.initial_pose.position.z = pos[2] self._spawn_proxy(msg)
return 'success'
class RemoveStimulusState(State):
def __init__(self, names, rate=10.0, outcomes=['success', 'aborted']):
super(RemoveStimulusState, self).__init__(outcomes=outcomes) self._rate = rospy.Rate(rate) self._names = names self._state_proxy = ServiceProxy('/gazebo/get_model_state', GetModelState, persistent=True) self._delete_proxy = ServiceProxy('/gazebo/delete_model', DeleteModel, persistent=True)
def execute(self, userdata):
start_time = rospy.get_rostime() while rospy.get_rostime().to_sec() - start_time.to_sec() < 1.0: self._rate.sleep()
for name in self._names: self._delete_proxy(name)
return 'success'
with sm:
StateMachine.add(“create_stimulus”, CreateStimulusState(box_names, box_positions, stim_sdf_xmls), transitions = {“success”: “remove_stimulus”, “aborted”: ERROR})
StateMachine.add(“remove_stimulus”, RemoveStimulusState(box_names ), transitions = {“success”: “create_stimulus”, “aborted”: ERROR})
TF:
@nrp.MapVariable( ‘isBlinking’, scope=nrp.GLOBAL, initial_value=False)
@nrp.MapSpikeSource(‘resetSignal’, nrp.map_neurons(range(1), lambda i: nrp.brain.ResetNeuron[i]), nrp.dc_source, amplitude=0.0)
@nrp.Robot2Neuron()
def blink_logic(t, isBlinking, resetSignal):
# Loop timing (stimulus onset, grouping trigger, selection signals, blinking) loopDuration = 1.00 # units: seconds, duration of a blinking loop (starts with blink) blinkDuration = 0.20 # units: seconds, how much time a blink lasts tTrue = (0.04+t)/1.1 # state-machine time de-synchronize with simulation time
# Initialization isBlinking.value = False
# Time-related variables loopCount = int(tTrue/loopDuration) lastLoopTime = loopCount*loopDuration timeInLoop = tTrue-lastLoopTime
# Choose if this is a normal or a blinking time-step if 0.0 <= timeInLoop < blinkDuration: isBlinking.value = True
# Send or not the reset signal if isBlinking.value: resetSignal.amplitude = 1.0 else: resetSignal.amplitude = 0.0