def __init__(self, time_step):
# time step in ms
self.time_step = time_step
# supevisor node
self.robot = Supervisor()
# self node to get velocity and position
self.bin = self.robot.getSelf()
# cameras
self.cameras = []
camerasNames = ['camera1', 'camera2']
for name in camerasNames:
self.cameras.append(self.robot.getCamera(name))
self.cameras[-1].enable(self.time_step)
self.img_size = (self.cameras[0].getWidth(),
self.cameras[0].getHeight())
# wheels
self.wheels = []
wheelsNames = ['wheel1', 'wheel2', 'wheel3', 'wheel4']
for name in wheelsNames:
self.wheels.append(self.robot.getMotor(name))
self.wheels[-1].setPosition(float('inf'))
self.wheels[-1].setVelocity(0.0)
# ball
self.ball = self.robot.getFromDef('Ball')
def __init__(self, ros_active=False, mode='normal'):
# requires WEBOTS_ROBOT_NAME to be set to "amy" or "rory"
self.ros_active = ros_active
self.time = 0
self.clock_msg = Clock()
self.supervisor = Supervisor()
self.amy_node = self.supervisor.getFromDef("amy")
self.rory_node = self.supervisor.getFromDef("rory")
self.jack_node = self.supervisor.getFromDef("jack")
self.donna_node = self.supervisor.getFromDef("donna")
self.melody_node = self.supervisor.getFromDef("melody")
if mode == 'normal':
self.supervisor.simulationSetMode(
Supervisor.SIMULATION_MODE_REAL_TIME)
elif mode == 'paused':
self.supervisor.simulationSetMode(Supervisor.SIMULATION_MODE_PAUSE)
elif mode == 'fast':
self.supervisor.simulationSetMode(Supervisor.SIMULATION_MODE_FAST)
else:
self.supervisor.simulationSetMode(
Supervisor.SIMULATION_MODE_REAL_TIME)
self.motors = []
self.sensors = []
self.timestep = int(self.supervisor.getBasicTimeStep())
# resolve the node for corresponding name
self.robot_names = ["amy", "rory", "jack", "donna", "melody"]
self.robot_nodes = {}
self.translation_fields = {}
self.rotation_fields = {}
# check if None
for name in self.robot_names:
node = self.supervisor.getFromDef(name)
if node is not None:
self.robot_nodes[name] = node
self.translation_fields[name] = node.getField("translation")
self.rotation_fields[name] = node.getField("rotation")
if self.ros_active:
rospy.init_node("webots_ros_supervisor", argv=['clock:=/clock'])
self.clock_publisher = rospy.Publisher("/clock",
Clock,
queue_size=1)
self.model_state_publisher = rospy.Publisher("/model_states",
ModelStates,
queue_size=1)
self.reset_service = rospy.Service("reset", Empty, self.reset)
self.initial_poses_service = rospy.Service("initial_pose", Empty,
self.set_initial_poses)
self.set_robot_position_service = rospy.Service(
"set_robot_position", SetRobotPose, self.robot_pose_callback)
self.reset_ball_service = rospy.Service("reset_ball", Empty,
self.reset_ball)
self.world_info = self.supervisor.getFromDef("world_info")
self.ball = self.supervisor.getFromDef("ball")
def main():
parser = argparse.ArgumentParser()
parser.add_argument('--duration',
type=float,
default=10,
help='Duration of the animation in seconds')
parser.add_argument('--output',
default='../../animation/index.html',
help='Path at which the animation will be saved')
args = parser.parse_args()
robot = Supervisor()
timestep = int(robot.getBasicTimeStep())
receiver = robot.getDevice('receiver')
receiver.enable(timestep)
robot.step(timestep)
robot.animationStartRecording(args.output)
step_i = 0
done = False
n_steps = (1000 * args.duration) / robot.getBasicTimeStep()
while not done and robot.step(timestep) != -1 and step_i < n_steps:
step_i += 1
if receiver.getQueueLength() > 0:
if receiver.getData().decode('utf-8') == 'done':
done = True
receiver.nextPacket()
robot.animationStopRecording()
for _ in range(10):
robot.step(timestep)
print('The animation is saved')
robot.simulationQuit(0)
def __init__(self, claim_log: ClaimLog,
attached_territories: AttachedTerritories) -> None:
self._claim_log = claim_log
self._attached_territories = attached_territories
self._robot = Supervisor()
self._claim_timer = ActionTimer(2, self.handle_claim_timer_tick)
self._connected_territories = self._attached_territories.build_attached_capture_trees(
)
self._emitters = {
station_code: get_robot_device(self._robot,
station_code + "Emitter", Emitter)
for station_code in StationCode
}
self._receivers = {
station_code: get_robot_device(self._robot,
station_code + "Receiver", Receiver)
for station_code in StationCode
}
for receiver in self._receivers.values():
receiver.enable(RECEIVE_TICKS)
for station_code in StationCode:
self.set_node_colour(station_code,
ZONE_COLOURS[Claimant.UNCLAIMED])
for claimant in Claimant.zones():
self.set_score_display(claimant, 0)
def move_walls_after(seconds: int) -> None:
robot = Supervisor()
timestep = robot.getBasicTimeStep()
walls = [
webots_utils.node_from_def(robot, 'west_moving_wall'),
webots_utils.node_from_def(robot, 'west_moving_triangle'),
webots_utils.node_from_def(robot, 'east_moving_wall'),
webots_utils.node_from_def(robot, 'east_moving_triangle'),
]
if controller_utils.get_robot_mode() == 'comp':
# Interact with the supervisor "robot" to wait for the start of the match.
while robot.getCustomData() != 'start':
robot.step(int(timestep))
# wait for the walls to start moving in ms
robot.step(seconds * 1000)
print('Moving arena walls') # noqa: T001
for wall in walls:
wall.setVelocity(LINEAR_DOWNWARDS)
# wait for the walls to reach their final location
robot.step(1000)
for wall in walls:
wall.resetPhysics()
def setup(self):
self.robot = Supervisor()
self.motor_left = self.robot.getMotor("motor_left")
self.motor_right = self.robot.getMotor("motor_right")
self.timestep = int(self.robot.getBasicTimeStep())
def main():
"""Main"""
# Duration of each simulation
simulation_duration = 20
# Get supervisor to take over the world
world = Supervisor()
n_joints = 10
timestep = int(world.getBasicTimeStep())
freqs = 1
# Get and control initial state of salamander
reset = RobotResetControl(world, n_joints)
# Simulation example
amplitude = None
phase_lag = None
turn = None
parameter_set = [[freqs, amplitude, phase_lag, turn],
[freqs, amplitude, phase_lag, turn]]
for simulation_i, parameters in enumerate(parameter_set):
reset.reset()
run_simulation(
world,
parameters,
timestep,
int(1000 * simulation_duration / timestep),
logs="./logs/example/simulation_{}.npz".format(simulation_i))
# Pause
world.simulationSetMode(world.SIMULATION_MODE_PAUSE)
pylog.info("Simulations complete")
world.simulationQuit(0)
def init_supervisor():
global supervisor, robot_node, target_node, enemy_node
global start_translation, start_rotation
# create the Supervisor instance.
supervisor = Supervisor()
# do this once only
root = supervisor.getRoot()
root_children_field = root.getField("children")
robot_node = None
target_node = None
for idx in range(root_children_field.getCount()):
if root_children_field.getMFNode(idx).getDef() == "Player":
robot_node = root_children_field.getMFNode(idx)
if root_children_field.getMFNode(idx).getDef() == "Goal":
target_node = root_children_field.getMFNode(idx)
if root_children_field.getMFNode(idx).getDef() == "Enemy":
enemy_node.append(root_children_field.getMFNode(idx))
enemy_init_trans.append(
copy.copy(enemy_node[-1].getField("translation").getSFVec3f()))
enemy_init_rot.append(
copy.copy(enemy_node[-1].getField("rotation").getSFRotation()))
start_translation = copy.copy(
robot_node.getField("translation").getSFVec3f())
start_rotation = copy.copy(robot_node.getField("rotation").getSFRotation())
def __init__(self):
self._supervisor = Supervisor()
self._timestep = 64
# List of objects to be taken into consideration
self._objects_names = []
self._objects = {}
# List of links name
self._link_names = ["arm1", "arm2", "arm3", "arm4", "arm5", "finger1", "finger2"]
# Get motors
self._link_objects = {}
for link in self._link_names:
self._link_objects[link] = self._supervisor.getMotor(link)
# Get sensors
self._link_position_sensors = {}
self._min_position = {}
self._max_position = {}
for link in self._link_names:
self._link_position_sensors[link] = self._link_objects[link].getPositionSensor()
self._link_position_sensors[link].enable(self._timestep)
self._min_position[link] = self._link_objects[link].getMinPosition()
self._max_position[link] = self._link_objects[link].getMaxPosition()
self._supervisor.step(self._timestep)
def main():
supervisor = Supervisor()
# assert supervisor.getSynchronization()
brokerAddress = EXTERN_BROKER_ADDRESS if EXTERN_BROKER_ADDRESS is not None\
else '127.0.0.1:{}'.format(BROKER_PORT)
if EXTERN_BROKER_ADDRESS is None:
brokerProcess = subprocess.Popen(
[findBrokerExecutable(), '-port',
str(BROKER_PORT)],
stdout=sys.stdout)
else:
brokerProcess = None
try:
channel = grpc.insecure_channel(brokerAddress)
stub = control_pb2_grpc.ControlStub(channel)
simStateHandler = SimStateHandler(stub=stub, supervisor=supervisor)
simStateHandler.start()
ticker = WbtTicker(supervisor=supervisor)
ticker.start()
ticker.join() # run forever
finally:
if brokerProcess is not None:
brokerProcess.terminate()
if ticker.isRunning:
ticker.isRunning = False
ticker.join()
simStateHandler.cancel()
simStateHandler.join()
def __init__(self,max_steps=200,ACTIONS='DISCRETE'):
self.name = "Mitsos"
self.max_steps = max_steps
self.robot = Supervisor() # create the Robot instance
self.timestep = int(self.robot.getBasicTimeStep()) # get the time step of the current world.
# crash sensor
self.bumper = self.robot.getDeviceByIndex(36) #### <---------
self.bumper.enable(self.timestep)
# camera sensor
self.camera = self.robot.getDevice("camera")
self.camera.enable(self.timestep)
# ir sensors
IR_names = ["ps0", "ps1", "ps2", "ps3", "ps4", "ps5", "ps6", "ps7"]
self.InfraredSensors = [self.robot.getDevice(s) for s in IR_names]
for ir in self.InfraredSensors: ir.enable(self.timestep)
# wheels motors
motors = ["left wheel motor","right wheel motor"]
self.wheels = []
for i in range(len(motors)):
self.wheels.append(self.robot.getDevice(motors[i]))
self.wheels[i].setPosition(float('inf'))
self.wheels[i].setVelocity(0)
self.robot.step(self.timestep)
self.cam_shape = (self.camera.getWidth(),self.camera.getHeight())
self.sensors_shape = (14,)
self.stepCounter = 0
self.shaping = None
# Actions
self.ACTIONS = ACTIONS
self.RELATIVE_ROTATIONS = True
self.FIXED_ORIENTATIONS = False
# State
self.POSITION = True
self.IRSENSORS = True
self.CAMERA = False
if self.FIXED_ORIENTATIONS:
self.discrete_actions = [0,1,2,3]
if self.RELATIVE_ROTATIONS:
self.discrete_actions = [0,1,2]
if self.ACTIONS=='DISCRETE':
self.action_size = len(self.discrete_actions)
else:
self.action_size = 2
self.path = []
self.substeps = 10
self.n_obstacles = 25
self.d = 0.3
self.create_world()
self.map = DynamicMap(self.START[0],self.START[1],0.02)
def __init__(self):
self.sup = Supervisor()
self.timeStep = int(4 * self.sup.getBasicTimeStep())
# Create the arm chain from the URDF
self.armChain = Chain.from_urdf_file('ur5e.urdf')
self.arm_motors = self.getARMJoint()
self.grip_motors = self.getGripperJoint()
def __init__(self, params):
self.robot = Supervisor()
self.robot_sup = self.robot.getFromDef("e-puck")
self.robot_trans = self.robot_sup.getField("translation")
self.robot_rotation = self.robot_sup.getField("rotation")
self.compass = self.robot.getCompass("compass")
self.motorLeft = self.robot.getMotor("left wheel motor")
self.motorRight = self.robot.getMotor("right wheel motor")
self.positionLeft = self.robot.getPositionSensor("left wheel sensor")
self.positionRight = self.robot.getPositionSensor("right wheel sensor")
self.params = params
# real robot state
self.x = []
self.y = []
self.theta = []
self.distance_sensors_info = []
# odometry estimation
self.x_odometry = []
self.y_odometry = []
self.theta_odometry = []
self.sensorNames = [
'ds0', 'ds1', 'ds2', 'ds3', 'ds4', 'ds5', 'ds6', 'ds7'
]
# predicted data
self.x_pred = []
self.y_pred = []
self.theta_pred = []
self.data_collector = DataCollector()
self.movement_controller = MovementController()
self.window_communicator = WindowCommunicator(self.robot)
# predictors
self.predictor = PredictorNNSensorsNotNormalized()
self.predictorCoord = PredictorNNCoordinates()
self.timestep = int(self.robot.getBasicTimeStep())
# particles filter initialization
robot_initial_conf = RobotConfiguration(
self.params.INIT_X, self.params.INIT_Y,
self.convert_angle_to_xy_coordinates(self.params.INIT_ANGLE))
environment_conf = EnvironmentConfiguration(self.params.MAX_X,
self.params.MAX_Y)
self.particles_filter = ParticlesFilter(environment_conf,
robot_initial_conf,
self.predictor, self.params)
self.movement_random = True
pass
def main():
supervisor = Supervisor()
# set positions of the robot and pedestrian
human_node = supervisor.getFromDef("HUMAN")
trans_field = human_node.getField("translation")
print(trans_field.getSFVec3f())
pos = getPosition()
trans_field.setSFVec3f([pos[0], pos[1], 0])
print(trans_field.getSFVec3f())
def __init__(self):
self.supervisor = Supervisor()
super().__init__(self.supervisor)
self.spot_node = self.supervisor.getFromDef(
"Spot_Node") # Taken from DEF in .wbt file
self.rot_vec = self.spot_node.getField("rotation")
self.com_node = self.supervisor.getFromDef("CentMass")
self.goal_pt = np.array([0, 0, -16.71])
self.goal_rad = 2
def __init__(self):
self.number_of_blocks = 5
# define the controller name we want to connect to
os.environ['WEBOTS_ROBOT_NAME'] = 'supervisor'
# get the controller
self.supervisor = Supervisor()
# get timestep of the simulation
self.timestep = int(self.supervisor.getBasicTimeStep())
# position and rotation of the cobot in the simulation
self.ur3e_position = [0.69, 0.74, 0]
self.ur3e_rotation = Rot.from_rotvec(-(np.pi / 2) *
np.array([1.0, 0.0, 0.0]))
# get all blocks of the simulation
self.blocks = []
for i in range(self.number_of_blocks):
self.blocks.append(self.supervisor.getFromDef(
"block{0}".format(i)))
# get the position of the end-effector
self.endEffector = self.supervisor.getFromDef("gps")
# initialize the variable to grab objects
self.grabbed = []
for i in range(self.number_of_blocks):
self.grabbed.append(False)
self.is_grabbed = False
# get the positions field for all blocks
self.block_positions_field = []
self.block_rotations_field = []
for i in range(self.number_of_blocks):
# get the fields
self.block_positions_field.append(
self.blocks[i].getField("translation"))
self.block_rotations_field.append(
self.blocks[i].getField("rotation"))
# get the name of the blocks
self.block_names = []
for i in range(self.number_of_blocks):
self.block_names.append(
self.blocks[i].getField("name").getSFString())
self.is_recording = False
self.table_color_field = self.supervisor.getFromDef("table").getField(
'trayAppearance').getSFNode().getField('baseColor')
rospy.Subscriber('touchsensor_status',
Int8,
self.touch_callback,
queue_size=1)
self.touchsensors = 0
def __init__(self, runtime: int, timestep: int, event_time: int):
# create the Robot instance.
# robot = Robot()
# Supervisor extends Robot but has access to all the world info.
# Useful for automating the simulation.
self.__robot = Supervisor()
# get the time step (ms) of the current world.
self.__sim_timestep = int(self.__robot.getBasicTimeStep())
self.__runtime = runtime
self.__event_time = event_time
# self.__timestep = timestep
self.__morphology = None
def main() -> None:
quit_if_development_mode()
supervisor = Supervisor()
prepare(supervisor)
remove_unused_robots(supervisor)
with record_animation(supervisor,
REPO_ROOT / 'recordings' / recording_path()):
run_match(supervisor)
def __init__(self, fake=False):
"""
The SupervisorController, a Webots controller that can control the world.
Set the environment variable WEBOTS_ROBOT_NAME to "supervisor_robot" if used with 1_bot.wbt or 4_bots.wbt.
:param ros_active: Whether to publish ROS messages
:param mode: Webots mode, one of 'normal', 'paused', or 'fast'
:param do_ros_init: Whether rospy.init_node should be called
:param base_ns: The namespace of this node, can normally be left empty
"""
# requires WEBOTS_ROBOT_NAME to be set to "supervisor_robot"
self.time = 0
self.clock_msg = Clock()
self.supervisor = Supervisor()
self.localization = fake
self.supervisor.simulationSetMode(Supervisor.SIMULATION_MODE_REAL_TIME)
self.motors = []
self.sensors = []
self.timestep = int(self.supervisor.getBasicTimeStep())
# resolve the node for corresponding name
self.robot_names = ["robot1", "robot2", "robot3", "robot4"]
self.robot_nodes = {}
self.translation_fields = {}
self.rotation_fields = {}
# check if None
for name in self.robot_names:
node = self.supervisor.getFromDef(name)
if node is not None:
self.robot_nodes[name] = node
self.translation_fields[name] = node.getField("translation")
self.rotation_fields[name] = node.getField("rotation")
clock_topic = "/clock"
self.clock_publisher = rospy.Publisher(clock_topic,
Clock,
queue_size=1)
self.reset_service = rospy.Subscriber("/reset_robot", Pose,
self.reset_robot)
self.initial_poses_service = rospy.Service("/initial_pose", Empty,
self.set_initial_poses)
self.reset_ball_service = rospy.Subscriber("/reset_ball", Pose,
self.reset_ball)
self.world_info = self.supervisor.getFromDef("world_info")
self.ball = self.supervisor.getFromDef("ball")
self.transform_broadcaster = tf.TransformBroadcaster()
def __init__(self,
timesteps=32,
gamma=0.99,
epsilon=1.0,
epsilon_min=0.01,
epsilon_log_decay=0.99,
alpha=0.01):
self.supervisor = Supervisor()
self.robot_node = self.supervisor.getFromDef("MY_BOT")
if self.robot_node is None:
sys.stderr.write(
"No DEF MY_ROBOT node found in the current world file\n")
sys.exit(1)
self.trans_field = self.robot_node.getField("translation")
self.rot_field = self.robot_node.getField("rotation")
self.timestep = timesteps
self.camera = Camera('camera')
self.camera.enable(self.timestep)
self.init_image = self.get_image()
self.timestep = timesteps
self.receiver = Receiver('receiver')
self.receiver.enable(self.timestep)
self.emitter = Emitter('emitter')
self.memory = deque(maxlen=50000)
self.batch_size = 128
self.alpha = alpha
self.gamma = gamma
self.epsion_init = epsilon
self.epsilon_min = epsilon_min
self.epsilon_decay = epsilon_log_decay
self.pre_state = self.init_image
self.pre_action = -1
self.pre_go_straight = False
self.reward = 0
self.step = 0
self.max_step = 200
self.file = None
# interactive
self.feedbackProbability = 0
self.feedbackAccuracy = 1
self.PPR = False
self.feedbackTotal = 0
self.feedbackAmount = 0
self.init_model()
self.init_parametter()
def main():
"""Main"""
# Get supervisor to take over the world
world = Supervisor()
n_joints = 10
timestep = int(world.getBasicTimeStep())
# Get and control initial state of salamander
reset = RobotResetControl(world, n_joints)
# Simulation arguments
arguments = world.getControllerArguments()
pylog.info("Arguments passed to smulation: {}".format(arguments))
# Exercise example to show how to run a grid search
if "example" in arguments:
exercise_example(world, timestep, reset)
# Exercise 9b - Phase lag + amplitude study
if "9b" in arguments:
pylog.info("Running 9b.....")
#exercise_example(world, timestep, reset)
exercise_9b(world, timestep, reset)
# Exercise 9c - Gradient amplitude study
if "9c" in arguments:
exercise_9c(world, timestep, reset)
# Exercise 9d1 - Turning
if "9d1" in arguments:
exercise_9d1(world, timestep, reset)
# Exercise 9d2 - Backwards swimming
if "9d2" in arguments:
exercise_9d2(world, timestep, reset)
# Exercise 9f - Walking
if "9f" in arguments:
exercise_9f(world, timestep, reset)
# Exercise 9g - Transitions
if "9g" in arguments:
exercise_9g(world, timestep, reset)
# Pause
world.simulationSetMode(world.SIMULATION_MODE_PAUSE)
pylog.info("Simulations complete")
world.simulationQuit(0)
def init_supervisor():
global robot_node, supervisor, goal_location
# create the Supervisor instance.
supervisor = Supervisor()
# do this once only
root = supervisor.getRoot()
root_children_field = root.getField("children")
for idx in range(root_children_field.getCount()):
name = root_children_field.getMFNode(idx).getTypeName()
#print('name', name)
if name == 'Mavic2Pro':
robot_node = root_children_field.getMFNode(idx)
if name == 'Solid':
goal_node = root_children_field.getMFNode(idx)
goal_location = goal_node.getField('translation').getSFVec3f()
def __init__(self): # this might be overly complicated :/
#self.robot = Robot()
self.robot = Supervisor()
def setup_motors(robot, motor_names):
return [robot.getMotor(motor_name) for motor_name in motor_names]
(self.m_fl, self.m_fr, self.m_rl, self.m_rr) = setup_motors(
self.robot,
["front_left", "front_right", "rear_left", "rear_right"])
self.leftMotor = 0
self.rightMotor = 0
self.timestep = int(self.robot.getBasicTimeStep())
def __init__(self,
emitter_name="emitter",
receiver_name="receiver",
time_step=None):
super(SupervisorEmitterReceiver, self).__init__()
self.supervisor = Supervisor()
if time_step is None:
self.timestep = int(self.supervisor.getBasicTimeStep())
else:
self.timestep = time_step
self.initialize_comms(emitter_name, receiver_name)
def main() -> None:
quit_if_development_mode()
supervisor = Supervisor()
prepare(supervisor)
# Check after we've paused the sim so that any errors won't be masked by
# subsequent console output from a robot.
check_required_libraries(REPO_ROOT / 'libraries.txt')
remove_unused_robots(supervisor)
with record_animation(supervisor,
REPO_ROOT / 'recordings' / recording_path()):
run_match(supervisor)
def __init__(self):
# Metadata of Robot
jointNames = ["1", "2", "3", "4", "5", "6", "7", "7 left"]
gpsNames = [f'gps{name}' for name in jointNames]
iuNames = [f'iu{name}' for name in jointNames]
posNames = [f'{name} sensor' for name in jointNames]
robotName = 'PROB'
# print('get into prob constructor')
# Robot Node
self.supervisor = Supervisor()
self.robotNode = self.supervisor.getFromDef(f'{robotName}')
# Device Node
self.joints = []
for i in range(len(jointNames)):
self.joints.append(Joint(self.supervisor, jointName=jointNames[i], gpsName=gpsNames[i], iuName=iuNames[i], posName=posNames[i]))
def __init__(self, action_dim, obs_dim):
self.robot = Supervisor()
solid_def_names = self.read_all_def()
self.def_node_field_list = self.get_all_fields(solid_def_names)
self.robot_node = self.robot.getFromDef('Atlas')
self.boom_base = self.robot.getFromDef('BoomBase')
self.boom_base_trans_field = self.boom_base.getField("translation")
self.timeStep = int(self.robot.getBasicTimeStep() * self.ignore_frame) # ms
self.find_and_enable_devices()
high = np.ones([action_dim])
self.action_space = gym.spaces.Box(-high, high, dtype=np.float32)
high = np.inf*np.ones([obs_dim])
self.observation_space = gym.spaces.Box(-high, high, dtype=np.float32)
def init_supervisor():
global supervisor, robot_node, target_node
# create the Supervisor instance.
supervisor = Supervisor()
# do this once only
root = supervisor.getRoot()
root_children_field = root.getField("children")
robot_node = None
target_node = None
for idx in range(root_children_field.getCount()):
if root_children_field.getMFNode(idx).getDef() == "EPUCK":
robot_node = root_children_field.getMFNode(idx)
if root_children_field.getMFNode(idx).getDef() == "EPUCK_TARGET":
target_node = root_children_field.getMFNode(idx)
start_translation = copy.copy(
robot_node.getField("translation").getSFVec3f())
start_rotation = copy.copy(robot_node.getField("rotation").getSFRotation())
def __init__(self, claim_log: ClaimLog) -> None:
self._claim_log = claim_log
self._robot = Supervisor()
self._claim_starts: Dict[Tuple[StationCode, Claimant], float] = {}
self._emitters = {
station_code: get_robot_device(self._robot,
station_code + "Emitter", Emitter)
for station_code in StationCode
}
self._receivers = {
station_code: get_robot_device(self._robot,
station_code + "Receiver", Receiver)
for station_code in StationCode
}
for receiver in self._receivers.values():
receiver.enable(RECEIVE_TICKS)
def __init__(self):
self.supervisor = Supervisor()
self.timeStep = int(4 * self.supervisor.getBasicTimeStep())
# Create the arm chain from the URDF
with tempfile.NamedTemporaryFile(suffix='.urdf', delete=False) as file:
filename = file.name
file.write(self.supervisor.getUrdf().encode('utf-8'))
armChain = Chain.from_urdf_file(filename)
armChain.active_links_mask[0] = False
# Initialize the arm motors and encoders.
self.motors = []
for link in armChain.links:
if 'motor' in link.name:
motor = self.supervisor.getDevice(link.name)
motor.setVelocity(1.0)
position_sensor = motor.getPositionSensor()
position_sensor.enable(self.timeStep)
self.motors.append(motor)
self.arm = self.supervisor.getSelf()
self.positions = [0 for _ in self.motors]
