class Atlas(gym.Env):
"""
Y axis is the vertical axis.
Base class for Webots actors in a Scene.
These environments create single-player scenes and behave like normal Gym environments, if
you don't use multiplayer.
"""
electricity_cost = -2.0 # cost for using motors -- this parameter should be carefully tuned against reward for making progress, other values less improtant
stall_torque_cost = -0.1 # cost for running electric current through a motor even at zero rotational speed, small
foot_collision_cost = -1.0 # touches another leg, or other objects, that cost makes robot avoid smashing feet into itself
joints_at_limit_cost = -0.2 # discourage stuck joints
episode_reward = 0
frame = 0
_max_episode_steps = 1000
initial_y = None
body_xyz = None
joint_angles = None
joint_exceed_limit = False
ignore_frame = 1
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 read_all_def(self, file_name ='../../worlds/atlas_change_foot.wbt'):
no_solid_str_list = ['HingeJoint', 'BallJoint', 'Hinge2Joint', 'Shape', 'Group', 'Physics']
with open(file_name) as f:
content = f.readlines()
# you may also want to remove whitespace characters like `\n` at the end of each line
content = [x.strip() for x in content]
def_str_list = []
for x in content:
if 'DEF' in x:
def_str_list.append(x)
for sub_str in no_solid_str_list:
for i in range(len(def_str_list)):
if sub_str in def_str_list[i]:
def_str_list[i] = 'remove_def'
solid_def_names = []
for def_str in def_str_list:
if 'remove_def' != def_str:
def_str_temp_list = def_str.split()
solid_def_names.append(def_str_temp_list[def_str_temp_list.index('DEF') + 1])
print(solid_def_names)
print('There are duplicates: ',len(solid_def_names) != len(set(solid_def_names)))
return solid_def_names
def get_all_fields(self, solid_def_names):
def_node_field_list = []
for def_name in solid_def_names:
def_node = self.robot.getFromDef(def_name)
node_trans_field = def_node.getField("translation")
node_rot_field = def_node.getField("rotation")
# print(def_name)
node_ini_trans = node_trans_field.getSFVec3f()
node_ini_rot = node_rot_field.getSFRotation()
def_node_field_list.append({'def_node': def_node,
'node_trans_field': node_trans_field,
'node_rot_field': node_rot_field,
'node_ini_trans': node_ini_trans,
'node_ini_rot': node_ini_rot})
return def_node_field_list
def reset_all_fields(self):
for def_node_field in self.def_node_field_list:
def_node_field['node_trans_field'].setSFVec3f(def_node_field['node_ini_trans'])
def_node_field['node_rot_field'].setSFRotation(def_node_field['node_ini_rot'])
def find_and_enable_devices(self):
# inertial unit
self.inertial_unit = self.robot.getInertialUnit("inertial unit")
self.inertial_unit.enable(self.timeStep)
# gps
self.gps = self.robot.getGPS("gps")
self.gps.enable(self.timeStep)
# foot sensors
self.fsr = [self.robot.getTouchSensor("RFsr"), self.robot.getTouchSensor("LFsr")]
for i in range(len(self.fsr)):
self.fsr[i].enable(self.timeStep)
# all motors
# motor_names = [# 'HeadPitch', 'HeadYaw',
# 'LLegUay', 'LLegLax', 'LLegKny',
# 'LLegLhy', 'LLegMhx', 'LLegUhz',
# 'RLegUay', 'RLegLax', 'RLegKny',
# 'RLegLhy', 'RLegMhx', 'RLegUhz',
# ]
motor_names = [ # 'HeadPitch', 'HeadYaw',
'BackLbz', 'BackMby', 'BackUbx', 'NeckAy',
'LLegLax', 'LLegMhx', 'LLegUhz',
'RLegLax', 'RLegMhx', 'RLegUhz',
]
self.motors = []
for i in range(len(motor_names)):
self.motors.append(self.robot.getMotor(motor_names[i]))
# leg pitch motors
self.legPitchMotor = [self.robot.getMotor('RLegLhy'),
self.robot.getMotor('RLegKny'),
self.robot.getMotor('RLegUay'),
self.robot.getMotor('LLegLhy'),
self.robot.getMotor('LLegKny'),
self.robot.getMotor('LLegUay')]
for i in range(len(self.legPitchMotor)):
self.legPitchMotor[i].enableTorqueFeedback(self.timeStep)
# leg pitch sensors
self.legPitchSensor =[self.robot.getPositionSensor('RLegLhyS'),
self.robot.getPositionSensor('RLegKnyS'),
self.robot.getPositionSensor('RLegUayS'),
self.robot.getPositionSensor('LLegLhyS'),
self.robot.getPositionSensor('LLegKnyS'),
self.robot.getPositionSensor('LLegUayS')]
for i in range(len(self.legPitchSensor)):
self.legPitchSensor[i].enable(self.timeStep)
def apply_action(self, a):
assert (np.isfinite(a).all())
for n, j in enumerate(self.legPitchMotor):
max_joint_angle = j.getMaxPosition()
min_joint_angle = j.getMinPosition()
mean_angle = 0.5 * (max_joint_angle + min_joint_angle)
half_range_angle = 0.5 * (max_joint_angle - min_joint_angle)
j.setPosition(mean_angle + half_range_angle * float(np.clip(a[n], -1, +1)))
# joint_angle = self.read_joint_angle(joint_idx=n)
# torque = 0.5 * j.getMaxTorque() * float(np.clip(a[n], -1, +1))
# if joint_angle > max_joint_angle:
# j.setPosition(max_joint_angle - 0.1)
# # j.setTorque(-1.0 * abs(torque))
# elif joint_angle < min_joint_angle:
# j.setPosition(min_joint_angle + 0.1)
# # j.setTorque(abs(torque))
# else:
# j.setTorque(torque)
def read_joint_angle(self, joint_idx):
joint_angle = self.legPitchSensor[joint_idx].getValue() % (2.0 * np.pi)
if joint_angle > np.pi:
joint_angle -= 2.0 * np.pi
max_joint_angle = self.legPitchMotor[joint_idx].getMaxPosition()
min_joint_angle = self.legPitchMotor[joint_idx].getMinPosition()
if joint_angle > max_joint_angle + 0.05 or joint_angle < min_joint_angle - 0.05:
self.joint_exceed_limit = True
return joint_angle
def calc_state(self):
joint_states = np.zeros(2*len(self.legPitchMotor))
# even elements [0::2] position, scaled to -1..+1 between limits
for r in range(6):
joint_angle = self.read_joint_angle(joint_idx=r)
if r in [0, 3]:
joint_states[2 * r] = (-joint_angle - np.deg2rad(35)) / np.deg2rad(80)
elif r in [1, 4]:
joint_states[2 * r] = 1 - joint_angle / np.deg2rad(75)
elif r in [2, 5]:
joint_states[2 * r] = -joint_angle / np.deg2rad(45)
# odd elements [1::2] angular speed, scaled to show -1..+1
for r in range(6):
if self.joint_angles is None:
joint_states[2 * r + 1] = 0.0
else:
joint_states[2 * r + 1] = 0.5 * (joint_states[2*r] - self.joint_angles[r])
self.joint_angles = np.copy(joint_states[0::2])
self.joint_speeds = joint_states[1::2]
self.joints_at_limit = np.count_nonzero(np.abs(joint_states[0::2]) > 0.99)
self.joint_torques = np.zeros(len(self.legPitchMotor))
for i in range(len(self.legPitchMotor)):
self.joint_torques[i] = self.legPitchMotor[i].getTorqueFeedback() \
/ self.legPitchMotor[i].getAvailableTorque()
if self.body_xyz is None:
self.body_xyz = np.asarray(self.gps.getValues())
self.body_speed = np.zeros(3)
else:
self.body_speed = (np.asarray(self.gps.getValues()) - self.body_xyz) / (self.timeStep * 1e-3)
self.body_xyz = np.asarray(self.gps.getValues())
body_local_speed = np.copy(self.body_speed)
body_local_speed[0], body_local_speed[2] = self.calc_local_speed()
# print('speed: ', np.linalg.norm(self.body_speed))
y = self.body_xyz[1]
if self.initial_y is None:
self.initial_y = y
self.body_rpy = self.inertial_unit.getRollPitchYaw()
'''
The roll angle indicates the unit's rotation angle about its x-axis,
in the interval [-π,π]. The roll angle is zero when the InertialUnit is horizontal,
i.e., when its y-axis has the opposite direction of the gravity (WorldInfo defines
the gravity vector).
The pitch angle indicates the unit's rotation angle about is z-axis,
in the interval [-π/2,π/2]. The pitch angle is zero when the InertialUnit is horizontal,
i.e., when its y-axis has the opposite direction of the gravity.
If the InertialUnit is placed on the Robot with a standard orientation,
then the pitch angle is negative when the Robot is going down,
and positive when the robot is going up.
The yaw angle indicates the unit orientation, in the interval [-π,π],
with respect to WorldInfo.northDirection.
The yaw angle is zero when the InertialUnit's x-axis is aligned with the north direction,
it is π/2 when the unit is heading east, and -π/2 when the unit is oriented towards the west.
The yaw angle can be used as a compass.
'''
more = np.array([
y - self.initial_y,
0, 0,
0.3 * body_local_speed[0], 0.3 * body_local_speed[1], 0.3 * body_local_speed[2],
# 0.3 is just scaling typical speed into -1..+1, no physical sense here
self.body_rpy[0] / np.pi, self.body_rpy[1] / np.pi], dtype=np.float32)
self.feet_contact = np.zeros(2)
for j in range(len(self.fsr)):
self.feet_contact[j] = self.fsr[j].getValue()
return np.clip(np.concatenate([more] + [joint_states] + [self.feet_contact]), -5, +5)
def calc_local_speed(self):
'''
# progress in potential field is speed*dt, typical speed is about 2-3 meter per second,
this potential will change 2-3 per frame (not per second),
# all rewards have rew/frame units and close to 1.0
'''
direction_r = self.body_xyz - np.asarray(self.boom_base_trans_field.getSFVec3f())
# print('robot_xyz: {}, boom_base_xyz: {}'.format(self.body_xyz,
# np.asarray(self.boom_base_trans_field.getSFVec3f())))
direction_r = direction_r[[0, 2]] / np.linalg.norm(direction_r[[0, 2]])
direction_t = np.dot(np.asarray([[0, 1],
[-1, 0]]), direction_r.reshape((-1, 1)))
return np.dot(self.body_speed[[0, 2]], direction_t), np.dot(self.body_speed[[0, 2]], direction_r)
def alive_bonus(self, y, pitch):
return +1 if abs(y) > 0.5 and abs(pitch) < 1.0 else -1
def render(self, mode='human'):
file_name = 'img.jpg'
self.robot.exportImage(file=file_name, quality=100)
return cv2.imread(file_name, -1)
def step(self, action):
for i in range(self.ignore_frame):
self.apply_action(action)
simulation_state = self.robot.step(self.timeStep)
state = self.calc_state() # also calculates self.joints_at_limit
# state[0] is body height above ground, body_rpy[1] is pitch
alive = float(self.alive_bonus(state[0] + self.initial_y,
self.body_rpy[1]))
progress, _ = self.calc_local_speed()
# print('progress: {}'.format(progress))
feet_collision_cost = 0.0
'''
let's assume we have DC motor with controller, and reverse current braking
'''
electricity_cost = self.electricity_cost * float(np.abs(
self.joint_torques * self.joint_speeds).mean())
electricity_cost += self.stall_torque_cost * float(np.square(self.joint_torques).mean())
joints_at_limit_cost = float(self.joints_at_limit_cost * self.joints_at_limit)
rewards = [
alive,
progress,
electricity_cost,
joints_at_limit_cost,
feet_collision_cost
]
self.episode_reward += progress
self.frame += 1
done = (-1 == simulation_state) or (self._max_episode_steps <= self.frame) \
or (alive < 0) or (not np.isfinite(state).all())
# print('frame: {}, alive: {}, done: {}, body_xyz: {}'.format(self.frame, alive, done, self.body_xyz))
# print('state_{} \n action_{}, reward_{}'.format(state, action, sum(rewards)))
return state, sum(rewards), done, {}
def run(self):
# Main loop.
for i in range(self._max_episode_steps):
action = np.random.uniform(-1, 1, 6)
state, reward, done, _ = self.step(action)
# print('state_{} \n action_{}, reward_{}'.format(state, action, reward))
if done:
break
def reset(self, is_eval_only = False):
self.initial_y = None
self.body_xyz = None
self.joint_angles = None
self.frame = 0
self.episode_reward = 0
self.joint_exceed_limit = False
for i in range(100):
for j in self.motors:
j.setPosition(0)
for k in range(len(self.legPitchMotor)):
j = self.legPitchMotor[k]
j.setPosition(0)
self.robot.step(self.timeStep)
self.robot.simulationResetPhysics()
self.reset_all_fields()
for i in range(10):
self.robot_node.moveViewpoint()
self.robot.step(self.timeStep)
if is_eval_only:
time.sleep(1)
return self.calc_state()
class DarwinController:
def __init__(self, namespace='', node=True):
self.time = 0
self.clock_msg = Clock()
self.namespace = namespace
self.supervisor = Supervisor()
self.motor_names = [
"ShoulderR", "ShoulderL", "ArmUpperR", "ArmUpperL", "ArmLowerR",
"ArmLowerL", "PelvYR", "PelvYL", "PelvR", "PelvL", "LegUpperR",
"LegUpperL", "LegLowerR", "LegLowerL", "AnkleR", "AnkleL", "FootR",
"FootL", "Neck", "Head"
]
self.walkready = [0] * 20
self.names_webots_to_bitbots = {
"ShoulderR": "RShoulderPitch",
"ShoulderL": "LShoulderPitch",
"ArmUpperR": "RShoulderRoll",
"ArmUpperL": "LShoulderRoll",
"ArmLowerR": "RElbow",
"ArmLowerL": "LElbow",
"PelvYR": "RHipYaw",
"PelvYL": "LHipYaw",
"PelvR": "RHipRoll",
"PelvL": "LHipRoll",
"LegUpperR": "RHipPitch",
"LegUpperL": "LHipPitch",
"LegLowerR": "RKnee",
"LegLowerL": "LKnee",
"AnkleR": "RAnklePitch",
"AnkleL": "LAnklePitch",
"FootR": "RAnkleRoll",
"FootL": "LAnkleRoll",
"Neck": "HeadPan",
"Head": "HeadTilt"
}
self.names_bitbots_to_webots = {
"RShoulderPitch": "ShoulderR",
"LShoulderPitch": "ShoulderL",
"RShoulderRoll": "ArmUpperR",
"LShoulderRoll": "ArmUpperL",
"RElbow": "ArmLowerR",
"LElbow": "ArmLowerL",
"RHipYaw": "PelvYR",
"LHipYaw": "PelvYL",
"RHipRoll": "PelvR",
"LHipRoll": "PelvL",
"RHipPitch": "LegUpperR",
"LHipPitch": "LegUpperL",
"RKnee": "LegLowerR",
"LKnee": "LegLowerL",
"RAnklePitch": "AnkleR",
"LAnklePitch": "AnkleL",
"RAnkleRoll": "FootR",
"LAnkleRoll": "FootL",
"HeadPan": "Neck",
"HeadTilt": "Head"
}
self.motors = []
self.sensors = []
self.timestep = int(self.supervisor.getBasicTimeStep())
self.timestep = 10
for motor_name in self.motor_names:
self.motors.append(self.supervisor.getMotor(motor_name))
self.motors[-1].enableTorqueFeedback(self.timestep)
self.sensors.append(
self.supervisor.getPositionSensor(motor_name + "S"))
self.sensors[-1].enable(self.timestep)
self.accel = self.supervisor.getAccelerometer("Accelerometer")
self.accel.enable(self.timestep)
self.gyro = self.supervisor.getGyro("Gyro")
self.gyro.enable(self.timestep)
self.camera = self.supervisor.getCamera("Camera")
self.camera.enable(self.timestep)
if node:
rospy.init_node("webots_darwin_ros_interface",
anonymous=True,
argv=['clock:=/' + self.namespace + '/clock'])
self.pub_js = rospy.Publisher(self.namespace + "/joint_states",
JointState,
queue_size=1)
self.pub_imu = rospy.Publisher(self.namespace + "/imu/data",
Imu,
queue_size=1)
self.pub_cam = rospy.Publisher(self.namespace + "/image_raw",
Image,
queue_size=1)
self.clock_publisher = rospy.Publisher(self.namespace + "/clock",
Clock,
queue_size=1)
rospy.Subscriber(self.namespace + "/DynamixelController/command",
JointCommand, self.command_cb)
self.world_info = self.supervisor.getFromDef("world_info")
self.hinge_joint = self.supervisor.getFromDef("barrier_hinge")
self.robot_node = self.supervisor.getFromDef("Darwin")
self.translation_field = self.robot_node.getField("translation")
self.rotation_field = self.robot_node.getField("rotation")
def step_sim(self):
self.supervisor.step(self.timestep)
def step(self):
self.step_sim()
self.time += self.timestep / 1000
self.publish_imu()
self.publish_joint_states()
self.clock_msg.clock = rospy.Time.from_seconds(self.time)
self.clock_publisher.publish(self.clock_msg)
def command_cb(self, command: JointCommand):
for i, name in enumerate(command.joint_names):
try:
motor_index = self.motor_names.index(
self.names_bitbots_to_webots[name])
self.motors[motor_index].setPosition(command.positions[i])
except ValueError:
print(
f"invalid motor specified ({self.names_bitbots_to_webots[name]})"
)
self.publish_joint_states()
self.publish_imu()
self.publish_camera()
def publish_joint_states(self):
js = JointState()
js.name = []
js.header.stamp = rospy.get_rostime()
js.position = []
js.effort = []
for i in range(len(self.sensors)):
js.name.append(self.names_webots_to_bitbots[self.motor_names[i]])
value = self.sensors[i].getValue()
js.position.append(value)
js.effort.append(self.motors[i].getTorqueFeedback())
self.pub_js.publish(js)
def publish_imu(self):
msg = Imu()
msg.header.stamp = rospy.get_rostime()
msg.header.frame_id = "imu_frame"
accel_vels = self.accel.getValues()
msg.linear_acceleration.x = ((accel_vels[0] - 512.0) / 512.0) * 3 * G
msg.linear_acceleration.y = ((accel_vels[1] - 512.0) / 512.0) * 3 * G
msg.linear_acceleration.z = ((accel_vels[2] - 512.0) / 512.0) * 3 * G
gyro_vels = self.gyro.getValues()
msg.angular_velocity.x = ((gyro_vels[0] - 512.0) / 512.0) * 1600 * (
math.pi / 180) # is 400 deg/s the real value
msg.angular_velocity.y = (
(gyro_vels[1] - 512.0) / 512.0) * 1600 * (math.pi / 180)
msg.angular_velocity.z = (
(gyro_vels[2] - 512.0) / 512.0) * 1600 * (math.pi / 180)
self.pub_imu.publish(msg)
def publish_camera(self):
img_msg = Image()
img_msg.header.stamp = rospy.get_rostime()
img_msg.height = self.camera.getHeight()
img_msg.width = self.camera.getWidth()
img_msg.encoding = "bgra8"
img_msg.step = 4 * self.camera.getWidth()
img = self.camera.getImage()
img_msg.data = img
self.pub_cam.publish(img_msg)
def set_gravity(self, active):
if active:
self.world_info.getField("gravity").setSFVec3f([0.0, -9.81, 0.0])
else:
self.world_info.getField("gravity").setSFVec3f([0.0, 0.0, 0.0])
def reset_robot_pose(self, pos, quat):
rpy = tf.transformations.euler_from_quaternion(quat)
self.set_robot_pose_rpy(pos, rpy)
self.robot_node.resetPhysics()
def reset_robot_pose_rpy(self, pos, rpy):
self.set_robot_pose_rpy(pos, rpy)
self.robot_node.resetPhysics()
def reset(self):
self.supervisor.simulationReset()
def node(self):
s = self.supervisor.getSelected()
if s is not None:
print(f"id: {s.getId()}, type: {s.getType()}, def: {s.getDef()}")
def set_robot_pose_rpy(self, pos, rpy):
self.translation_field.setSFVec3f(pos_ros_to_webots(pos))
self.rotation_field.setSFRotation(rpy_to_axis(*rpy))
def get_robot_pose_rpy(self):
pos = self.translation_field.getSFVec3f()
rot = self.rotation_field.getSFRotation()
return pos_webots_to_ros(pos), axis_to_rpy(*rot)
"""Sample Webots controller for the inverted pendulum benchmark."""
from controller import Supervisor
import os
import math
# Get pointer to the robot.
robot = Supervisor()
# Get the time step of the current world.
timestep = int(robot.getBasicTimeStep())
# Get pointers to the position sensor and enable it.
ps = robot.getPositionSensor('pendulum sensor')
ps.enable(timestep)
# Get pointers to the motors and set target position to infinity (speed control).
leftMotor = robot.getMotor("left wheel motor")
rightMotor = robot.getMotor("right wheel motor")
leftMotor.setPosition(float('+inf'))
rightMotor.setPosition(float('+inf'))
leftMotor.setVelocity(0.0)
rightMotor.setVelocity(0.0)
maxSpeed = min(rightMotor.getMaxVelocity(), leftMotor.getMaxVelocity())
# Define the PID control constants and variables.
KP = float(os.environ.get('P_GAIN', '31.4'))
KI = float(os.environ.get('I_GAIN', '100.5'))
KD = float(os.environ.get('D_GAIN', '0'))
integral = 0.0
previous_position = 0.0
INIT_ANGLE = 0
PRED_STEPS = 450
correction_x = 0
correction_y = 0
correction_theta = 0
# create the Robot instance.
robot = Supervisor()
robot_sup = robot.getFromDef("e-puck")
robot_trans = robot_sup.getField("translation")
compass = robot.getCompass("compass")
motorLeft = robot.getMotor("left wheel motor")
motorRight = robot.getMotor("right wheel motor")
positionLeft = robot.getPositionSensor("left wheel sensor")
positionRight = robot.getPositionSensor("right wheel sensor")
predictor = Predictor()
timestep = int(robot.getBasicTimeStep())
x = []
y = []
theta = []
distance_sensors_info = []
x_odometry = []
y_odometry = []
theta_odometry = []
sensorNames = ['ds0', 'ds1', 'ds2', 'ds3', 'ds4', 'ds5', 'ds6', 'ds7']
"front_right_1",
"front_left_2",
"front_right_2",
"front_left_3",
"front_right_3",
"back_left_1",
"back_right_1",
"back_left_2",
"back_right_2",
"back_left_3",
"back_right_3",
"head",
]
for m in moduleNames:
supervisor.getPositionSensor(m + "_sensor").enable(timestep)
def act(motorName, action):
shift = action
# phase shift for this module
if (type(shift) == np.ndarray):
shift = shift[0]
phase = 2.0 * pi * F * time
factor = 1.0
if factor * shift > 1.5:
shift = 1.5 * factor
if factor * shift < -1.5:
shift = -1.5 * factor
#robot.getMotor("motor").setPosition(factor*shift)
class RobotManager:
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 init_actuators(self):
self.compass.enable(self.timestep)
self.motorLeft.setPosition(float('inf'))
self.motorRight.setPosition(float('inf'))
self.positionRight.enable(self.timestep)
self.positionLeft.enable(self.timestep)
def robot_to_xy(self, x, y):
return x + 1, y + 0.75
def xy_to_robot(self, x, y):
return x - 1.00000, y - 0.750000
def init_robot_pos(self, x, y):
x, y = self.xy_to_robot(x, y)
self.robot_trans.setSFVec3f([y, 0, x])
self.robot_rotation.setSFRotation([0, 1, 0, 0])
self.robot_sup.resetPhysics()
def get_bearing_degrees(self):
north = self.compass.getValues()
rad = np.arctan2(north[0], north[2])
bearing = (rad) / np.pi * 180
if bearing < 0.0:
bearing += 360
bearing = 360 - bearing - 90
if bearing < 0.0:
bearing += 360
return bearing
def save_supervisor_coordinates(self):
# true robot position information
trans_info = self.robot_trans.getSFVec3f()
# print('SUP COORD:', trans_info)
x_coordinate, y_coordinate = self.robot_to_xy(trans_info[2],
trans_info[0])
self.x.append(x_coordinate)
self.y.append(y_coordinate)
angle = self.get_bearing_degrees()
self.theta.append(angle)
def step(self):
return self.robot.step(self.timestep) != -1
def save_odometry_coordinates(self, coordinate):
# convert robot coordinates into global coordinate system
self.x_odometry.append(coordinate.x)
self.y_odometry.append(coordinate.y)
self.theta_odometry.append(
self.convert_angle_to_xy_coordinates(coordinate.theta))
def save_sensor_distances(self, distanceSensors):
distances = []
for distanceSensor in distanceSensors:
distance = distanceSensor.getValue()
#there is no real messure.
if distance == 10:
distance = None
distances.append(distance)
self.distance_sensors_info.append(distances)
def get_sensor_distance(self):
# Read the sensors, like:
distanceSensors = []
for sensorName in self.sensorNames:
sensor = self.robot.getDistanceSensor(sensorName)
sensor.enable(self.timestep)
distanceSensors.append(sensor)
return distanceSensors
def convert_angle_to_xy_coordinates(self, angle):
angle = angle * 180 / np.pi
if angle < 0.0:
angle = 360 + angle
return angle
def plot(self):
# Enter here exit cleanup code.
plt.ylim([0, self.params.MAX_Y])
plt.xlim([0, self.params.MAX_X])
plt.xlabel("x")
plt.ylabel("y")
plt.plot(self.x, self.y, label="real")
plt.plot(self.x_odometry, self.y_odometry, label="odometry")
plt.plot(self.x_pred, self.y_pred, 's', label="correction", marker='o')
plt.title("Robot position estimation")
plt.legend()
plt.savefig("results/position.eps", format='eps')
def move_robot_to_random_position(self):
new_x = -(1 / 2 * self.params.MAX_X -
0.1) + np.random.random() * (self.params.MAX_X - 0.2)
new_y = -(1 / 2 * self.params.MAX_Y -
0.1) + np.random.random() * (self.params.MAX_Y - 0.2)
# old_z = robot_trans.getSFVec3f()[1]
new_theta = np.random.random() * 2 * np.pi
self.robot_trans.setSFVec3f([new_y, 0, new_x])
self.robot_rotation.setSFRotation([0, 1, 0, new_theta])
self.robot_sup.resetPhysics()
def execute(self):
self.init_actuators()
self.init_robot_pos(self.params.INIT_X, self.params.INIT_Y)
self.step()
self.init_robot_pos(self.params.INIT_X, self.params.INIT_Y)
self.window_communicator.sendInitialParams(self.params)
odometry = Odometry(
self.params.ENCODER_UNIT * (self.positionLeft.getValue()),
self.params.ENCODER_UNIT * (self.positionRight.getValue()),
self.params.INIT_X, self.params.INIT_Y, self.params.INIT_ANGLE)
count = 0
particles = np.array([[], []])
last_move = 'none'
errorPos = []
errorOdo = []
continue_running = True
# principal robot step cycle
while (continue_running):
# if the number of steps is greater than EXPERIMENT_DURATION_STEPS then stop running
if count == self.params.EXPERIMENT_DURATION_STEPS:
continue_running = False
# receive message from robot window
message = self.window_communicator.receiveMessage()
message = json.loads(message) if message else None
if message and message['code'] == 'start_randomness':
self.movement_random = True
elif message and message['code'] == 'stop_randomness':
self.movement_random = False
elif message and message['code'] == 'params_modification':
self.particles_filter.reset_particles(
message["number_particles"], message["sigma_xy"],
message["sigma_theta"],
RobotConfiguration(self.x_pred[-1], self.y_pred[-1],
self.theta_pred[-1]))
# get odometry data
odometry_info, delta_movement = odometry.track_step(
self.params.ENCODER_UNIT * (self.positionLeft.getValue()),
self.params.ENCODER_UNIT * (self.positionRight.getValue()))
# transoform the angle to xy coordinates
delta_movement[2] = self.convert_angle_to_xy_coordinates(
delta_movement[2])
# for capturing robot positioning
if not self.step() and self.params.CAPTURING_DATA:
print('saving data')
self.data_collector.collect(
self.x, self.y, self.theta,
np.array(self.distance_sensors_info))
self.plot()
# get sensor distances
distanceSensors = self.get_sensor_distance()
# collect data: real, odometry, predicted
self.save_sensor_distances(distanceSensors)
self.save_odometry_coordinates(odometry_info)
self.save_supervisor_coordinates()
# calculate new velocity
left_speed, right_speed = 0, 0
if self.movement_random:
if self.params.GO_STRAIGHT_MOVE:
left_speed, right_speed = self.movement_controller.calculate_velocity(
distanceSensors)
else:
left_speed, right_speed = self.movement_controller.calculate_velocity_random_move(
distanceSensors)
last_move = 'none'
else:
# joystick control
if message and message[
'code'] == "move" and last_move != message['direction']:
last_move = message['direction']
if last_move == 'UP':
left_speed, right_speed = self.movement_controller.move_straight(
)
elif last_move == 'DOWN':
left_speed, right_speed = self.movement_controller.move_backwards(
)
elif last_move == 'RIGHT':
left_speed, right_speed = self.movement_controller.move_right(
)
elif last_move == 'LEFT':
left_speed, right_speed = self.movement_controller.move_left(
)
self.motorLeft.setVelocity(left_speed)
self.motorRight.setVelocity(right_speed)
# predict position based on particles data
if not self.params.CAPTURING_DATA and count % self.params.PRED_STEPS == 0 and count != 0:
# get particles
particles = self.particles_filter.get_particles(
delta_movement, self.distance_sensors_info[-1],
count % 2 == 0)
# get weights sum
weighted_sum = np.sum(particles[3])
# get weighted average from particles data
x_prim = np.sum(particles[0] * particles[3]) / weighted_sum
y_prim = np.sum(particles[1] * particles[3]) / weighted_sum
theta_prim = np.sum(particles[2] * particles[3]) / weighted_sum
# # get the position prediction given the sensor measurements
# predicted_coord = predictorCoord.predict(distance_sensors_info[-1])
#
# print(predicted_coord)
# # combine both previous models
# x_pred.append((x_prim + float(predicted_coord[0]))/2)
# y_pred.append((y_prim + float(predicted_coord[1]))/2)
self.x_pred.append(x_prim)
self.y_pred.append(y_prim)
self.theta_pred.append(theta_prim)
# predictor.refit_models(x[-1], y[-1], theta[-1], distance_sensors_info[-1])
# calculate error
if self.params.CALCULATE_PRED_ERROR:
errorPos.append(
np.sqrt((self.x[-1] - self.x_pred[-1])**2 +
(self.y[-1] - self.y_pred[-1])**2))
if self.params.CALCULATE_ODO_ERROR:
errorOdo.append(
np.sqrt((self.x[-1] - self.x_odometry[-1])**2 +
(self.y[-1] - self.y_odometry[-1])**2))
# send data to html page
self.window_communicator.sendCoordinatesParticles(
self.x, self.y, self.x_odometry, self.y_odometry, self.x_pred,
self.y_pred, particles.tolist())
# move robot to a random position after a while
if self.params.CAPTURING_DATA and count % self.params.MOVING_ROBOT_STEPS == 0:
self.move_robot_to_random_position()
count += 1
if self.params.CALCULATE_PRED_ERROR:
print('Saving prediction SDE')
pickle.dump(errorPos, open(self.params.PRED_ERROR_FILE, "wb"))
if self.params.CALCULATE_ODO_ERROR:
print('Saving odometry SDE')
pickle.dump(errorOdo, open(self.params.ODO_ERROR_FILE, "wb"))
