def __init__(self):
"""
Initializes the controller's motor which behaves like an oscillator and creates two connectors.
"""
Robot.__init__(self)
# Derive the oscillator id from its name, which is usually s.th. like "module_1"
self.id = int(self.getName()[-1]) - 1
# Get access to the motor
self.motor = self.getMotor('motor')
# Get access to the connectors
self.rear_connector = self.getConnector('rear_connector')
self.front_connector = self.getConnector('front_connector')
# The configuration of the oscillator is stored in a file which can be changed by a supervisor
self.config = {}
try:
time.sleep(0.5)
self.load_configuration()
except:
self.config = {
'active': False
}
def __init__(self):
Robot.__init__(self)
# You should insert a getDevice-like function in order to get the
# instance of a device of the robot. Something like:
# led = self.getLed('ledname')
print "Program Start"
# ---------------------------------------------
# ------------- Robot Start Setup -------------
# ---------------------------------------------
def runCommand(self, key):
self.RHP = self.getServo('RHP')
#self.setPosition(RHP, 45.2)
# ---------------------------------------------
# ------------- Robot End Setup ---------------
# ---------------------------------------------
# from http://wiki.python.org/moin/UdpCommunication
UDP_IP="127.0.0.1"
UDP_PORT=11000
sock = socket.socket( socket.AF_INET, # internet
socket.SOCK_DGRAM ) # UDP
sock.bind( (UDP_IP,UDP_PORT) )
# Main loop
while True:
print "waiting for UDP"
rxData, addr = sock.recvfrom( 1024 ) # buffer size is 1024 bytes
print "received message Length:", len(rxData)
print "Data 1:", ord(rxData[1]) #print "Data 1:", data;
N = ord(rxData[2]) # message length
Nn = len(rxData) # received message length
NN = Nn - N # difference between received length and message length (should be 4)
#Servo.setPosition("RHP",20)
#for i in range(0,N)
# Servo.setPosition("RHP",20)
self.RHP.setPosition(25.2)
def __init__(self):
Robot.__init__(self)
#timestep info
self.timeStep = 40
self.f = finfo(float)
#Choose Humanoid Robot
self.bot = Nao(self, self.timeStep)
#Keep track of starting and stopping of walk cycle
self.idle = True
self.stepStop = True
self.stepStart = True
self.stepCount = 0
self.stepSign = -1 # -1 for right support, +1 for left support
self.tStep = 0.5
self.upPhase = 0.3
self.downPhase = 0.8
self.t0 = self.getTime()
self.t = 0
self.dt = 0
self.tPhase = 0.
self.phShift = 0.
self.key = 0
# define the motion which is currently playing
self.currentlyPlaying = None
self.start()
#Print Menu
self.bot.printHelp()
self.walkForward()
while not self.key:
self.key = self.keyboardGetKey()
self.bot.update()
self.simulationStep()
print("turn right to wall", psValues[2])
leftMotor.setVelocity(leftSpeed)
rightMotor.setVelocity(rightSpeed)
robot.step(timestep)
updatePsValues()
if psValues[2] > closedDistance:
isUpdate = 1
closedDistance = psValues[2]
elif psValues[2] < Dwth:
continue
else:
break
# create the Robot instance.
robot = Robot()
gps = GPS(name="e-puck-gps")
# get the time step of the current world.
timestep = 64
MAX_SPEED = 6.28
Dwth = 160
w_d2 = 0.7
time90 = 12
# You should insert a getDevice-like function in order to get the
# instance of a device of the robot. Something like:
# motor = robot.getMotor('motorname')
# ds = robot.getDistanceSensor('dsname')
# ds.enable(timestep)
ps = []
psNames = ['ps0', 'ps1', 'ps2', 'ps3', 'ps4', 'ps5', 'ps6', 'ps7']
"""epuck_avoid_object controller."""
# You may need to import some classes of the controller module. Ex:
# from controller import Robot, Motor, DistanceSensor
from controller import Robot. DistanceSensor, Motor
# get the time step of the current world.
#timestep = int(robot.getBasicTimeStep())
TIME_STEP = 64
NUM_DISTSENS = 8
MAX_SPEED = 6.28
# create the Robot instance.
robot = Robot()
# You should insert a getDevice-like function in order to get the
# instance of a device of the robot. Something like:
# motor = robot.getMotor('motorname')
# ds = robot.getDistanceSensor('dsname')
# ds.enable(timestep)
# initialise devices
ps = []
psNames = [
'ps0', 'ps1', 'ps2', 'ps3',
'ps4', 'ps5', 'ps6', 'ps7'
]
for i = range(NUM_DISTSENS):
ps.append(robot.getDistanceSensor(psNames[i]))
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)
# Multiply -1 then +1
pose_y = 1.75
pose_theta = 3 * math.pi / 2
# Constants to help with the Odometry update
WHEEL_FORWARD = 1
WHEEL_STOPPED = 0
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)
emitter.send(message)
elif code == "Stall":
message = bytes("All Stall Hound 0", 'utf-8')
emitter.send(message)
else:
message = bytes(code, 'utf-8')
emitter.send(message)
# Needs to be filled up with further code to get full implementation
return 1
#Max angular rotation of Hound wheels
MAX_SPEED = 20
cheated = False
#Initializes Robot and its parts
robot = Robot()
TIME_STEP = int(robot.getBasicTimeStep())
print(TIME_STEP)
emitter = robot.getEmitter('emitter')
receiver = robot.getReceiver('receiver')
receiver.enable(TIME_STEP)
wheels = []
wheelsNames = ['wheel1', 'wheel2', 'boop']
for i in range(3):
wheels.append(robot.getMotor(wheelsNames[i]))
if i == 2:
wheels[i].setPosition(0.0)
else:
wheels[i].setPosition(float('inf'))
wheels[i].setVelocity(0.0)
# Maximum range of the sensors.
# Don't change this value.
MAX_SENSOR_RANGE = 5
# Minimum distance for an obstacle to be considered as touching the robot.
HIT_WALL_DISTANCE = 0.01
# States used by our state machine.
ST_INITIAL = 0
ST_FORWARD = 1
ST_RIGHT = 2
ST_BACKWARD = 3
# Recovers robot instance.
robot = Robot()
# Recovers basic timestep.
timestep = int(robot.getBasicTimeStep())
# Initializes the wheels.
leftWheel = robot.getMotor('left wheel')
rightWheel = robot.getMotor('right wheel')
leftWheel.setVelocity(0)
rightWheel.setVelocity(0)
leftWheel.setPosition(float('inf'))
rightWheel.setPosition(float('inf'))
# Initializes the sensors.
sonarSensor = [None] * 16
for i in range(0, 16):
elif force < -120:
motorLeft.setVelocity(3)
motorRight.setVelocity(10)
# go straight
else:
motorLeft.setVelocity(10)
motorRight.setVelocity(10)
def stopMotors():
motorLeft.setVelocity(0)
motorRight.setVelocity(0)
robot = Robot()
# get the time step of the current world.
timestep = int(robot.getBasicTimeStep())
#setup motors
motorLeft = robot.getMotor('wheel_left')
motorRight = robot.getMotor('wheel_right')
motorLeft.setPosition(float('inf'))
motorRight.setPosition(float('inf'))
motorLeft.setVelocity(0.0)
motorRight.setVelocity(0.0)
#setup sensors
ds = robot.getLidar('lidar')
ds.enable(timestep)
from controller import Robot
# Get pointer to the robot.
robot = Robot()
# Get pointer to each wheel of our robot.
leftWheel = robot.getMotor('left wheel')
rightWheel = robot.getMotor('right wheel')
#Get right wheel sensor
rightWheelSensor = robot.getPositionSensor('right wheel sensor')
rightWheelSensor.enable(16)
#Get left wheel sensor
leftWheelSensor = robot.getPositionSensor('left wheel sensor')
leftWheelSensor.enable(16)
# Текущее значение датчика положения колес
sensorValue = 0
# Радиус колеса
wheel_radius = 0.195 / 2
MAX_SPEED = 5.24
# Repeat the following 4 times (once for each side).
for i in range(0, 4):
# First set both wheels to go forward, so the robot goes straight.
leftWheel.setPosition(1000)
rightWheel.setPosition(1000)
robot.step(16)
# Двигаться пока текущее положение не будет равно двум метрам + положение пред. вершины
while ((rightWheelSensor.getValue() * wheel_radius) < (2.0 + sensorValue)):
# Снижаем скорость почти достигнув вершины
if ((rightWheelSensor.getValue() * wheel_radius) >
from controller import Robot
from robot1 import MyRobot1
from robot2 import MyRobot2
from robot3 import MyRobot3
robot = Robot()
name = robot.getName()
robot_number = int(name[1])
if robot_number == 1:
robot_controller = MyRobot1(robot)
elif robot_number == 2:
robot_controller = MyRobot2(robot)
else:
robot_controller = MyRobot3(robot)
robot_controller.run()
from controller import Robot, Motor, DistanceSensor, LightSensor, GPS, Compass, Receiver, Emitter
import math
import struct
TIME_STEP = 32
MAX_SPEED = 10
# create a robot
robot = Robot()
# get devices
us_right = robot.getDevice("us_right")
us_left = robot.getDevice("us_left")
ir = robot.getDevice("ir")
light_sensor = robot.getDevice("TEPT4400")
light_sensor_l = robot.getDevice("TEPT4400_left")
light_sensor_r = robot.getDevice("TEPT4400_right")
motor_left = robot.getDevice("Wheel_L")
motor_right = robot.getDevice("Wheel_R")
arm_left = robot.getDevice("Arm_L")
arm_right = robot.getDevice("Arm_R")
compass = robot.getDevice("compass")
gps = robot.getDevice("gps")
receiver = robot.getDevice("receiver")
emitter = robot.getDevice("Emitter")
#enable devices
us_right.enable(TIME_STEP)
us_left.enable(TIME_STEP)
ir.enable(TIME_STEP)
light_sensor.enable(TIME_STEP)
light_sensor_l.enable(TIME_STEP)
light_sensor_r.enable(TIME_STEP)
compass.enable(TIME_STEP)
gps.enable(TIME_STEP)
supervisor = True
else:
# List of cells that were visited by other robot
data = [(int(cell[0]), int(cell[1])) for cell in message[1:]]
return data, supervisor
def synchronize(update):
for (i, j) in update:
GRID[i, j] += CHARGE_INCREMENT
# Create the Robot instance.
robot = Robot()
# Get ID
ID = int(robot.getName().split("_")[1])
#GETS THE DISPLAY WORKS!
# disp = robot.getDisplay("display")
# print("DISP WIDTH")
# print(disp.getWidth())
# disp.setColor(0xFF0FFF)
# disp.drawRectangle()
# get the time step of the current world.
timestep = int(robot.getBasicTimeStep())
print("Timestep:", timestep)
"""main controller."""
# You may need to import some classes of the controller module. Ex:
# from controller import Robot, Motor, DistanceSensor
from controller import Robot, Motor, DistanceSensor, Camera, RangeFinder
import cv2
import numpy as np
hizi = 6.28
maxMesafe = 1024
#sensörün mesafede nasıl algı m
min_uzaklık = 1.0
# create the Robot instance.
robot = Robot()
# get the time step of the current world.
timestep = int(robot.getBasicTimeStep())
#Camera ve RangeFinder getirir
kinectColor = robot.getCamera("kinect color")
kinectRange = robot.getRangeFinder("kinect range")
#camera ve rangefinder başlatıldı
Camera.enable(kinectColor, timestep)
RangeFinder.enable(kinectRange, timestep)
# motorların tagını getirir
#motorları getirir
solMotor = robot.getMotor("left wheel")
left_speed = 3
right_speed = 3
count = 0
elif (left_ir_value > 700 and middle_ir_value > 700
and right_ir_value > 700):
print("Go Reverse")
left_speed = -max_speed * 0.25
count = 0
elif ((left_ir_value < 700 and middle_ir_value < 700
and right_ir_value < 700)
or (right_ir_value > 700 and left_ir_value < 700)):
print("Go left")
print(count)
left_speed = 0
right_speed = 6
count = count + 1
print(count)
if (stopMotors == 1):
left_motor.setVelocity(0)
right_motor.setVelocity(0)
else:
left_motor.setVelocity(left_speed)
right_motor.setVelocity(right_speed)
if __name__ == '__main__':
my_robot = Robot()
run_robot(my_robot)
from controller import Robot, Motor, Emitter, InertialUnit
import time
import numpy as np
import time
node='13_'
actual_id=13
print("Initializing node",node)
def send_message(message):
emitter.send(message.encode('utf-8'))
TIME_STEP = 32
MAX_SPEED = 6.28
# create the Robot instance.
robot = Robot()
# get a handler 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)
rightMotor.setVelocity(0)
emitter = robot.getEmitter('emitter')
receiver = robot.getReceiver('receiver')
receiver.enable(TIME_STEP)
# Simulação extra 03 com robô Pioneer 3AT - Webots R2020a
# Position sensors - encoders
# Python 3.5 na IDE Pycharm - controller <extern>
# By: Jefferson S.Almeida
# Data: 02/07/2020
# **************************************************************
from controller import Robot
from controller import Motor
from controller import PositionSensor
import math
TIME_STEP = 64
MAX_SPEED = 1.2
robot = Robot()
positionLeft = robot.getPositionSensor('left wheel sensor')
positionRight = robot.getPositionSensor('right wheel sensor')
PositionSensor.enable(positionLeft, TIME_STEP)
PositionSensor.enable(positionRight, TIME_STEP)
leftMotor = robot.getMotor('left wheel')
rightMotor = robot.getMotor('right wheel')
leftMotor.setPosition(float('inf'))
rightMotor.setPosition(float('inf'))
leftMotor.setVelocity(0.5 * MAX_SPEED)
rightMotor.setVelocity(0.5 * MAX_SPEED)
from controller import Robot
from controller import Camera
import math
#returns list in string form with number rounded to 2 decimals
def roundedList(myList):
string = "[ "
for i in myList:
string += str(round(i, 2)) + ", "
string += "]"
return string
recognitionColor = [0, 0, 0]
timeStep = 32
myRobot = Robot()
camera = myRobot.getCamera("camera")
#camera.enable(timeStep)
camera.recognitionEnable(timeStep)
while myRobot.step(timeStep) != -1:
objects = camera.getRecognitionObjects()
for obj in objects:
if obj.get_colors() == recognitionColor:
print("Recognized object!")
print("Position: " + roundedList(obj.get_position()))
break
"""Sample Webots controller for the square path benchmark."""
from controller import Robot
# Get pointer to the robot.
robot = Robot()
# Get pointer to each wheel of our robot.
leftWheel = robot.getMotor('left wheel')
rightWheel = robot.getMotor('right wheel')
# Repeat the following 4 times (once for each side).
for i in range(0, 4):
# First set both wheels to go forward, so the robot goes straight.
leftWheel.setPosition(1000)
rightWheel.setPosition(1000)
# Wait for the robot to reach a corner.
robot.step(3900)
# Then, set the right wheel backward, so the robot will turn right.
leftWheel.setPosition(1000)
rightWheel.setPosition(-1000)
# Wait until the robot has turned 90 degrees clockwise.
robot.step(480)
# Stop the robot when path is completed, as the robot performance
# is only computed when the robot has stopped.
leftWheel.setVelocity(0)
rightWheel.setVelocity(0)
"""wheels1 controller."""
# You may need to import some classes of the controller module. Ex:
# from controller import Robot, Motor, DistanceSensor
from controller import Robot
# create the Robot instance.
robot = Robot()
# get the time step of the current world.
timestep = int(robot.getBasicTimeStep())
leftWheel = robot.getMotor('FrontLeftWheel')
rightWheel = robot.getMotor('FrontRightWheel')
leftArmMotor = robot.getMotor('FrontLeftArm')
rightArmMotor = robot.getMotor('FrontRightArm')
leftWheel.setPosition(float('inf'))
rightWheel.setPosition(float('inf'))
leftWheel.setVelocity(0.6)
rightWheel.setVelocity(0.6)
leftArmMotor.setPosition(float(0.3))
rightArmMotor.setPosition(float(0.3))
leftArmMotor.setVelocity(2)
rightArmMotor.setVelocity(2)
# You should insert a getDevice-like function in order to get the
def turn(rob, c, l, r, direction):
angle = getAngle(c)
targetComp = getTurnDirection(getComVal(c), direction)
while rob.step(TIME_STEP) != -1 and not isStraight(angle, targetComp):
angle = getAngle(c)
if not isStraight(angle, targetComp):
l.setVelocity(MAX_SPEED)
r.setVelocity(-1 * MAX_SPEED)
#Create Optimizer instance
opt = Optimizer()
write = not opt.doesFileExist()
#print(opt.data)
#Create the Robot instance
robot = Robot()
com = robot.getCompass('compass')
com.enable(TIME_STEP)
acc = robot.getAccelerometer('accelerometer')
acc.enable(TIME_STEP)
currentPos = [0, 0]
#Initialize devices
ps = []
psNames = [
'ps0', 'ps1', 'ps2', 'ps3',
'ps4', 'ps5', 'ps6', 'ps7'
]
#!/usr/bin/python3
from controller import Robot, Motor
from controller import GPS
from controller import Emitter
import numpy as np
# Constants
l_dist_sensor_angle = -np.pi / 4 # angle of left dist. sensor rel. to robot
r_dist_sensor_angle = np.pi / 4 # angle of right dist. sensor rel. to robot
wheel_radius = 0.04
# create the Robot instance.
robot = Robot()
# get the time step of the current world.
timestep = int(robot.getBasicTimeStep())
# get the motor devices
leftMotor = robot.getDevice('wheel1')
rightMotor = robot.getDevice('wheel2')
# set the target position of the motors
leftMotor.setPosition(float("Inf"))
rightMotor.setPosition(float("Inf"))
leftMotor.setVelocity(0.0)
rightMotor.setVelocity(-0.0)
main_gps = robot.getDevice('gps_main')
main_gps.enable(timestep)
compass = robot.getDevice('compass')
compass.enable(timestep)
"""PAUL_demo1 controller."""
from controller import Robot, Motor, DistanceSensor
# create the Robot instance.
robot = Robot()
# get the time step of the current world.
timestep = int(robot.getBasicTimeStep())
# initialise the 3 wheels
wheels = []
wheelsNames = ['wheel1', 'wheel2', 'wheel3']
for i in range(3):
wheels.append(robot.getDevice(wheelsNames[i]))
wheels[i].setPosition(float('inf'))
wheels[i].setVelocity(0.0) # unit: rad/s
# initialise distance sensors
ds_top = robot.getDevice('ds_top')
ds_bottom = robot.getDevice('ds_bottom')
ds_top.enable(timestep)
ds_bottom.enable(timestep)
# stores current direction & state
goingUp = True
# changingDir = False
def setSpeed(v):
for i in range(3):
wheels[i].setVelocity(v)
import time
import threading
import numpy as np
import OpenCVClient as client
from controller import Robot
from OpenCVServer import OpenCVServer
TIME_STEP = 64
print("Starting, getting camera...")
bot = Robot()
# Get the camera device, enable it, and store its width and height
camera = bot.getCamera("camera")
camera.enable(TIME_STEP)
width = camera.getWidth()
height = camera.getHeight()
print("Done.")
def start_cv_client():
time.sleep(10)
while True:
try:
client.main()
break
except:
print("Restarting OpenCVClient...")
time.sleep(3)
from controller import Robot
import struct
robot = Robot()
timeStep = 32
emitter = robot.getEmitter('emitter')
while robot.step(timeStep) != -1:
message = struct.pack('i i i c', 0, 0, 0, b'E')
emitter.send(message)
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)
"""Sample Webots controller for the inverted pendulum benchmark."""
from controller import Robot
import math
# Get pointer to the robot.
robot = Robot()
# 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 = 31.4
KI = 100.5
KD = 0
integral = 0.0
previous_position = 0.0
import numpy as np
from controller import Robot, Motor
#-------General-----------
TIME_STEP = 64
MAX_SPEED = 6.28
#distance to the wall
dist = 12
# create the Robot instance.
robot = Robot()
#See Thymio specific Webots names: https://cyberbotics.com/doc/guide/thymio2#thymio2-wbt
#See "robot" specific functions: https://cyberbotics.com/doc/reference/robot?tab=python#getlightsensor
#Motors
leftMotor = robot.getMotor('motor.left')
rightMotor = robot.getMotor('motor.right')
leftMotor.setPosition(float('inf'))
rightMotor.setPosition(float('inf'))
leftMotor.setVelocity(0)
rightMotor.setVelocity(0)
#see functions of DistanceSensor: https://cyberbotics.com/doc/reference/distancesensor#wb_distance_sensor_enable
#sensors
sens0 = robot.getDistanceSensor("prox.horizontal.0")
sens1 = robot.getDistanceSensor("prox.horizontal.1")
sens2 = robot.getDistanceSensor("prox.horizontal.2")
sens3 = robot.getDistanceSensor("prox.horizontal.3")
sens4 = robot.getDistanceSensor("prox.horizontal.4")
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
"""enable_all_lidars controller."""
from controller import Node, Robot
import sys
robot = Robot()
timestep = int(robot.getBasicTimeStep())
lidars = []
for i in range(robot.getNumberOfDevices()):
device = robot.getDeviceByIndex(i)
if (device.getNodeType() == Node.LIDAR):
lidars.append(device)
device.enable(timestep)
device.enablePointCloud()
if len(lidars) == 0:
sys.exit("This vehicle has no 'Lidar' node.")
while robot.step(timestep) != -1:
for lidar in lidars:
class RCJSoccerRobot:
def __init__(self):
# create the Robot instance.
self.robot = Robot()
self.name = self.robot.getName()
self.team = self.name[0]
self.player_id = int(self.name[1])
self.receiver = self.robot.getReceiver("receiver")
self.receiver.enable(TIME_STEP)
self.left_motor = self.robot.getMotor("left wheel motor")
self.right_motor = self.robot.getMotor("right wheel motor")
self.left_motor.setPosition(float('+inf'))
self.right_motor.setPosition(float('+inf'))
self.left_motor.setVelocity(0.0)
self.right_motor.setVelocity(0.0)
def parse_supervisor_msg(self, packet: str) -> dict:
"""Parse message received from supervisor
Returns:
dict: Location info about each robot and the ball.
Example:
{
'B1': {'x': 0.0, 'y': 0.2, 'orientation': 1},
'B2': {'x': 0.4, 'y': -0.2, 'orientation': 1},
...
'ball': {'x': -0.7, 'y': 0.3}
}
"""
# X, Z and rotation for each robot
# plus X and Z for ball
struct_fmt = 'ddd' * 6 + 'dd'
unpacked = struct.unpack(struct_fmt, packet)
data = {}
for i, r in enumerate(ROBOT_NAMES):
data[r] = {
"x": unpacked[3 * i],
"y": unpacked[3 * i + 1],
"orientation": unpacked[3 * i + 2]
}
data["ball"] = {
"x": unpacked[3 * N_ROBOTS],
"y": unpacked[3 * N_ROBOTS + 1]
}
return data
def get_new_data(self) -> dict:
"""Read new data from supervisor
Returns:
dict: See `parse_supervisor_msg` method
"""
packet = self.receiver.getData()
self.receiver.nextPacket()
return self.parse_supervisor_msg(packet)
def is_new_data(self) -> bool:
"""Check if there are new data to be received
Returns:
bool: Whether there is new data received from supervisor.
"""
return self.receiver.getQueueLength() > 0
def get_angles(self, ball_pos: dict, robot_pos: dict) -> Tuple[float, float]:
"""Get angles in degrees.
Args:
ball_pos (dict): Dict containing info about position of the ball
robot_pos (dict): Dict containing info about position and rotation
of the robot
Returns:
:rtype: (float, float):
Angle between the robot and the ball
Angle between the robot and the north
"""
robot_angle: float = robot_pos['orientation']
# Get the angle between the robot and the ball
angle = math.atan2(
ball_pos['y'] - robot_pos['y'],
ball_pos['x'] - robot_pos['x'],
)
if angle < 0:
angle = 2 * math.pi + angle
if robot_angle < 0:
robot_angle = 2 * math.pi + robot_angle
robot_ball_angle = math.degrees(angle + robot_angle)
# Axis Z is forward
# TODO: change the robot's orientation so that X axis means forward
robot_ball_angle -= 90
if robot_ball_angle > 360:
robot_ball_angle -= 360
return robot_ball_angle, robot_angle
def run(self):
raise NotImplementedError
# Import modules
from controller import Robot
from controller import Motor
from controller import DistanceSensor
from controller import Receiver
# Create a robot instance
robot = Robot()
# Declare constants
TIMESTEP = int(robot.getBasicTimeStep())
COMM_CHANNEL = 14
ROBOT_SPEED = 5.0
# Sensors setup
ds = []
dsNames = ['ds0', 'ds1']
for name in dsNames:
ds.append(robot.getDistanceSensor(name))
for i in range(2):
ds[i].enable(TIMESTEP)
# Motors setup
wheels = []
wheelsNames = ['left wheel motor', 'right wheel motor']
for name in wheelsNames:
wheels.append(robot.getMotor(name))
for i in range(2):
wheels[i].setPosition(float('inf'))
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
from controller import Robot
robot = Robot()
timestep = int(robot.getBasicTimeStep())
print('Move forward until an obstabcle is detected.')
robot.getDevice('motor led').set(0xFF0000)
robot.getDevice('distance sensor led').set(0x00FF00)
motor = robot.getDevice('motor')
motor.setPosition(float('inf')) # Velocity control mode.
motor.setVelocity(0.5 * motor.getMaxVelocity())
distanceSensor = robot.getDevice('distance sensor')
distanceSensor.enable(timestep)
while robot.step(timestep) != -1:
def __init__(self):
Robot.__init__(self)
# define the motion which is currently playing
self.currentlyPlaying = None
self.key = 0
self.timeStep = 40
self.joints = []
self.findAndEnableDevices()
self.f = finfo(float)
###AshGavs edit
self.fsr = self.getFsr()
self.idle = True
self.stepStop = True
self.stepStart = True
self.stepCount = 0
self.stepSign = -1 # -1 for right support, +1 for left support
self.tStep = 0.5
self.upPhase = 0.3
self.downPhase = 0.8
# Nao details:
self.bodyHeight = 0.28
self.dHeight = 0.00
self.tZmp = 0.17
self.stepHeight = 0.025
self.bodyRoll = 0*pi/180
self.bodyTilt = 0*pi/180
self.supportX = 0.015
self.supportY = 0
self.velCurrent = array([0., 0., 0.0])
self.velCommand = array([0., 0., 0.])
#self.setVelocity(array([1., 0, 0.]))
self.velMax = 0.06
self.velScale = array([1., 2.5, .15])
self.velChange = 0.25
self.velOdometry = array([0., 0., 0.])
self.odometry = array([0., 0., 0.])
self.odometryScale = 0.99
# self.footX = 0
self.footX = -self.supportX
self.footY = 0.0525
self.uLeft0 = array([self.footX, self.footY, 0.])
self.uLeft = self.uLeft0
self.uLeft1 = self.uLeft
self.uLeft2 = self.uLeft
self.uRight0 = array([self.footX, -self.footY, 0.])
self.uRight = self.uRight0
self.uRight1 = self.uRight
self.uRight2 = self.uRight
self.uBody = array([0., 0., 0.])
self.uBody1 = self.uBody
self.uBody2 = self.uBody
self.qLegs = zeros((12,1),float)
self.legsIndex = range(6,18)#[6:18] #changed 7 to 6
self.qLegsIndexStart = 6
self.qLegsIndexStop = 18
self.hsupT = array([.8, .8, .8, .8, .6, .6])
self.hardnessSupport = self.hsupT.transpose()
self.hswingT = array([.8, .6, .6, .6, .2, .2])
self.hardnessSwing = self.hswingT.transpose()
self.qArms = zeros((8,1),float)
self.qArmsT = array([120, 15, -90, -60, 120, -15, 90, 60])
self.qArmsT2 = self.qArmsT.transpose()
self.qArms0 = (pi/180)*self.qArmsT2
print "self.qArms0: "
print self.qArms0
self.armsIndex1Start = 0
self.armsIndex1Stop = 3
self.armsIndex2Start = 13
self.armsIndex2Stop = 15
self.hardnessArms = ones((8,1),float)*.2
#self.setJointName("LAnklePitch", "RAnklePitch", -.2)
#self.setJointName("LHipPitch", "RHipPitch", -.2)
#self.setJointName("LKneePitch", "RKneePitch", -.2)
#self.setJointName("LHipPitch", "RHipPitch", -.3)
#self.setJointName("LHipRoll", "RHipRoll", -.2)
self.t0 = self.getTime()
self.tPhase = 0.
self.phShift = 0.
self.start()
self.printHelp()
#Move Left
self.setVelocity([0.0, 0.03, 0.0])
while not self.key:
self.key = self.keyboardGetKey()
self.update()
self.simulationStep()
