class Paul(object):
def __init__(self):
# Name of the controller
self.name = "PAUL controller for Webots"
# Current Speed of the robot (Used for exception handling recovery)
self.speed = 0
self.up_speed = 5
self.down_speed = -5
# May want to consider making both of these final variables
self.top_threshold = 1000
self.bottom_threshold = 1000
# create the Robot instance.
self.robot = Robot()
# get the time step of the current world.
self.timestep = int(self.robot.getBasicTimeStep())
# initialise the 3 wheels
self.wheels = []
wheelsNames = ['wheel1', 'wheel2', 'wheel3']
for i in range(3):
self.wheels.append(self.robot.getDevice(wheelsNames[i]))
self.wheels[i].setPosition(float('inf'))
self.wheels[i].setVelocity(0.0) # unit: rad/s
# initialise distance sensors
self.top_ds = self.robot.getDevice('ds_top')
self.bottom_ds = self.robot.getDevice('ds_bottom')
self.top_ds.enable(self.timestep)
self.bottom_ds.enable(self.timestep)
# initialise LED
self.uv_led = self.robot.getDevice('uv_led')
# In the case that at init both distance sensors were already within a threshold, don't start up
self.completed_init = True
if self.check_top_ds_reading() and self.check_bottom_ds_reading():
self.completed_init = False
# Movement Functions
# Function to start the robot moving at a speed defined in the speed parameter
def start_motors(self, speed):
print("Starting motors...")
self.speed = speed
for i in range(3):
self.wheels[i].setVelocity(speed)
# Function to stop the robot moving
def stop_motors(self):
print("Stopping motors...")
self.speed = 0
for i in range(3):
self.wheels[i].setVelocity(0)
# Motors and Sensors Readings
# Function to display all readings from the motors and sensors
def display_all_readings(self):
self.display_motor_readings()
self.display_top_ds_reading()
self.display_bottom_ds_reading()
# Function to display any readings from the motors
def display_motor_readings(self):
print("Motor speed: " + str(self.speed))
# Function get the current speed of the robot
# TODO: Potentially remove, equivalent to getSpeed?
def get_motor_readings(self):
return self.speed
# Function to check if the top distance sensor reading is within the threshold to trigger a change in direction
# Returns True if the distance should change
# Returns False if the distance is not within the threshold
def check_top_ds_reading(self):
return self.get_top_ds_reading() < self.get_top_threshold()
# Function to display any readings from the top distance sensor
def display_top_ds_reading(self):
print("Top DS Reading: " + str(self.get_top_ds_reading()))
# Function to display get readings from the top distance sensor
def get_top_ds_reading(self):
return self.top_ds.getValue()
# Function to check if the top distance sensor reading is within the threshold to trigger a change in direction
# Returns True if the distance should change
# Returns False if the distance is not within the threshold
def check_bottom_ds_reading(self):
return self.get_bottom_ds_reading() < self.get_bottom_threshold()
# Function to display any readings from the bottom distance sensor
def display_bottom_ds_reading(self):
print("Bottom DS Reading: " + str(self.get_bottom_ds_reading()))
# Function to display get readings from the bottom distance sensor
def get_bottom_ds_reading(self):
return self.bottom_ds.getValue()
def turn_uv_on(self):
self.uv_led.set(255)
def turn_uv_off(self):
self.uv_led.set(0)
def wait_for_time(self, time):
self.robot.waitForUserInputEvent(self.robot.EVENT_NO_EVENT, time)
# Getters and Setters
def get_name(self):
return self.name
def set_name(self, name):
self.name = name
def get_speed(self):
return self.speed
def set_speed(self, speed):
self.speed = speed
def get_up_speed(self):
return self.up_speed
def set_up_speed(self, up_speed):
self.up_speed = up_speed
def get_down_speed(self):
return self.down_speed
def set_down_speed(self, down_speed):
self.down_speed = down_speed
def get_top_threshold(self):
return self.top_threshold
def set_top_threshold(self, top_threshold):
self.top_threshold = top_threshold
def get_bottom_threshold(self):
return self.bottom_threshold
def set_bottom_threshold(self, bottom_threshold):
self.bottom_threshold = bottom_threshold
from controller import Camera
from controller import CameraRecognitionObject
import math
#Function definitions
def clamp(n, smallest, largest):
return max(smallest, min(n, largest))
# create the Robot instance.
agro_drone = Robot()
# get the time step of the current world.
timestep = int(agro_drone.getBasicTimeStep())
camera = agro_drone.getCamera("camera")
# Initialise devices
imu = agro_drone.getInertialUnit('inertial unit')
imu.enable(timestep)
gps = agro_drone.getGPS('gps')
gps.enable(timestep)
gyro = agro_drone.getGyro('gyro')
gyro.enable(timestep)
front_left_motor = agro_drone.getMotor("front left propeller")
front_right_motor = agro_drone.getMotor("front right propeller")
class RobotEmitterReceiver(ABC):
"""
This RobotEmitterReceiver implements only the most basic run method, that
steps the robot and calls the handleEmitter, handleReceiver methods that
are needed for communication with the supervisor.
This class must be inherited by all robot controllers created by the user
and the handleEmitter, handleReceiver, initialize_comms methods are all
abstract and need to be implemented, according to their docstrings. For a
simpler RobotController that implements the methods in a basic form
inherit the RobotEmitterReceiver class.
"""
def __init__(self,
emitter_name="emitter",
receiver_name="receiver",
timestep=None):
"""
The basic robot constructor.
Initializes the Webots Robot and sets up a basic timestep if None is
supplied.
Also initializes the emitter and the receiver used to communicate with
the supervisor, using the initialize_comms() method which must be
implemented by the user. The two methods handle_emitter() and
handle_receiver() must also be implemented by the user.
For the step argument see relevant Webots documentation:
https://cyberbotics.com/doc/guide/controller-programming#the-step-and-wb_robot_step-functions
:param timestep: float, positive or None
"""
self.robot = Robot()
if timestep is None:
self.timestep = int(self.robot.getBasicTimeStep())
else:
self.timestep = timestep
self.emitter, self.receiver = self.initialize_comms(
emitter_name, receiver_name)
def get_timestep(self):
return self.timestep
@abstractmethod
def initialize_comms(self, emitter_name, receiver_name):
"""
This method should initialize and the return emitter and receiver in a
tuple as expected by the constructor.
A basic example implementation can be:
emitter = self.robot.getEmitter("emitter")
receiver = self.robot.getReceiver("receiver")
receiver.enable(self.timestep)
return emitter, receiver
:return: (emitter, receiver) tuple, as initialized
"""
pass
@abstractmethod
def handle_emitter(self):
"""
This method should take data from the robot, eg. sensor data, parse
it into a message and use the robot's emitter to send it to the
supervisor. This message will be used as the observation of the robot.
"""
pass
@abstractmethod
def handle_receiver(self):
"""
This method should take data through the receiver in the form of a
message and parse into data usable by the robot.
For example the message might include a motor speed, which should be
parsed and applied to the robot's motor.
"""
pass
def run(self):
"""
This method is required by Webots to update the robot in the
simulation. It steps the robot and in each step it runs the two
handler methods to use the emitter and receiver components.
This method should be called by a robot manager to run the robot.
"""
while self.robot.step(self.timestep) != -1:
self.handle_receiver()
self.handle_emitter()
import math
import threading
from tornado import httpserver
from tornado import gen
from tornado.ioloop import IOLoop
import tornado.web
#from controller import Robot, Motor, DistanceSensor
from controller import Robot, Motor, DistanceSensor, InertialUnit
from controller import LED
# create the Robot instance.
robot = Robot()
lock = threading.Lock()
#TIME_STEP = 64
TIME_STEP = int(robot.getBasicTimeStep())
MAX_SPEED = float(6)
WHEEL_DIAMETER = 0.096
axle_length = 0.33
radius_wheel = 0.048
DEFAULT_SPEED = 6.28
DRIVE_FACTOR = 12.2 # positions to meters
TURN_FACTOR = 30
port = robot.getCustomData()
robot_name = robot.getName()
print("Port: ", port)
# init inertialunit
#inertialUnit = robot.getDevice('inertial unit')
#inertialUnit.enable(TIME_STEP)
TRANSLATE_FACTOR = 0.096
# ratio between the value reported by the wheel sensor and the actual
# angle traveled by the robot while turning, in radian.
ROTATE_FACTOR = 0.566
# maximum speed for the velocity value of the wheels.
MAX_SPEED = 5.24
# minimum speed used while slowing down and trying to stop at a precise
# location.
MIN_SPEED = 0.1
robot = Robot()
timestep = int(robot.getBasicTimeStep())
leftWheel = robot.getDevice('left wheel')
rightWheel = robot.getDevice('right wheel')
leftWheel.setVelocity(0)
rightWheel.setVelocity(0)
leftWheel.setPosition(float('inf'))
rightWheel.setPosition(float('inf'))
leftWheelSensor = robot.getDevice('left wheel sensor')
leftWheelSensor.enable(timestep)
rightWheelSensor = robot.getDevice('right wheel sensor')
rightWheelSensor.enable(timestep)
class CustomRobotClass:
def __init__(self):
# Initialize Robot
self.robot = Robot()
self.time_step = int(self.robot.getBasicTimeStep())
print("Robot time step ", self.time_step)
# Initialize sensors
self.metacamera = self.robot.getCamera("MultiSense S21 meta camera")
self.leftcamera = self.robot.getCamera("MultiSense S21 left camera")
self.rightcamera = self.robot.getCamera("MultiSense S21 right camera")
self.rangeFinder = self.robot.getRangeFinder(
"MultiSense S21 meta range finder")
self.cameras = {
"left": self.leftcamera,
"right": self.rightcamera,
"meta": self.metacamera,
"depth": self.rangeFinder
}
self.leftpositionsensor = self.robot.getPositionSensor(
'back left wheel sensor')
self.rightpositionsensor = self.robot.getPositionSensor(
'back right wheel sensor')
self.keyboard = Keyboard()
self.pen = self.robot.getPen('pen')
# Enable sensors
self.metacamera.enable(self.time_step)
self.leftcamera.enable(self.time_step)
self.rightcamera.enable(self.time_step)
self.rangeFinder.enable(self.time_step)
self.leftpositionsensor.enable(self.time_step)
self.rightpositionsensor.enable(self.time_step)
self.keyboard.enable(self.time_step)
# Initialize wheels
backrightwheel = self.robot.getMotor('back right wheel')
backleftwheel = self.robot.getMotor('back left wheel')
frontrightwheel = self.robot.getMotor('front right wheel')
frontleftwheel = self.robot.getMotor('front left wheel')
self.wheels = [
backrightwheel, backleftwheel, frontrightwheel, frontleftwheel
]
self.set_velocity_control()
self.prevlspeed = 0
self.prevrspeed = 0
self.curlspeed = 0
self.currspeed = 0
def set_velocity_control(self):
for wheel in self.wheels:
wheel.setPosition((float('inf')))
wheel.setVelocity(0)
def step(self):
self.robot.step(self.time_step)
def set_speed(self, l, r):
print("Inside set speed", SPEED_UNIT * l, SPEED_UNIT * r)
if self.prevlspeed != l or self.prevrspeed != r:
self.wheels[0].setVelocity(SPEED_UNIT * r)
self.wheels[1].setVelocity(SPEED_UNIT * l)
self.wheels[2].setVelocity(SPEED_UNIT * r)
self.wheels[3].setVelocity(SPEED_UNIT * l)
self.prevlspeed = self.curlspeed
self.prevrspeed = self.currspeed
self.curlspeed = l
self.currspeed = r
def get_image(self, camera_name, depth_option=False):
if (depth_option == True):
return self.cameras[camera_name].getRangeImageArray()
else:
return self.cameras[camera_name].getImageArray()
class ManualTimestepRobot:
"""
Primary API for access to robot parts.
This robot requires that the consumer manage the progession of time manually
by calling the `sleep` method.
"""
def __init__(self, quiet: bool = False, init: bool = True) -> None:
self._initialised = False
self._quiet = quiet
self.webot = WebotsRobot()
# returns a float, but should always actually be an integer value
self._timestep = int(self.webot.getBasicTimeStep())
self.mode = environ.get("SR_ROBOT_MODE", "dev")
self.zone = int(environ.get("SR_ROBOT_ZONE", 0))
self.arena = "A"
self.usbkey = path.normpath(path.join(environ["SR_ROBOT_FILE"], "../"))
# Lock used to guard access to Webot's time stepping machinery, allowing
# us to safely advance simulation time from *either* the competitor's
# code (in the form of our `sleep` method) or from our background
# thread, but not both.
self._step_lock = Lock()
if init:
self.init()
self.wait_start()
@classmethod
def setup(cls):
return cls(init=False)
def init(self) -> None:
self._init_devs()
self._initialised = True
self.display_info()
def _get_user_code_info(self) -> Optional[str]:
user_version_path = path.join(self.usbkey, '.user-rev')
if path.exists(user_version_path):
with open(user_version_path) as f:
return f.read().strip()
return None
def display_info(self) -> None:
user_code_version = self._get_user_code_info()
parts = [
"Zone: {}".format(self.zone),
"Mode: {}".format(self.mode),
]
if user_code_version:
parts.append("User code: {}".format(user_code_version))
print("Robot Initialized. {}.".format(", ".join(parts))) # noqa:T001
def webots_step_and_should_continue(self, duration_ms: int) -> bool:
"""
Run a webots step of the given duration in milliseconds.
Returns whether or not the simulation should continue (based on
Webots telling us whether or not the simulation is about to end).
"""
if duration_ms <= 0:
raise ValueError(
"Duration must be greater than zero, not {!r}".format(
duration_ms), )
with self._step_lock:
# We use Webots in synchronous mode (specifically
# `synchronization` is left at its default value of `TRUE`). In
# that mode, Webots returns -1 from step to indicate that the
# simulation is terminating, or 0 otherwise.
result = self.webot.step(duration_ms)
return result != -1
def wait_start(self) -> None:
"Wait for the start signal to happen"
if self.mode not in ["comp", "dev"]:
raise Exception(
"mode of '%s' is not supported -- must be 'comp' or 'dev'" %
self.mode, )
if self.zone < 0 or self.zone > 3:
raise Exception(
"zone must be in range 0-3 inclusive -- value of %i is invalid"
% self.zone, )
if self.arena not in ["A", "B"]:
raise Exception("arena must be A or B")
def _init_devs(self) -> None:
"Initialise the attributes for accessing devices"
# Motor boards
self._init_motors()
# Ruggeduinos
self._init_ruggeduinos()
# Camera
self._init_camera()
def _init_motors(self) -> None:
self.motors = motor.init_motor_array(self.webot)
def _init_ruggeduinos(self) -> None:
self.ruggeduinos = ruggeduino.init_ruggeduino_array(self.webot)
def _init_camera(self) -> None:
# See comment in Camera.see for why we need to pass the step lock here.
self.camera = camera.Camera(self.webot, self._step_lock)
self.see = self.camera.see
def time(self) -> float:
"""
Roughly equivalent to `time.time` but for simulation time.
"""
return self.webot.getTime()
def sleep(self, secs: float) -> None:
"""
Roughly equivalent to `time.sleep` but accounting for simulation time.
"""
# Checks that secs is positive or zero
if secs < 0:
raise ValueError('sleep length must be non-negative')
# Ensure the time delay is a valid step increment, while also ensuring
# that small values remain nonzero.
n_steps = math.ceil((secs * 1000) / self._timestep)
duration_ms = n_steps * self._timestep
# We're in the main thread here, so we don't really need to do any
# cleanup if Webots tells us the simulation is terminating. When webots
# kills the process all the proper tidyup will happen anyway.
self.webots_step_and_should_continue(duration_ms)
def __init__(self, webot: Robot):
self._compass = get_robot_device(webot, "robot compass", WebotsCompass)
self._compass.enable(int(webot.getBasicTimeStep()))
from controller import Robot
from controller import Camera
TIME_STEP = 64
robot = Robot()
#Define motors
motors = []
motorNames = ['left motor', 'right motor']
for i in range(2):
motors.append(robot.getMotor(motorNames[i]))
motors[i].setPosition(float('inf'))
motors[i].setVelocity(0.0)
motors[i].setAcceleration(25)
camera = Camera('camera')
camera.enable(int(robot.getBasicTimeStep()))
SPEED = 2
while robot.step(TIME_STEP) != -1:
leftSpeed = 0.0
rightSpeed = 0.0
#get image and process it
image = camera.getImage()
leftSum = 0
rightSum = 0
cameraData = camera.getImage()
for x in range(0, camera.getWidth()):
for y in range(int(camera.getHeight() * 0.9), camera.getHeight()):
gray = Camera.imageGetGray(cameraData, camera.getWidth(), x, y)
if x < camera.getWidth() / 2:
leftSum += gray
else:
def main():
global robot, _timestep, _pop, _gbest, _w, _wdamp, _c1, _c2, iter
ph = 0.0
chi = 0.0
outbuffer = [0.0 for i in range(0, 3)]
# setting parameters for PSO
_w = _IW
_wdamp = _IW_Damp
_c1 = _CT1
_c2 = _CT2
# If you would like to use Constriction Coefficients for PSO,
# uncomment the following block and comment the above set of parameters.
# Constriction Coefficients
# -------------------------------------------------------
ph = _PH1 + _PH2 # was _PH1+_PH1
chi = 2.0 / (ph - 2 + math.sqrt(ph * ph - 4 * ph)
) # velocity constrain factor
_w = chi # inertia Weight
_wdamp = 1 # inertia Weight Damping Ratio
_c1 = chi * _PH1 # personal Learning Coefficient
_c2 = chi * _PH2 # global Learning Coefficient was _c1=chi*_PH2
# -------------------------------------------------------
# create the Robot instance.
robot = Robot()
# get the time step of the current world.
timestep = int(robot.getBasicTimeStep())
_timestep = timestep
# 每16个基本步算一步
InitRobot()
# Main loop:
InitSwarm()
Report(0)
for iter in range(1, _MaxIter + 1): # 开始 实验 _MaxIter 次看看会不会成功
for i in range(1,
_SwarmSize + 1): # 我们每一次Iter 其实都有_SwarmSize 个 particle
_pop[i].update_velocity()
# 公式15
_pop[i].update_position()
# 公式16
# Evaluation
_pop[i].fitness = Evaluation(_pop[i])
# evaluate 获取fitness
# Update Global Best
if _pop[i].fitness < _gbest.fitness:
_gbest.fitness = _pop[i].fitness
for j in range(1, _LenChrom + 1):
_gbest.pos[j] = _pop[i].pos[j]
else: #如果globle fitness没有变化或者还变差了，那就用firework炸一波
# 用FA更新swarm
FA_results = FWA(_pop, 5)
for i in range(len(_pop) - 1):
_pop[i + 1].FA_pop(FA_results['fireworks'][i],
FA_results['new_fitness'][i])
_gbest.fitness = _pop[1].fitness
for j in range(1, _LenChrom + 1):
_gbest.pos[j] = _pop[1].pos[j]
# update inertia weight with damping ratio
_w = _w * _wdamp
# update inertia weight with damping ratio
# save and report information of gbest
Report(iter)
# If the robot is capable of following the boundary more than one round,
# then PSO terminates.
if _gbest.fitness <= 1.0 / _MaxTimestep:
iter = _MaxIter + 1
outbuffer[0] = 100
# terminating simulation command
outbuffer[1] = _gbest.fitness
# fitness value
robot_send_data(outbuffer, 2)
break
# loop for iteration ends
# reload the environment.
outbuffer[0] = -1
# reset command
outbuffer[1] = 0
# fitness value
robot_send_data(outbuffer, 2)
return 0
from controller import Robot
from controller import Motor
from controller import PositionSensor
from controller import InertialUnit
import math
#create a robot instace
arm = Robot()
target1 = 0
target2 = 0
#target2=0
#kp=50
#kd=5
count = 0
r = 0
timestep = int(arm.getBasicTimeStep())
dt = 0.001 * timestep
#print(timestep)
logfile_name = "twolink_pd.log"
#initialise every component
motor1 = arm.getMotor("rm@flt")
motor2 = arm.getMotor("rm@flk")
motor3 = arm.getMotor("rm@blt")
motor4 = arm.getMotor("rm@blk")
motor5 = arm.getMotor("rm@frt")
motor6 = arm.getMotor("rm@frk")
motor7 = arm.getMotor("rm@brt")
motor8 = arm.getMotor("rm@brk")
ps1 = arm.getPositionSensor("ps@flt")
ps2 = arm.getPositionSensor("ps@flk")
ENABLE_SOCKET = True
ENABLE_VISION_DISPLAY = False
VISION_DISPLAY_NAME = 'vision_display'
# create the Robot instance.
robot = Robot()
# initialize vision display
vision_display = False
if ENABLE_VISION_DISPLAY:
vision_display = VisionDisplay(robot.getDisplay(VISION_DISPLAY_NAME),
robot.getCamera('camera'), Display.RGB)
# get the time step of the current world.
Constants.TIMESTEP = int(robot.getBasicTimeStep())
# get the robot movement logic
rbc = RobotControls(robot)
# get the keyboard for user input
kb = robot.getKeyboard()
kb.enable(Constants.TIMESTEP)
socket = False
if ENABLE_SOCKET:
socket = SocketClient('localhost', 4444)
# configure Controller class to handle all logic (robot movement and user input)
RobotController = RobotController(rbc,
url="ws://localhost:3333",
class Walk():
# class standingState(Enum):
# STANDING = 1
# FALL_BACK = 2
# FALL_FRONT = 3
# FALL_RIGHT = 4
# FALL_LEFT = 5
def __init__(self):
self.robot = Robot() # 初始化Robot类以控制机器人
self.mTimeStep = int(
self.robot.getBasicTimeStep()) # 获取当前每一个仿真步所仿真时间mTimeStep
self.HeadLed = self.robot.getLED('HeadLed') # 获取头部LED灯
self.EyeLed = self.robot.getLED('EyeLed') # 获取眼部LED灯
self.HeadLed.set(0xff0000) # 点亮头部LED灯并设置一个颜色
self.EyeLed.set(0xa0a0ff) # 点亮眼部LED灯并设置一个颜色
self.mAccelerometer = self.robot.getAccelerometer(
'Accelerometer') # 获取加速度传感器
self.mAccelerometer.enable(self.mTimeStep) # 激活传感器，并以mTimeStep为周期更新数值
# self.falling_state = standingState.STANDING
self.fup = 0
self.fdown = 0 # 定义两个类变量，用于之后判断机器人是否摔倒
self.fleft = 0
self.fright = 0 # 定义两个类变量，用于之后判断机器人是否摔倒
self.mGyro = self.robot.getGyro('Gyro') # 获取陀螺仪
self.mGyro.enable(self.mTimeStep) # 激活陀螺仪，并以mTimeStep为周期更新数值
self.positionSensors = [] # 初始化关节角度传感器
self.positionSensorNames = (
'ShoulderR', 'ShoulderL', 'ArmUpperR', 'ArmUpperL', 'ArmLowerR',
'ArmLowerL', 'PelvYR', 'PelvYL', 'PelvR', 'PelvL', 'LegUpperR',
'LegUpperL', 'LegLowerR', 'LegLowerL', 'AnkleR', 'AnkleL', 'FootR',
'FootL', 'Neck', 'Head') # 初始化各传感器名
# 获取各传感器并激活，以mTimeStep为周期更新数值
for i in range(0, len(self.positionSensorNames)):
self.positionSensors.append(
self.robot.getPositionSensor(self.positionSensorNames[i] +
'S'))
self.positionSensors[i].enable(self.mTimeStep)
self.mKeyboard = self.robot.getKeyboard() # 初始化键盘读入类
self.mKeyboard.enable(self.mTimeStep) # 以mTimeStep为周期从键盘读取
self.mMotionManager = RobotisOp2MotionManager(
self.robot) # 初始化机器人动作组控制器
self.mGaitManager = RobotisOp2GaitManager(self.robot,
"config.ini") # 初始化机器人步态控制器
def myStep(self):
ret = self.robot.step(self.mTimeStep)
if ret == -1:
exit(0)
def wait(self, ms):
startTime = self.robot.getTime()
s = ms / 1000.0
while s + startTime >= self.robot.getTime():
self.myStep()
def run(self):
print("-------Walk example of ROBOTIS OP2-------")
print("This example illustrates Gait Manager")
print("Press the space bar to start/stop walking")
print("Use the arrow keys to move the robot while walking")
self.myStep() # 仿真一个步长，刷新传感器读数
self.mMotionManager.playPage(9) # 执行动作组9号动作，初始化站立姿势，准备行走
self.wait(200) # 等待200ms
self.isWalking = False # 初始时机器人未进入行走状态
while True:
self.checkIfFallen()
self.mGaitManager.setXAmplitude(0.0) # 前进为0
self.mGaitManager.setAAmplitude(0.0) # 转体为0
key = 0 # 初始键盘读入默认为0
key = self.mKeyboard.getKey() # 从键盘读取输入
if key == 32: # 如果读取到空格，则改变行走状态
if (self.isWalking): # 如果当前机器人正在走路，则使机器人停止
self.mGaitManager.stop()
self.isWalking = False
self.wait(200)
else: # 如果机器人当前停止，则开始走路
self.mGaitManager.start()
self.isWalking = True
self.wait(200)
elif key == 315: # 如果读取到‘↑’，则前进
self.mGaitManager.setXAmplitude(1.0)
elif key == 317: # 如果读取到‘↓’，则后退
self.mGaitManager.setXAmplitude(-1.0)
elif key == 316: # 如果读取到‘←’，则左转
self.mGaitManager.setAAmplitude(-0.5)
elif key == 314: # 如果读取到‘→’，则右转
self.mGaitManager.setAAmplitude(0.5)
self.mGaitManager.step(self.mTimeStep) # 步态生成器生成一个步长的动作
self.myStep() # 仿真一个步长
def checkIfFallen(self):
acc = self.mAccelerometer.getValues() # 通过加速度传感器获取三轴的对应值
acc_tolerance = 80.0
acc_step = 100
# print(acc)
# count how many steps the accelerometer
# says that the robot is down
if (acc[1] < 512.0 - acc_tolerance):
self.fup = self.fup + 1
else:
self.fup = 0
if (acc[1] > 512.0 + acc_tolerance):
self.fdown = self.fdown + 1
else:
self.fdown = 0
if (acc[0] < 512.0 - acc_tolerance):
self.fright = self.fright + 1
else:
self.fright = 0
if (acc[0] > 512.0 + acc_tolerance):
self.fleft = self.fleft + 1
else:
self.fleft = 0
# the robot face is down
if (self.fup > acc_step):
print("1")
self.mMotionManager.playPage(10) # f_up
self.mMotionManager.playPage(9) # init position
self.fup = 0
# the back face is down
elif (self.fdown > acc_step):
print("2")
self.mMotionManager.playPage(11) # b_up
self.mMotionManager.playPage(9) # init position
self.fdown = 0
# the back face is down
elif (self.fright > acc_step):
print("3")
self.mMotionManager.playPage(13)
self.fright = 0
# the back face is down
elif (self.fleft > acc_step):
print("4")
self.mMotionManager.playPage(12)
self.fleft = 0
'f4_pos',
't4_pos', # LEG 4
'c5_pos',
'f5_pos',
't5_pos', # LEG 5
'c6_pos',
'f6_pos',
't6_pos',
] # LEG 6
for i in range(18):
ps.append(robot.getPositionSensor(psName[i]))
ps[i].enable(POSITION_SENSOR_SAMPLE_PERIOD)
robot = Robot()
timeStep = int(4 * robot.getBasicTimeStep())
keyboard = robot.getKeyboard()
init_servos()
init_positional_sensors()
def run():
positions = [
1.0, 1.0, 2, 1.0, 1.0, 2, 1.0, 1.0, 2, 1.0, 1.0, 2, 1.0, 1.0, 2, 1.0,
1.0, 2
]
while robot.step(timeStep) != 1:
for servo, position in zip(servos, positions):
servo.setPosition(position)
break
class SimCamera(object):
"""Acquire image and depth from a camera sensor and publish on ROS"""
def __init__(self):
# webots
os.environ['WEBOTS_ROBOT_NAME'] = 'camera'
self.robot = Robot()
self.bridge = CvBridge()
self.timestep = int(self.robot.getBasicTimeStep())
# timestep is 4ms equals to 250FPS
# nopub 4ms aprox 250FPS avg 0.5x simulation time
# nopub 33ms aprox 30FPS avg 4x simulation time
# nopub 67ms aprox 15FPS avg 7x simulation time
# nopub 100ms aprox 10FPS avg 8x simulation time
# nopub 500ms aprox 2FPS avg 8.5x simulation time
self.camera = self.robot.getDeviceByIndex(1)
self.depthcamera = self.robot.getDeviceByIndex(2)
# ROS
rospy.init_node('camera_server', anonymous=False)
self.service_image = rospy.Service('image_camera_service', SimImageCameraService, self.getImage)
self.service_depth = rospy.Service('depth_camera_service', SimDepthCameraService, self.getDepth)
self.flag = False
def getImage(self, data):
"""
respond to image request
"""
# enable the camera
self.camera.enable(self.timestep)
# take a time step
# for i in range(5):
self.robot.step(self.timestep)
# get the image
self.camera.getImage()
self.camera.saveImage('tmp-img.png', 100)
img = cv2.imread('tmp-img.png')
# os.remove('tmp-img.png')
self.camera_msg = self.bridge.cv2_to_imgmsg(img)
# disable the camera to save processing power
self.camera.disable()
self.flag = True
# send the image
rospy.loginfo('Sending Image ')
return self.camera_msg
def getDepth(self, data):
"""
respond to depth image request
"""
# enable the camera
self.depthcamera.enable(self.timestep)
# take a time step
# for i in range(5):
self.robot.step(self.timestep)
# get the depth image
self.depthcamera.getRangeImage()
self.depthcamera.saveImage('tmp-dep.png', 100)
dep = self.depthcamera.getRangeImageArray()
# print(type(dep))
# print(len(dep),len(dep[0]))
dep = np.array(dep).transpose()
self.depthcamera_msg = self.bridge.cv2_to_imgmsg(dep)
# disable the camera to save processing power
self.depthcamera.disable()
# send the image
rospy.loginfo('Sending Depth')
return self.depthcamera_msg
def update(self):
"""
publish the image and depth
"""
# image
return
self.camera.getImage()
self.camera.saveImage('tmp-img.png', 100)
img = cv2.imread('tmp-img.png')
self.camera_msg = self.bridge.cv2_to_imgmsg(img)
self.camera_pub.publish(self.camera_msg)
# depth
self.depthcamera.getRangeImage()
dep = self.depthcamera.getRangeImageArray()
dep = np.array(dep).transpose()
self.depthcamera_msg = self.bridge.cv2_to_imgmsg(dep)
self.depthcamera_pub.publish(self.depthcamera_msg)
class RobotController:
def __init__(self,
ros_active=False,
robot='wolfgang',
do_ros_init=True,
robot_node=None,
base_ns='',
recognize=False,
camera_active=True,
foot_sensors_active=True):
"""
The RobotController, a Webots controller that controls a single robot.
The environment variable WEBOTS_ROBOT_NAME should be set to "amy", "rory", "jack" or "donna" if used with
4_bots.wbt or to "amy" if used with 1_bot.wbt.
:param ros_active: Whether ROS messages should be published
:param robot: The name of the robot to use, currently one of wolfgang, darwin, nao, op3
:param do_ros_init: Whether to call rospy.init_node (only used when ros_active is True)
:param external_controller: Whether an external controller is used, necessary for RobotSupervisorController
:param base_ns: The namespace of this node, can normally be left empty
"""
self.ros_active = ros_active
self.recognize = recognize
self.camera_active = camera_active
self.foot_sensors_active = foot_sensors_active
if robot_node is None:
self.robot_node = Robot()
else:
self.robot_node = robot_node
self.walkready = [0] * 20
self.time = 0
self.motors = []
self.sensors = []
# for direct access
self.motors_dict = {}
self.sensors_dict = {}
self.timestep = int(self.robot_node.getBasicTimeStep())
self.switch_coordinate_system = True
self.is_wolfgang = False
self.pressure_sensors = None
if robot == 'wolfgang':
self.is_wolfgang = True
self.proto_motor_names = [
"RShoulderPitch [shoulder]", "LShoulderPitch [shoulder]",
"RShoulderRoll", "LShoulderRoll", "RElbow", "LElbow",
"RHipYaw", "LHipYaw", "RHipRoll [hip]", "LHipRoll [hip]",
"RHipPitch", "LHipPitch", "RKnee", "LKnee", "RAnklePitch",
"LAnklePitch", "RAnkleRoll", "LAnkleRoll", "HeadPan",
"HeadTilt"
]
self.external_motor_names = [
"RShoulderPitch", "LShoulderPitch", "RShoulderRoll",
"LShoulderRoll", "RElbow", "LElbow", "RHipYaw", "LHipYaw",
"RHipRoll", "LHipRoll", "RHipPitch", "LHipPitch", "RKnee",
"LKnee", "RAnklePitch", "LAnklePitch", "RAnkleRoll",
"LAnkleRoll", "HeadPan", "HeadTilt"
]
self.initial_joint_positions = {
"LAnklePitch": -30,
"LAnkleRoll": 0,
"LHipPitch": 30,
"LHipRoll": 0,
"LHipYaw": 0,
"LKnee": 60,
"RAnklePitch": 30,
"RAnkleRoll": 0,
"RHipPitch": -30,
"RHipRoll": 0,
"RHipYaw": 0,
"RKnee": -60,
"LShoulderPitch": 75,
"LShoulderRoll": 0,
"LElbow": 36,
"RShoulderPitch": -75,
"RShoulderRoll": 0,
"RElbow": -36,
"HeadPan": 0,
"HeadTilt": 0
}
self.sensor_suffix = "_sensor"
accel_name = "imu accelerometer"
gyro_name = "imu gyro"
camera_name = "camera"
self.pressure_sensor_names = []
if self.foot_sensors_active:
self.pressure_sensor_names = [
"llb", "llf", "lrf", "lrb", "rlb", "rlf", "rrf", "rrb"
]
self.pressure_sensors = []
for name in self.pressure_sensor_names:
sensor = self.robot_node.getDevice(name)
sensor.enable(self.timestep)
self.pressure_sensors.append(sensor)
elif robot == 'darwin':
self.proto_motor_names = [
"ShoulderR", "ShoulderL", "ArmUpperR", "ArmUpperL",
"ArmLowerR", "ArmLowerL", "PelvYR", "PelvYL", "PelvR", "PelvL",
"LegUpperR", "LegUpperL", "LegLowerR", "LegLowerL", "AnkleR",
"AnkleL", "FootR", "FootL", "Neck", "Head"
]
self.external_motor_names = [
"RShoulderPitch", "LShoulderPitch", "RShoulderRoll",
"LShoulderRoll", "RElbow", "LElbow", "RHipYaw", "LHipYaw",
"RHipRoll", "LHipRoll", "RHipPitch", "LHipPitch", "RKnee",
"LKnee", "RAnklePitch", "LAnklePitch", "RAnkleRoll",
"LAnkleRoll", "HeadPan", "HeadTilt"
]
self.sensor_suffix = "S"
accel_name = "Accelerometer"
gyro_name = "Gyro"
camera_name = "Camera"
elif robot == 'nao':
self.proto_motor_names = [
"RShoulderPitch", "LShoulderPitch", "RShoulderRoll",
"LShoulderRoll", "RElbowYaw", "LElbowYaw", "RHipYawPitch",
"LHipYawPitch", "RHipRoll", "LHipRoll", "RHipPitch",
"LHipPitch", "RKneePitch", "LKneePitch", "RAnklePitch",
"LAnklePitch", "RAnkleRoll", "LAnkleRoll", "HeadYaw",
"HeadPitch"
]
self.external_motor_names = self.proto_motor_names
self.sensor_suffix = "S"
accel_name = "accelerometer"
gyro_name = "gyro"
camera_name = "CameraTop"
self.switch_coordinate_system = False
elif robot == 'op3':
self.proto_motor_names = [
"ShoulderR", "ShoulderL", "ArmUpperR", "ArmUpperL",
"ArmLowerR", "ArmLowerL", "PelvYR", "PelvYL", "PelvR", "PelvL",
"LegUpperR", "LegUpperL", "LegLowerR", "LegLowerL", "AnkleR",
"AnkleL", "FootR", "FootL", "Neck", "Head"
]
self.external_motor_names = [
"r_sho_pitch", "l_sho_pitch", "r_sho_roll", "l_sho_roll",
"r_el", "l_el", "r_hip_yaw", "l_hip_yaw", "r_hip_roll",
"l_hip_roll", "r_hip_pitch", "l_hip_pitch", "r_knee", "l_knee",
"r_ank_pitch", "l_ank_pitch", "r_ank_roll", "l_ank_roll",
"head_pan", "head_tilt"
]
self.sensor_suffix = "S"
accel_name = "Accelerometer"
gyro_name = "Gyro"
camera_name = "Camera"
self.switch_coordinate_system = False
self.motor_names_to_external_names = {}
self.external_motor_names_to_motor_names = {}
for i in range(len(self.proto_motor_names)):
self.motor_names_to_external_names[
self.proto_motor_names[i]] = self.external_motor_names[i]
self.external_motor_names_to_motor_names[
self.external_motor_names[i]] = self.proto_motor_names[i]
self.current_positions = {}
self.joint_limits = {}
for motor_name in self.proto_motor_names:
motor = self.robot_node.getDevice(motor_name)
motor.enableTorqueFeedback(self.timestep)
self.motors.append(motor)
self.motors_dict[
self.motor_names_to_external_names[motor_name]] = motor
sensor = self.robot_node.getDevice(motor_name + self.sensor_suffix)
sensor.enable(self.timestep)
self.sensors.append(sensor)
self.sensors_dict[
self.motor_names_to_external_names[motor_name]] = sensor
self.current_positions[self.motor_names_to_external_names[
motor_name]] = sensor.getValue()
# min, max and middle position (precomputed since it will be used at each step)
self.joint_limits[
self.motor_names_to_external_names[motor_name]] = (
motor.getMinPosition(), motor.getMaxPosition(),
0.5 * (motor.getMinPosition() + motor.getMaxPosition()))
self.accel = self.robot_node.getDevice(accel_name)
self.accel.enable(self.timestep)
self.gyro = self.robot_node.getDevice(gyro_name)
self.gyro.enable(self.timestep)
if self.is_wolfgang:
self.accel_head = self.robot_node.getDevice(
"imu_head accelerometer")
self.accel_head.enable(self.timestep)
self.gyro_head = self.robot_node.getDevice("imu_head gyro")
self.gyro_head.enable(self.timestep)
self.camera = self.robot_node.getDevice(camera_name)
self.camera_counter = 0
if self.camera_active:
self.camera.enable(self.timestep * CAMERA_DIVIDER)
if self.recognize:
self.camera.recognitionEnable(self.timestep)
self.last_img_saved = 0.0
self.img_save_dir = "/tmp/webots/images" + \
time.strftime("%Y-%m-%d-%H-%M-%S") + \
os.getenv('WEBOTS_ROBOT_NAME')
if not os.path.exists(self.img_save_dir):
os.makedirs(self.img_save_dir)
self.imu_frame = rospy.get_param("~imu_frame", "imu_frame")
if self.ros_active:
if base_ns == "":
clock_topic = "/clock"
else:
clock_topic = base_ns + "clock"
if do_ros_init:
rospy.init_node("webots_ros_interface",
argv=['clock:=' + clock_topic])
self.l_sole_frame = rospy.get_param("~l_sole_frame", "l_sole")
self.r_sole_frame = rospy.get_param("~r_sole_frame", "r_sole")
self.camera_optical_frame = rospy.get_param(
"~camera_optical_frame", "camera_optical_frame")
self.head_imu_frame = rospy.get_param("~head_imu_frame",
"imu_frame_2")
self.pub_js = rospy.Publisher(base_ns + "joint_states",
JointState,
queue_size=1)
self.pub_imu = rospy.Publisher(base_ns + "imu/data_raw",
Imu,
queue_size=1)
self.pub_imu_head = rospy.Publisher(base_ns + "imu_head/data",
Imu,
queue_size=1)
self.pub_cam = rospy.Publisher(base_ns + "camera/image_proc",
Image,
queue_size=1)
self.pub_cam_info = rospy.Publisher(base_ns + "camera/camera_info",
CameraInfo,
queue_size=1,
latch=True)
self.pub_pres_left = rospy.Publisher(base_ns +
"foot_pressure_left/raw",
FootPressure,
queue_size=1)
self.pub_pres_right = rospy.Publisher(base_ns +
"foot_pressure_right/raw",
FootPressure,
queue_size=1)
self.cop_l_pub_ = rospy.Publisher(base_ns + "cop_l",
PointStamped,
queue_size=1)
self.cop_r_pub_ = rospy.Publisher(base_ns + "cop_r",
PointStamped,
queue_size=1)
rospy.Subscriber(base_ns + "DynamixelController/command",
JointCommand, self.command_cb)
# publish camera info once, it will be latched
self.cam_info = CameraInfo()
self.cam_info.header.stamp = rospy.Time.from_seconds(self.time)
self.cam_info.header.frame_id = self.camera_optical_frame
self.cam_info.height = self.camera.getHeight()
self.cam_info.width = self.camera.getWidth()
f_y = self.mat_from_fov_and_resolution(
self.h_fov_to_v_fov(self.camera.getFov(), self.cam_info.height,
self.cam_info.width), self.cam_info.height)
f_x = self.mat_from_fov_and_resolution(self.camera.getFov(),
self.cam_info.width)
self.cam_info.K = [
f_x, 0, self.cam_info.width / 2, 0, f_y,
self.cam_info.height / 2, 0, 0, 1
]
self.cam_info.P = [
f_x, 0, self.cam_info.width / 2, 0, 0, f_y,
self.cam_info.height / 2, 0, 0, 0, 1, 0
]
self.pub_cam_info.publish(self.cam_info)
if robot == "op3":
# start pose
command = JointCommand()
command.joint_names = ["r_sho_roll", "l_sho_roll"]
command.positions = [-math.tau / 8, math.tau / 8]
self.command_cb(command)
# needed to run this one time to initialize current position, otherwise velocity will be nan
self.get_joint_values(self.external_motor_names)
def mat_from_fov_and_resolution(self, fov, res):
return 0.5 * res * (math.cos((fov / 2)) / math.sin((fov / 2)))
def h_fov_to_v_fov(self, h_fov, height, width):
return 2 * math.atan(math.tan(h_fov * 0.5) * (height / width))
def step_sim(self):
self.time += self.timestep / 1000
self.robot_node.step(self.timestep)
def step(self):
self.step_sim()
if self.ros_active:
self.publish_ros()
def publish_ros(self):
self.publish_imu()
self.publish_joint_states()
if self.camera_active and self.camera_counter == 0:
self.publish_camera()
self.publish_pressure()
if self.recognize:
self.save_recognition()
self.camera_counter = (self.camera_counter + 1) % CAMERA_DIVIDER
def convert_joint_radiant_to_scaled(self, joint_name, pos):
# helper method to convert to scaled position between [-1,1] for this joint using min max scaling
lower_limit, upper_limit, mid_position = self.joint_limits[joint_name]
return 2 * (pos - mid_position) / (upper_limit - lower_limit)
def convert_joint_scaled_to_radiant(self, joint_name, position):
# helper method to convert to scaled position for this joint using min max scaling
lower_limit, upper_limit, mid_position = self.joint_limits[joint_name]
return position * (upper_limit - lower_limit) / 2 + mid_position
def set_joint_goal_position(self,
joint_name,
goal_position,
goal_velocity=-1,
scaled=False,
relative=False):
motor = self.motors_dict[joint_name]
if scaled:
goal_position = self.convert_joint_radiant_to_scaled(
joint_name, goal_position)
if relative:
goal_position = goal_position + self.get_joint_values([joint_name
])[0]
motor.setPosition(goal_position)
if goal_velocity == -1:
motor.setVelocity(motor.getMaxVelocity())
else:
motor.setVelocity(goal_velocity)
def set_joint_goals_position(self,
joint_names,
goal_positions,
goal_velocities=[]):
for i in range(len(joint_names)):
try:
if len(goal_velocities) != 0:
self.set_joint_goal_position(joint_names[i],
goal_positions[i],
goal_velocities[i])
else:
self.set_joint_goal_position(joint_names[i],
goal_positions[i])
except ValueError:
print(f"invalid motor specified ({joint_names[i]})")
def command_cb(self, command: JointCommand):
if len(command.positions) != 0:
# position control
# todo maybe needs to match external motor names to interal ones fist?
self.set_joint_goals_position(command.joint_names,
command.positions,
command.velocities)
else:
# torque control
for i, name in enumerate(command.joint_names):
try:
self.motors_dict[name].setTorque(command.accelerations[i])
except ValueError:
print(f"invalid motor specified ({name})")
def set_head_tilt(self, pos):
self.motors[-1].setPosition(pos)
def set_arms_zero(self):
positions = [
-0.8399999308200574, 0.7200000596634105, -0.3299999109923385,
0.35999992683575216, 0.5099999812500172, -0.5199999789619728
]
for i in range(0, 6):
self.motors[i].setPosition(positions[i])
def get_joint_values(self, used_joint_names, scaled=False):
joint_positions = []
joint_velocities = []
joint_torques = []
for joint_name in used_joint_names:
value = self.sensors_dict[joint_name].getValue()
if scaled:
value = self.convert_joint_radiant_to_scaled(joint_name, value)
joint_positions.append(value)
joint_velocities.append(self.current_positions[joint_name] - value)
joint_torques.append(
self.motors_dict[joint_name].getTorqueFeedback())
self.current_positions[joint_name] = value
return joint_positions, joint_velocities, joint_torques
def get_joint_state_msg(self):
js = JointState()
js.name = []
js.header.stamp = rospy.Time.from_seconds(self.time)
js.position = []
js.effort = []
for joint_name in self.external_motor_names:
js.name.append(joint_name)
value = self.sensors_dict[joint_name].getValue()
js.position.append(value)
js.velocity.append(self.current_positions[joint_name] - value)
js.effort.append(self.motors_dict[joint_name].getTorqueFeedback())
self.current_positions[joint_name] = value
return js
def publish_joint_states(self):
self.pub_js.publish(self.get_joint_state_msg())
def get_imu_msg(self, head=False):
msg = Imu()
msg.header.stamp = rospy.Time.from_seconds(self.time)
if head:
msg.header.frame_id = self.head_imu_frame
else:
msg.header.frame_id = self.imu_frame
# change order because webots has different axis
if head:
accel_vels = self.accel_head.getValues()
msg.linear_acceleration.x = accel_vels[2]
msg.linear_acceleration.y = -accel_vels[0]
msg.linear_acceleration.z = -accel_vels[1]
else:
accel_vels = self.accel.getValues()
msg.linear_acceleration.x = accel_vels[0]
msg.linear_acceleration.y = accel_vels[1]
msg.linear_acceleration.z = accel_vels[2]
# make sure that acceleration is not completely zero or we will get error in filter.
# Happens if robot is moved manually in the simulation
if msg.linear_acceleration.x == 0 and msg.linear_acceleration.y == 0 and msg.linear_acceleration.z == 0:
msg.linear_acceleration.z = 0.001
if head:
gyro_vels = self.gyro_head.getValues()
msg.angular_velocity.x = gyro_vels[2]
msg.angular_velocity.y = -gyro_vels[0]
msg.angular_velocity.z = -gyro_vels[1]
else:
gyro_vels = self.gyro.getValues()
msg.angular_velocity.x = gyro_vels[0]
msg.angular_velocity.y = gyro_vels[1]
msg.angular_velocity.z = gyro_vels[2]
return msg
def publish_imu(self):
self.pub_imu.publish(self.get_imu_msg(head=False))
if self.is_wolfgang:
self.pub_imu_head.publish(self.get_imu_msg(head=True))
def publish_camera(self):
img_msg = Image()
img_msg.header.stamp = rospy.Time.from_seconds(self.time)
img_msg.header.frame_id = self.camera_optical_frame
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 save_recognition(self):
if self.time - self.last_img_saved < 1.0:
return
self.last_img_saved = self.time
annotation = ""
img_stamp = f"{self.time:.2f}".replace(".", "_")
img_name = f"img_{os.getenv('WEBOTS_ROBOT_NAME')}_{img_stamp}.PNG"
recognized_objects = self.camera.getRecognitionObjects()
# variables for saving not in image later
found_ball = False
found_wolfgang = False
for e in range(self.camera.getRecognitionNumberOfObjects()):
model = recognized_objects[e].get_model()
position = recognized_objects[e].get_position_on_image()
size = recognized_objects[e].get_size_on_image()
if model == b"soccer ball":
found_ball = True
vector = f"""{{"x1": {position[0] - 0.5 * size[0]}, "y1": {position[1] - 0.5 * size[1]}, "x2": {position[0] + 0.5 * size[0]}, "y2": {position[1] + 0.5 * size[1]}}}"""
annotation += f"{img_name}|"
annotation += "ball|"
annotation += vector
annotation += "\n"
if model == b"wolfgang":
found_wolfgang = True
vector = f"""{{"x1": {position[0] - 0.5 * size[0]}, "y1": {position[1] - 0.5 * size[1]}, "x2": {position[0] + 0.5 * size[0]}, "y2": {position[1] + 0.5 * size[1]}}}"""
annotation += f"{img_name}|"
annotation += "robot|"
annotation += vector
annotation += "\n"
if not found_ball:
annotation += f"{img_name}|ball|not in image\n"
if not found_wolfgang:
annotation += f"{img_name}|robot|not in image\n"
with open(os.path.join(self.img_save_dir, "annotations.txt"),
"a") as f:
f.write(annotation)
self.camera.saveImage(filename=os.path.join(self.img_save_dir,
img_name),
quality=100)
def get_image(self):
return self.camera.getImage()
def get_pressure_message(self):
current_time = rospy.Time.from_sec(self.time)
if not self.foot_sensors_active:
cop_r = PointStamped()
cop_r.header.frame_id = self.r_sole_frame
cop_r.header.stamp = current_time
cop_l = PointStamped()
cop_l.header.frame_id = self.l_sole_frame
cop_l.header.stamp = current_time
return FootPressure(), FootPressure(), cop_l, cop_r
left_pressure = FootPressure()
left_pressure.header.stamp = current_time
left_pressure.left_back = self.pressure_sensors[0].getValue()
left_pressure.left_front = self.pressure_sensors[1].getValue()
left_pressure.right_front = self.pressure_sensors[2].getValue()
left_pressure.right_back = self.pressure_sensors[3].getValue()
right_pressure = FootPressure()
right_pressure.header.stamp = current_time
right_pressure.left_back = self.pressure_sensors[4].getValue()
right_pressure.left_front = self.pressure_sensors[5].getValue()
right_pressure.right_front = self.pressure_sensors[6].getValue()
right_pressure.right_back = self.pressure_sensors[7].getValue()
# compute center of pressures of the feet
pos_x = 0.085
pos_y = 0.045
# we can take a very small threshold, since simulation gives more accurate values than reality
threshold = 1
cop_l = PointStamped()
cop_l.header.frame_id = self.l_sole_frame
cop_l.header.stamp = current_time
sum = left_pressure.left_back + left_pressure.left_front + left_pressure.right_front + left_pressure.right_back
if sum > threshold:
cop_l.point.x = (left_pressure.left_front +
left_pressure.right_front -
left_pressure.left_back -
left_pressure.right_back) * pos_x / sum
cop_l.point.x = max(min(cop_l.point.x, pos_x), -pos_x)
cop_l.point.y = (left_pressure.left_front + left_pressure.left_back
- left_pressure.right_front -
left_pressure.right_back) * pos_y / sum
cop_l.point.y = max(min(cop_l.point.x, pos_y), -pos_y)
else:
cop_l.point.x = 0
cop_l.point.y = 0
cop_r = PointStamped()
cop_r.header.frame_id = self.r_sole_frame
cop_r.header.stamp = current_time
sum = right_pressure.right_back + right_pressure.right_front + right_pressure.right_front + right_pressure.right_back
if sum > threshold:
cop_r.point.x = (right_pressure.left_front +
right_pressure.right_front -
right_pressure.left_back -
right_pressure.right_back) * pos_x / sum
cop_r.point.x = max(min(cop_r.point.x, pos_x), -pos_x)
cop_r.point.y = (right_pressure.left_front +
right_pressure.left_back -
right_pressure.right_front -
right_pressure.right_back) * pos_y / sum
cop_r.point.y = max(min(cop_r.point.x, pos_y), -pos_y)
else:
cop_r.point.x = 0
cop_r.point.y = 0
return left_pressure, right_pressure, cop_l, cop_r
def publish_pressure(self):
left, right, cop_l, cop_r = self.get_pressure_message()
self.pub_pres_left.publish(left)
self.pub_pres_right.publish(right)
self.cop_l_pub_.publish(cop_l)
self.cop_r_pub_.publish(cop_r)
import numpy as np import pickle # Import evalution functions from eval import showPlots # Import functions from other scripts in controller folder from lqr_controller import LQRController from adaptive_controller import AdaptiveController #from lqr_controller import CustomController # Instantiate dron driver supervisor driver = Robot() # Get the time step of the current world timestep = int(driver.getBasicTimeStep()) # Set your percent loss of thrust lossOfThust = 0.5 # Instantiate controller and start sensors customController = AdaptiveController(driver, lossOfThust) customController.initializeMotors() customController.startSensors(timestep) # Initialize state storage vectors stateHistory = [] referenceHistory = [] # flag for motor failure motor_failure = False
import os
import time
import psutil
os.environ['WEBOTS_ROBOT_NAME'] = 'icebox'
found_icebox = False
for proc in psutil.process_iter():
try:
if 'icebox' in proc.name().lower():
found_icebox = True
break
except (psutil.NoSuchProcess, psutil.AccessDenied, psutil.ZombieProcess):
pass
if found_icebox is False:
# print('icebox controller: killing previous instances')
# os.system("killall -9 icebox")
print('icebox controller: running new')
os.system("icebox --Ice.Config=icebox.conf &")
from controller import Robot
robot = Robot()
while robot.step(int(robot.getBasicTimeStep())) != -1:
pass
class Controller():
jointNames = [
# coxa, femur, tibia
'c1',
'f1',
't1', # LEG 1
'c2',
'f2',
't2', # LEG 2
'c3',
'f3',
't3', # LEG 3
'c4',
'f4',
't4', # LEG 4
'c5',
'f5',
't5', # LEG 5
'c6',
'f6',
't6'
] # LEG 6
def __init__(self):
# Initialize the Webots Supervisor.
self.robot = Robot()
self.timeStep = int(4 * self.robot.getBasicTimeStep())
self.keyboard = self.robot.getKeyboard()
# Define list for motors and position sensors
self.motors = []
self.position_sensors = []
# Initialise the motors and position sensors (could be moved into __init__ into a single for-loop)
self.init_motors()
self.init_positional_sensors()
def init_motors(self):
for name in Controller.jointNames:
motor = self.robot.getMotor(name + '_motor')
# motor.setPosition(float('inf'))
# motor.setVelocity(0)
self.motors.append(motor)
def init_positional_sensors(self):
for name in Controller.jointNames:
positional_sensor = self.robot.getPositionSensor(
name + '_position_sensor')
positional_sensor.enable(POSITION_SENSOR_SAMPLE_PERIOD)
self.position_sensors.append(positional_sensor)
def positionN(self):
while self.robot.step(self.timeStep) != 1:
for motor, position in zip(self.motors, positions):
motor.setPosition(position)
break
def readPos(self):
value = []
while self.robot.step(self.timeStep) != 1:
for i in range(len(self.jointNames)):
value = self.position_sensors[i].getValue()
all_positions.append(value)
print(all_positions)
#if all_positions == positions:
# all_positions = []
# return
#all_positions = []
return # not sure about that one
def walk(self):
while self.robot.step(self.timeStep) != -1:
tripodGait(0, 20, 10, 1)
'--recognize',
action='store_true',
help="if true, recognition is active (for training data collection)")
args, unknown = parser.parse_known_args()
rospy.init_node("webots_ros_interface", argv=['clock:=/clock'])
pid_param_name = "/webots_pid" + args.sim_id
while not rospy.has_param(pid_param_name):
rospy.logdebug("Waiting for parameter " + pid_param_name + " to be set..")
time.sleep(2.0)
webots_pid = rospy.get_param(pid_param_name)
os.environ["WEBOTS_PID"] = str(webots_pid)
os.environ["WEBOTS_ROBOT_NAME"] = args.robot_name
if args.void_controller:
rospy.logdebug("Starting void interface for " + args.robot_name)
from controller import Robot
r = Robot()
while not rospy.is_shutdown():
r.step(int(r.getBasicTimeStep()))
else:
rospy.logdebug("Starting ros interface for " + args.robot_name)
r = RobotController(ros_active=True,
do_ros_init=False,
recognize=args.recognize,
camera_active=(not args.disable_camera))
while not rospy.is_shutdown():
r.step()
# Update: 17 September 2021 - add comments and adjust variable names # Update: 03 March 2022 - change the coordinate system to ENU to match the default of Webots R2022a from controller import Robot, DistanceSensor, Motor import numpy as np #------------------------------------------------------- # Initialize variables MAX_SPEED = 6.28 # create the Robot instance. robot = Robot() # get the time step of the current world. timestep = int(robot.getBasicTimeStep()) # [ms] delta_t = robot.getBasicTimeStep() / 1000.0 # [s] # states states = ['forward', 'turn_right', 'turn_left'] current_state = states[0] # counter: used to maintain an active state for a number of cycles counter = 0 COUNTER_MAX = 3 # Robot pose # Adjust the initial values to match the initial robot pose in your simulation x = -0.06 # position in x [m] y = 0.436 # position in y [m] phi = 0.0531 # orientation [rad]
class RobotController:
def __init__(self, ros_active=False, robot='wolfgang', do_ros_init=True):
# requires WEBOTS_ROBOT_NAME to be set to "amy" or "rory"
self.ros_active = ros_active
self.robot_node = Robot()
self.walkready = [0] * 20
self.time = 0
self.motors = []
self.sensors = []
self.timestep = int(self.robot_node.getBasicTimeStep())
self.switch_coordinate_system = True
self.is_wolfgang = False
self.pressure_sensors = None
if robot == 'wolfgang':
self.is_wolfgang = True
self.motor_names = [
"RShoulderPitch", "LShoulderPitch", "RShoulderRoll",
"LShoulderRoll", "RElbow", "LElbow", "RHipYaw", "LHipYaw",
"RHipRoll", "LHipRoll", "RHipPitch", "LHipPitch", "RKnee",
"LKnee", "RAnklePitch", "LAnklePitch", "RAnkleRoll",
"LAnkleRoll", "HeadPan", "HeadTilt"
]
self.external_motor_names = self.motor_names
sensor_postfix = "_sensor"
accel_name = "imu accelerometer"
gyro_name = "imu gyro"
camera_name = "camera"
pressure_sensor_names = [
"llb", "llf", "lrf", "lrb", "rlb", "rlf", "rrf", "rrb"
]
self.pressure_sensors = []
for name in pressure_sensor_names:
sensor = self.robot_node.getDevice(name)
sensor.enable(30)
self.pressure_sensors.append(sensor)
elif robot == 'darwin':
self.motor_names = [
"ShoulderR", "ShoulderL", "ArmUpperR", "ArmUpperL",
"ArmLowerR", "ArmLowerL", "PelvYR", "PelvYL", "PelvR", "PelvL",
"LegUpperR", "LegUpperL", "LegLowerR", "LegLowerL", "AnkleR",
"AnkleL", "FootR", "FootL", "Neck", "Head"
]
self.external_motor_names = [
"RShoulderPitch", "LShoulderPitch", "RShoulderRoll",
"LShoulderRoll", "RElbow", "LElbow", "RHipYaw", "LHipYaw",
"RHipRoll", "LHipRoll", "RHipPitch", "LHipPitch", "RKnee",
"LKnee", "RAnklePitch", "LAnklePitch", "RAnkleRoll",
"LAnkleRoll", "HeadPan", "HeadTilt"
]
sensor_postfix = "S"
accel_name = "Accelerometer"
gyro_name = "Gyro"
camera_name = "Camera"
elif robot == 'nao':
self.motor_names = [
"RShoulderPitch", "LShoulderPitch", "RShoulderRoll",
"LShoulderRoll", "RElbowYaw", "LElbowYaw", "RHipYawPitch",
"LHipYawPitch", "RHipRoll", "LHipRoll", "RHipPitch",
"LHipPitch", "RKneePitch", "LKneePitch", "RAnklePitch",
"LAnklePitch", "RAnkleRoll", "LAnkleRoll", "HeadYaw",
"HeadPitch"
]
self.external_motor_names = self.motor_names
sensor_postfix = "S"
accel_name = "accelerometer"
gyro_name = "gyro"
camera_name = "CameraTop"
self.switch_coordinate_system = False
elif robot == 'op3':
self.motor_names = [
"ShoulderR", "ShoulderL", "ArmUpperR", "ArmUpperL",
"ArmLowerR", "ArmLowerL", "PelvYR", "PelvYL", "PelvR", "PelvL",
"LegUpperR", "LegUpperL", "LegLowerR", "LegLowerL", "AnkleR",
"AnkleL", "FootR", "FootL", "Neck", "Head"
]
self.external_motor_names = [
"r_sho_pitch", "l_sho_pitch", "r_sho_roll", "l_sho_roll",
"r_el", "l_el", "r_hip_yaw", "l_hip_yaw", "r_hip_roll",
"l_hip_roll", "r_hip_pitch", "l_hip_pitch", "r_knee", "l_knee",
"r_ank_pitch", "l_ank_pitch", "r_ank_roll", "l_ank_roll",
"head_pan", "head_tilt"
]
sensor_postfix = "S"
accel_name = "Accelerometer"
gyro_name = "Gyro"
camera_name = "Camera"
self.switch_coordinate_system = False
# self.robot_node = self.supervisor.getFromDef(self.robot_node_name)
for motor_name in self.motor_names:
self.motors.append(self.robot_node.getDevice(motor_name))
self.motors[-1].enableTorqueFeedback(self.timestep)
self.sensors.append(
self.robot_node.getDevice(motor_name + sensor_postfix))
self.sensors[-1].enable(self.timestep)
self.accel = self.robot_node.getDevice(accel_name)
self.accel.enable(self.timestep)
self.gyro = self.robot_node.getDevice(gyro_name)
self.gyro.enable(self.timestep)
if self.is_wolfgang:
self.accel_head = self.robot_node.getDevice(
"imu_head accelerometer")
self.accel_head.enable(self.timestep)
self.gyro_head = self.robot_node.getDevice("imu_head gyro")
self.gyro_head.enable(self.timestep)
self.camera = self.robot_node.getDevice(camera_name)
self.camera.enable(self.timestep)
if self.ros_active:
if do_ros_init:
rospy.init_node("webots_ros_interface", argv=['clock:=/clock'])
self.l_sole_frame = rospy.get_param("~l_sole_frame", "l_sole")
self.r_sole_frame = rospy.get_param("~r_sole_frame", "r_sole")
self.camera_optical_frame = rospy.get_param(
"~camera_optical_frame", "camera_optical_frame")
self.head_imu_frame = rospy.get_param("~head_imu_frame",
"imu_frame_2")
self.imu_frame = rospy.get_param("~imu_frame", "imu_frame")
self.pub_js = rospy.Publisher("joint_states",
JointState,
queue_size=1)
self.pub_imu = rospy.Publisher("imu/data_raw", Imu, queue_size=1)
self.pub_imu_head = rospy.Publisher("imu_head/data",
Imu,
queue_size=1)
self.pub_cam = rospy.Publisher("camera/image_proc",
Image,
queue_size=1)
self.pub_cam_info = rospy.Publisher("camera/camera_info",
CameraInfo,
queue_size=1,
latch=True)
self.pub_pres_left = rospy.Publisher("foot_pressure_left/filtered",
FootPressure,
queue_size=1)
self.pub_pres_right = rospy.Publisher(
"foot_pressure_right/filtered", FootPressure, queue_size=1)
self.cop_l_pub_ = rospy.Publisher("cop_l",
PointStamped,
queue_size=1)
self.cop_r_pub_ = rospy.Publisher("cop_r",
PointStamped,
queue_size=1)
rospy.Subscriber("DynamixelController/command", JointCommand,
self.command_cb)
# publish camera info once, it will be latched
self.cam_info = CameraInfo()
self.cam_info.header.stamp = rospy.Time.from_seconds(self.time)
self.cam_info.header.frame_id = self.camera_optical_frame
self.cam_info.height = self.camera.getHeight()
self.cam_info.width = self.camera.getWidth()
f_y = self.mat_from_fov_and_resolution(
self.h_fov_to_v_fov(self.camera.getFov(), self.cam_info.height,
self.cam_info.width), self.cam_info.height)
f_x = self.mat_from_fov_and_resolution(self.camera.getFov(),
self.cam_info.width)
self.cam_info.K = [
f_x, 0, self.cam_info.width / 2, 0, f_y, self.cam_info.height / 2,
0, 0, 1
]
self.cam_info.P = [
f_x, 0, self.cam_info.width / 2, 0, 0, f_y,
self.cam_info.height / 2, 0, 0, 0, 1, 0
]
if self.ros_active:
self.pub_cam_info.publish(self.cam_info)
def mat_from_fov_and_resolution(self, fov, res):
return 0.5 * res * (math.cos((fov / 2)) / math.sin((fov / 2)))
def h_fov_to_v_fov(self, h_fov, height, width):
return 2 * math.atan(math.tan(h_fov * 0.5) * (height / width))
def step_sim(self):
self.time += self.timestep / 1000
self.robot_node.step(self.timestep)
def step(self):
self.step_sim()
if self.ros_active:
self.publish_imu()
self.publish_joint_states()
self.publish_camera()
self.publish_pressure()
def command_cb(self, command: JointCommand):
for i, name in enumerate(command.joint_names):
try:
motor_index = self.external_motor_names.index(name)
self.motors[motor_index].setPosition(command.positions[i])
if command.velocities[i] == -1:
self.motors[motor_index].setVelocity(
self.motors[motor_index].getMaxVelocity())
else:
self.motors[motor_index].setVelocity(command.velocities[i])
self.motors[motor_index].setAcceleration(
command.accelerations[i])
except ValueError:
print(f"invalid motor specified ({name})")
def set_head_tilt(self, pos):
self.motors[-1].setPosition(pos)
def set_arms_zero(self):
positions = [
-0.8399999308200574, 0.7200000596634105, -0.3299999109923385,
0.35999992683575216, 0.5099999812500172, -0.5199999789619728
]
for i in range(0, 6):
self.motors[i].setPosition(positions[i])
def get_joint_state_msg(self):
js = JointState()
js.name = []
js.header.stamp = rospy.Time.from_seconds(self.time)
js.position = []
js.effort = []
for i in range(len(self.sensors)):
js.name.append(self.external_motor_names[i])
value = self.sensors[i].getValue()
js.position.append(value)
js.effort.append(self.motors[i].getTorqueFeedback())
return js
def publish_joint_states(self):
self.pub_js.publish(self.get_joint_state_msg())
def get_imu_msg(self, head=False):
msg = Imu()
msg.header.stamp = rospy.Time.from_seconds(self.time)
if head:
msg.header.frame_id = self.head_imu_frame
else:
msg.header.frame_id = self.imu_frame
# change order because webots has different axis
if head:
accel_vels = self.accel_head.getValues()
msg.linear_acceleration.x = accel_vels[2]
msg.linear_acceleration.y = -accel_vels[0]
msg.linear_acceleration.z = -accel_vels[1]
else:
accel_vels = self.accel.getValues()
msg.linear_acceleration.x = accel_vels[0]
msg.linear_acceleration.y = accel_vels[1]
msg.linear_acceleration.z = accel_vels[2]
if head:
gyro_vels = self.gyro_head.getValues()
msg.angular_velocity.x = gyro_vels[2]
msg.angular_velocity.y = -gyro_vels[0]
msg.angular_velocity.z = -gyro_vels[1]
else:
gyro_vels = self.gyro.getValues()
msg.angular_velocity.x = gyro_vels[0]
msg.angular_velocity.y = gyro_vels[1]
msg.angular_velocity.z = gyro_vels[2]
return msg
def publish_imu(self):
self.pub_imu.publish(self.get_imu_msg(head=False))
self.pub_imu_head.publish(self.get_imu_msg(head=True))
def publish_camera(self):
img_msg = Image()
img_msg.header.stamp = rospy.Time.from_seconds(self.time)
img_msg.header.frame_id = self.camera_optical_frame
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 get_image(self):
return self.camera.getImage()
def get_pressure_message(self):
current_time = rospy.Time.from_sec(self.time)
left_pressure = FootPressure()
left_pressure.header.stamp = current_time
left_pressure.left_back = self.pressure_sensors[0].getValues()[2]
left_pressure.left_front = self.pressure_sensors[1].getValues()[2]
left_pressure.right_front = self.pressure_sensors[2].getValues()[2]
left_pressure.right_back = self.pressure_sensors[3].getValues()[2]
right_pressure = FootPressure()
left_pressure.header.stamp = current_time
right_pressure.left_back = self.pressure_sensors[4].getValues()[2]
right_pressure.left_front = self.pressure_sensors[5].getValues()[2]
right_pressure.right_front = self.pressure_sensors[6].getValues()[2]
right_pressure.right_back = self.pressure_sensors[7].getValues()[2]
# compute center of pressures of the feet
pos_x = 0.085
pos_y = 0.045
# we can take a very small threshold, since simulation gives more accurate values than reality
threshold = 1
cop_l = PointStamped()
cop_l.header.frame_id = self.l_sole_frame
cop_l.header.stamp = current_time
sum = left_pressure.left_back + left_pressure.left_front + left_pressure.right_front + left_pressure.right_back
if sum > threshold:
cop_l.point.x = (left_pressure.left_front +
left_pressure.right_front -
left_pressure.left_back -
left_pressure.right_back) * pos_x / sum
cop_l.point.x = max(min(cop_l.point.x, pos_x), -pos_x)
cop_l.point.y = (left_pressure.left_front + left_pressure.left_back
- left_pressure.right_front -
left_pressure.right_back) * pos_y / sum
cop_l.point.y = max(min(cop_l.point.x, pos_y), -pos_y)
else:
cop_l.point.x = 0
cop_l.point.y = 0
cop_r = PointStamped()
cop_r.header.frame_id = self.r_sole_frame
cop_r.header.stamp = current_time
sum = right_pressure.right_back + right_pressure.right_front + right_pressure.right_front + right_pressure.right_back
if sum > threshold:
cop_r.point.x = (right_pressure.left_front +
right_pressure.right_front -
right_pressure.left_back -
right_pressure.right_back) * pos_x / sum
cop_r.point.x = max(min(cop_r.point.x, pos_x), -pos_x)
cop_r.point.y = (right_pressure.left_front +
right_pressure.left_back -
right_pressure.right_front -
right_pressure.right_back) * pos_y / sum
cop_r.point.y = max(min(cop_r.point.x, pos_y), -pos_y)
else:
cop_r.point.x = 0
cop_r.point.y = 0
return left_pressure, right_pressure, cop_l, cop_r
def publish_pressure(self):
left, right, cop_l, cop_r = self.get_pressure_message()
self.pub_pres_left.publish(left)
self.pub_pres_right.publish(right)
self.cop_l_pub_.publish(cop_l)
self.cop_r_pub_.publish(cop_r)
"""An open loop controller to make a robot follow a square motion path."""
from controller import Robot
from math import pi # from the python "math" module import the "pi" variable
# Create an instance of the robot:
robot = Robot()
# get the time step of the simulation (See WorldInfo > basicTimeStep
# in the scene tree):
timestep = int(robot.getBasicTimeStep()) # in milliseconds
# Create an object for each of the robots wheels:
leftWheel = robot.getMotor('left wheel motor')
rightWheel = robot.getMotor('right wheel motor')
# some key robot geometries and equations of motion calculations:
wheel_diameter = 0.05 # meters
robot_diameter = 0.09 # meters (the distance between the two wheels)
# We can find out the maximum velocity of our robots wheels by
# accessing the .getMaxVelocity() function from either of our wheels
# (it is the same for both)
max_velocity = leftWheel.getMaxVelocity() # radians per second
# print this to the console so that we can see what it is:
print('The Robot max wheel velocity is {} rad/s'.format(max_velocity))
wheel_circumference = pi * wheel_diameter # meters
# calculate the time required to move 0.5 meters in a straight line:
fwd_displacement = 0.5 / wheel_circumference # wheel revolutions
# one wheel revolution is 2*pi radians, so:
fwd_displacement = 2 * pi * fwd_displacement # in radians
# time required to travel this far:
class Controller():
jointNames = [
# coxa, femur, tibia
'c1',
'f1',
't1', # LEG 1
'c2',
'f2',
't2', # LEG 2
'c3',
'f3',
't3', # LEG 3
'c4',
'f4',
't4', # LEG 4
'c5',
'f5',
't5', # LEG 5
'c6',
'f6',
't6'
] # LEG 6
touchNames = ['f1', 'f2', 'f3', 'f4', 'f5', 'f6']
def __init__(self):
# Initialize the Webots Supervisor.
self.robot = Robot()
self.timeStep = int(4 * self.robot.getBasicTimeStep())
self.keyboard = self.robot.getKeyboard()
# Define list for motors and position sensors
self.motors = []
self.position_sensors = []
self.touch_sensors = []
# Initialise the motors and position sensors (could be moved into __init__ into a single for-loop)
self.init_motors()
self.init_positional_sensors()
self.init_touching_sensors()
def init_motors(self):
for name in Controller.jointNames:
motor = self.robot.getMotor(name + '_motor')
# motor.setPosition(float('inf'))
# motor.setVelocity(0)
self.motors.append(motor)
def init_positional_sensors(self):
for name in Controller.jointNames:
positional_sensor = self.robot.getPositionSensor(
name + '_position_sensor')
positional_sensor.enable(SENSOR_SAMPLE_PERIOD)
self.position_sensors.append(positional_sensor)
def init_touching_sensors(self):
for name in Controller.jointNames[1::3]:
touching_sensor = self.robot.getTouchSensor(name + '_touch_sensor')
touching_sensor.enable(SENSOR_SAMPLE_PERIOD)
self.touch_sensors.append(touching_sensor)
# Test initialiser before I got the above version working.
#def init_touching_sensors(self):
# for name in Controller.touchNames:
# touching_sensor = self.robot.getTouchSensor(name + '_touch_sensor')
# touching_sensor.enable(SENSOR_SAMPLE_PERIOD)
# self.touch_sensors.append(touching_sensor)
def positionAll(self, positions):
while self.robot.step(self.timeStep) != 1:
for motor, position in zip(self.motors, positions):
motor.setPosition(position)
C.reachedAllPos(positions)
break
def positionN(self, id_list, positions):
id_list = [x - 1 for x in id_list]
while self.robot.step(self.timeStep) != 1:
for i in range(len(id_list)):
j = id_list[i]
self.motors[j].setPosition(positions[i])
C.reachedNPos(id_list, positions)
break
def readPos(self):
all_positions = []
valuePos = []
while self.robot.step(self.timeStep) != 1:
for i in range(len(self.jointNames)):
valuePos = self.position_sensors[i].getValue()
all_positions.append(valuePos)
#print(all_positions) # just for debugging
return all_positions
def readTouch(self):
all_touches = []
valueTouch = []
while self.robot.step(self.timeStep) != 1:
for i in range(len(self.jointNames[1::3])):
valueTouch = self.touch_sensors[i].getValue()
all_touches.append(valueTouch)
#print(all_touches) # just for debugging
return all_touches
def reachedAllPos(self, positions):
all_positions = []
#while self.robot.step(self.timeStep) != 1:
while all_positions != positions:
all_positions = [round(x, 2) for x in C.readPos()]
positions = [round(y, 2) for y in positions]
#print(all_positions) # just for debugging
#print(positions) # just for debugging
def reachedNPos(self, id_list, positions):
#print('start id list:', id_list)
#print('start pos list:', positions)
temp_positions = []
all_positions = [0] * len(id_list)
#while self.robot.step(self.timeStep) != 1:
while all_positions != positions:
temp_positions = [round(x, 2) for x in C.readPos()]
positions = [round(y, 2) for y in positions]
for i in range(len(id_list)):
j = id_list[i]
all_positions[i] = temp_positions[j]
#all_positions = [0.0 if i == -0.0 else i for i in all_positions] # for fixing -0 value, but not needed
#print('all_positions:', all_positions) # just for debugging
#print(' positions:', positions) # just for debugging
return
def walk(self):
standUp()
# different types of gaits
#tripod_gait_test_for_lars(0,10,10,1)
#tripodGait(0, 20, 10, 10)
#waveGait(0, 20, 10, 1)
#rippleGait(0, 40, 10, 5)
# ripple gait manual imported
#singleLeg(0, 20, 10, 0, 0, 0, 1)
#singleLeg(0, 20, 10, 0, 0, 0, 4)
#singleLeg(0, -40/2, 0, 0, 0, 0, 2)
#singleLeg(0, -40/2, 0, 0, 0, 0, 5)
#singleLeg(0, -40/2, 0, 0, 0, 0, 3)
#singleLeg(0, -40/2, 0, 0, 0, 0, 6)
#print('0')
#singleLeg(0, 0, -10, 0, 0, 0, 1)
#print('1')
#singleLeg(0, 0, 40, 0, 0, 0, 4)
# some other translations
#translationZ(-50)
#parallelGait(0, 0, 3, 0, 0, 0)
self.robot.step(1000)
# reading and printing for debugging
all_positions = C.readPos()
K.printForward(all_positions)
print(K.rad_to_step(all_positions))
WHEEL_BACKWARD = -1
state = "send_pose" # End with the more complex feedback control method!
sub_state = "bearing" # TODO: It may be helpful to use sub_state to designate operation modes within the "turn_drive_turn_control" state
# create the Robot instance.
# csci3302_lab3_supervisor.init_supervisor()
# robot = csci3302_lab3_supervisor.supervisor
robot = Robot()
EPUCK_MAX_WHEEL_SPEED = 0.12880519 # Unnecessarily precise ePuck speed in m/s. REMINDER: motor.setVelocity() takes ROTATIONS/SEC as param, not m/s.
EPUCK_AXLE_DIAMETER = 0.053 # ePuck's wheels are 53mm apart.
EPUCK_WHEEL_RADIUS = 0.0205 # ePuck's wheels are 0.041m in diameter.
# get the time step of the current world.
SIM_TIMESTEP = int(robot.getBasicTimeStep())
# Initialize Motors
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)
def update_odometry(left_wheel_direction, right_wheel_direction, time_elapsed):
'''
Given the amount of time passed and the direction each wheel was rotating,
update the robot's pose information accordingly
'''
class Controller():
servos = []
psNames = []
ps = []
psValues = []
def __init__(self):
self.robot = Robot()
self.timeStep = int(4 * self.robot.getBasicTimeStep())
self.keyboard = self.robot.getKeyboard()
self.init_servos()
self.init_positional_sensors()
def init_servos(self):
# coxa, femur, tibia
for servosName in (
'c1_ser',
'f1_ser',
't1_ser', # LEG 1
'c2_ser',
'f2_ser',
't2_ser', # LEG 2
'c3_ser',
'f3_ser',
't3_ser', # LEG 3
'c4_ser',
'f4_ser',
't4_ser', # LEG 4
'c5_ser',
'f5_ser',
't5_ser', # LEG 5
'c6_ser',
'f6_ser',
't6_ser',
): # LEG 6
servos = self.robot.getMotor(servosName)
#servos.setPosition(float('inf'))
#servos.setVelocity(0)
self.servos.append(servos)
# def init_positional_sensors(self):
# # coxa, femur, tibia
# for positionsensorName in ('c1_pos', 'f1_pos', 't1_pos', # LEG 1
# 'c2_pos', 'f2_pos', 't2_pos', # LEG 2
# 'c3_pos', 'f3_pos', 't3_pos', # LEG 3
# 'c4_pos', 'f4_pos', 't4_pos', # LEG 4
# 'c5_pos', 'f5_pos', 't5_pos', # LEG 5
# 'c6_pos', 'f6_pos', 't6_pos',): # LEG 6
# positional_sensor = self.robot.getPositionSensor(positionsensorName)
# positional_sensor.enable(POSITION_SENSOR_SAMPLE_PERIOD)
# self.position_sensors.append(positional_sensor)
def init_positional_sensors(self):
psNames = [
'c1_pos',
'f1_pos',
't1_pos', # LEG 1
'c2_pos',
'f2_pos',
't2_pos', # LEG 2
'c3_pos',
'f3_pos',
't3_pos', # LEG 3
'c4_pos',
'f4_pos',
't4_pos', # LEG 4
'c5_pos',
'f5_pos',
't5_pos', # LEG 5
'c6_pos',
'f6_pos',
't6_pos',
] # LEG 6
for i in range(18):
self.ps.append(self.robot.getPositionSensor(psNames[i]))
self.ps[i].enable(POSITION_SENSOR_SAMPLE_PERIOD)
def run(self):
positions = [
1.0, 1.0, 2, 1.0, 1.0, 2, 1.0, 1.0, 2, 1.0, 1.0, 2, 1.0, 1.0, 2,
1.0, 1.0, 2
]
while self.robot.step(self.timeStep) != 1:
for servos, position in zip(self.servos, positions):
servos.setPosition(position)
break
def read(self):
for i in range(18):
self.psValues.append(self.ps[i].getValue())
return self.psValues
