def setParams(self, params):
SCENE_FILE = '../../etc/omnirobot.ttt'
#SCENE_FILE = '../etc/viriato_dwa.ttt'
self.pr = PyRep()
self.pr.launch(SCENE_FILE, headless=False)
self.pr.start()
#self.robot = Viriato()
self.robot = YouBot()
self.robot_object = Shape("youBot")
#self.ViriatoBase_WheelRadius = 76.2 #mm real robot
self.ViriatoBase_WheelRadius = 44 # mm coppelia
self.ViriatoBase_DistAxes = 380.
self.ViriatoBase_AxesLength = 422.
self.ViriatoBase_Rotation_Factor = 8.1 # it should be (DistAxes + AxesLength) / 2
# Each laser is composed of two cameras. They are converted into a 360 virtual laser
self.hokuyo_base_front_left = VisionSensor("hokuyo_base_front_left")
self.hokuyo_base_front_right = VisionSensor("hokuyo_base_front_right")
self.hokuyo_base_back_right = VisionSensor("hokuyo_base_back_right")
self.hokuyo_base_back_left = VisionSensor("hokuyo_base_back_left")
self.ldata = []
self.joystick_newdata = []
self.speed_robot = []
self.speed_robot_ant = []
self.last_received_data_time = 0
def test_get_linear_path_visualize(self):
mobile = YouBot()
waypoint = Dummy('youBot_waypoint')
path = mobile.get_linear_path(waypoint.get_position(),
waypoint.get_orientation()[-1])
# Check that it does not error
path.visualize()
def test_get_linear_path_and_get_end(self):
mobile = YouBot()
waypoint = Dummy('youBot_waypoint')
path = mobile.get_linear_path(waypoint.get_position(),
waypoint.get_orientation()[-1])
path.set_to_end()
self.assertTrue(
np.allclose(mobile.get_position()[:2],
waypoint.get_position()[:2],
atol=0.001))
def __init__(self):
self.pr = PyRep()
self.pr.launch(SCENE_FILE, headless=False)
self.pr.start()
self.agent = YouBot()
self.agent.set_motor_locked_at_zero_velocity(True)
self.target = Shape.create(type=PrimitiveShape.SPHERE,
size=[0.05, 0.05, 0.05],
color=[1.0, 0.1, 0.1],
static=True,
respondable=False)
self.target_position = None
def test_get_linear_path_and_step(self):
mobile = YouBot()
waypoint = Dummy('youBot_waypoint')
path = mobile.get_linear_path(waypoint.get_position(),
waypoint.get_orientation()[-1])
self.assertIsNotNone(path)
done = False
while not done:
done = path.step()
self.pyrep.step()
self.assertTrue(
np.allclose(mobile.get_position()[:2],
waypoint.get_position()[:2],
atol=0.001))
from pyrep.robots.arms.panda import Panda
from pyrep.robots.mobiles.youbot import YouBot
from pyrep.objects.shape import Shape
from pyrep.const import PrimitiveShape
from pyrep.errors import ConfigurationPathError
import numpy as np
import math
from pyrep.robots.end_effectors.panda_gripper import PandaGripper
LOOPS = 10
SCENE_FILE = join(dirname(abspath(__file__)), 'colab.ttt')
pr = PyRep()
pr.launch(SCENE_FILE, headless=False)
pr.start()
arm = Panda()
mobile = YouBot()
#gripper = PandaGripper()
# We could have made this target in the scene, but lets create one dynamically
target = Shape.create(type=PrimitiveShape.SPHERE,
size=[0.05, 0.05, 0.05],
color=[1.0, 0.1, 0.1],
static=True,
respondable=False)
target1 = Shape.create(type=PrimitiveShape.SPHERE,
size=[0.05, 0.05, 0.05],
color=[1.0, 0.4, 0.2],
static=True,
respondable=False)
position_min, position_max = [0.8, 1.8, 0.0], [1.0, 2.2, 0.4]
This script contains examples of:
- Linear mobile paths with an omnidirectional robot to reach a target.
"""
from os.path import dirname, join, abspath
from pyrep import PyRep
from pyrep.robots.mobiles.youbot import YouBot
from pyrep.objects.shape import Shape
from pyrep.const import PrimitiveShape
import numpy as np
LOOPS = 4
SCENE_FILE = join(dirname(abspath(__file__)), 'scene_youbot_navigation.ttt')
pr = PyRep()
pr.launch(SCENE_FILE, headless=False)
pr.start()
agent = YouBot()
# We could have made this target in the scene, but lets create one dynamically
target = Shape.create(type=PrimitiveShape.SPHERE,
size=[0.05, 0.05, 0.05],
color=[1.0, 0.1, 0.1],
static=True,
respondable=False)
position_min, position_max = [-0.5, 1.4, 0.1], [1.0, 0.5, 0.1]
starting_pose = agent.get_2d_pose()
for i in range(LOOPS):
agent.set_2d_pose(starting_pose)
class SpecificWorker(GenericWorker):
def __init__(self, proxy_map):
super(SpecificWorker, self).__init__(proxy_map)
def __del__(self):
print('SpecificWorker destructor')
def setParams(self, params):
SCENE_FILE = '../../etc/omnirobot.ttt'
#SCENE_FILE = '../etc/viriato_dwa.ttt'
self.pr = PyRep()
self.pr.launch(SCENE_FILE, headless=False)
self.pr.start()
#self.robot = Viriato()
self.robot = YouBot()
self.robot_object = Shape("youBot")
#self.ViriatoBase_WheelRadius = 76.2 #mm real robot
self.ViriatoBase_WheelRadius = 44 # mm coppelia
self.ViriatoBase_DistAxes = 380.
self.ViriatoBase_AxesLength = 422.
self.ViriatoBase_Rotation_Factor = 8.1 # it should be (DistAxes + AxesLength) / 2
# Each laser is composed of two cameras. They are converted into a 360 virtual laser
self.hokuyo_base_front_left = VisionSensor("hokuyo_base_front_left")
self.hokuyo_base_front_right = VisionSensor("hokuyo_base_front_right")
self.hokuyo_base_back_right = VisionSensor("hokuyo_base_back_right")
self.hokuyo_base_back_left = VisionSensor("hokuyo_base_back_left")
self.ldata = []
self.joystick_newdata = []
self.speed_robot = []
self.speed_robot_ant = []
self.last_received_data_time = 0
#@QtCore.Slot()
def compute(self):
cont = 0
start = time.time()
while True:
self.pr.step()
self.read_laser()
self.read_joystick()
self.read_robot_pose()
self.move_robot()
elapsed = time.time() - start
if elapsed < 0.05:
time.sleep(0.05 - elapsed)
cont += 1
if elapsed > 1:
print("Freq -> ", cont)
cont = 0
start = time.time()
###########################################
### LASER get and publish laser data
###########################################
def read_laser(self):
self.ldata = self.compute_omni_laser([
self.hokuyo_base_front_right, self.hokuyo_base_front_left,
self.hokuyo_base_back_left, self.hokuyo_base_back_right
], self.robot)
try:
self.laserpub_proxy.pushLaserData(self.ldata)
except Ice.Exception as e:
print(e)
###########################################
### JOYSITCK read and move the robot
###########################################
def read_joystick(self):
if self.joystick_newdata: #and (time.time() - self.joystick_newdata[1]) > 0.1:
self.update_joystick(self.joystick_newdata[0])
self.joystick_newdata = None
self.last_received_data_time = time.time()
else:
elapsed = time.time() - self.last_received_data_time
if elapsed > 2 and elapsed < 3:
self.robot.set_base_angular_velocites([0, 0, 0])
###########################################
### ROBOT POSE get and publish robot position
###########################################
def read_robot_pose(self):
pose = self.robot.get_2d_pose()
linear_vel, ang_vel = self.robot_object.get_velocity()
# print("Veld:", linear_vel, ang_vel)
try:
isMoving = np.abs(linear_vel[0]) > 0.01 or np.abs(
linear_vel[1]) > 0.01 or np.abs(ang_vel[2]) > 0.01
self.bState = RoboCompGenericBase.TBaseState(
x=pose[0] * 1000,
z=pose[1] * 1000,
alpha=pose[2],
advVx=linear_vel[0] * 1000,
advVz=linear_vel[1] * 1000,
rotV=ang_vel[2],
isMoving=isMoving)
self.omnirobotpub_proxy.pushBaseState(self.bState)
except Ice.Exception as e:
print(e)
###########################################
### MOVE ROBOT from Omnirobot interface
###########################################
def move_robot(self):
if self.speed_robot != self.speed_robot_ant: # or (isMoving and self.speed_robot == [0,0,0]):
self.robot.set_base_angular_velocites(self.speed_robot)
#print("Velocities sent to robot:", self.speed_robot)
self.speed_robot_ant = self.speed_robot
########################################
## General laser computation
########################################
def compute_omni_laser(self, lasers, robot):
c_data = []
coor = []
for laser in lasers:
semiwidth = laser.get_resolution()[0] / 2
semiangle = np.radians(laser.get_perspective_angle() / 2)
focal = semiwidth / np.tan(semiangle)
data = laser.capture_depth(in_meters=True)
m = laser.get_matrix(robot) # these data should be read first
imat = np.array([[m[0], m[1], m[2], m[3]],
[m[4], m[5], m[6], m[7]],
[m[8], m[9], m[10], m[11]], [0, 0, 0, 1]])
for i, d in enumerate(data.T):
z = d[0] # min if more than one row in depth image
vec = np.array([-(i - semiwidth) * z / focal, 0, z, 1])
res = imat.dot(
vec)[:3] # translate to robot's origin, homogeneous
c_data.append(
[np.arctan2(res[0], res[1]),
np.linalg.norm(res)]) # add to list in polar coordinates
# create 360 polar rep
c_data_np = np.asarray(c_data)
angles = np.linspace(-np.pi, np.pi,
360) # create regular angular values
positions = np.searchsorted(
angles,
c_data_np[:, 0]) # list of closest position for each laser meas
ldata = [RoboCompLaser.TData(a, 0)
for a in angles] # create empty 360 angle array
pos, medians = npi.group_by(positions).median(
c_data_np[:, 1]) # group by repeated positions
for p, m in zip_longest(pos, medians): # fill the angles with measures
ldata[p].dist = int(m * 1000) # to millimeters
if ldata[0] == 0:
ldata[0] = 200 #half robot width
for i in range(0, len(ldata)):
if ldata[i].dist == 0:
ldata[i].dist = ldata[i - 1].dist
#ldata[0].dist = ldata[len(data)-1].dist
return ldata
def update_joystick(self, datos):
adv = 0.0
rot = 0.0
side = 0.0
#linear_vel, ang_vel = self.robot_object.get_velocity()
for x in datos.axes:
if x.name == "advance":
adv = x.value if np.abs(x.value) > 1 else 0
if x.name == "rotate":
rot = x.value if np.abs(x.value) > 0.01 else 0
if x.name == "side":
side = x.value if np.abs(x.value) > 1 else 0
converted = self.convert_base_speed_to_radians(adv, side, rot)
print("Joystick ", [adv, side, rot], converted)
self.robot.set_base_angular_velocites(converted)
def convert_base_speed_to_radians(self, adv, side, rot):
# rot has to be neg so neg rot speeds go clock wise. It is probably a sign in Pyrep forward kinematics
return [
adv / self.ViriatoBase_WheelRadius,
side / self.ViriatoBase_WheelRadius,
rot * self.ViriatoBase_Rotation_Factor
]
##################################################################################
# SUBSCRIPTION to sendData method from JoystickAdapter interface
###################################################################################
def JoystickAdapter_sendData(self, data):
self.joystick_newdata = [data, time.time()]
##################################################################################
# Methods for CameraRGBDSimple
# ===============================================================================
#
# getAll
#
def CameraRGBDSimple_getAll(self, camera):
return RoboCompCameraRGBDSimple.TRGBD(self.cameras[camera]["rgb"],
self.cameras[camera]["depth"])
#
# getDepth
#
def CameraRGBDSimple_getDepth(self, camera):
return self.cameras[camera]["depth"]
#
# getImage
#
def CameraRGBDSimple_getImage(self, camera):
return self.cameras[camera]["rgb"]
#######################################################
#### Laser
#######################################################
#
# getLaserAndBStateData
#
def Laser_getLaserAndBStateData(self):
bState = RoboCompGenericBase.TBaseState()
return self.ldata, bState
#
# getLaserConfData
#
def Laser_getLaserConfData(self):
ret = RoboCompLaser.LaserConfData()
return ret
#
# getLaserData
#
def Laser_getLaserData(self):
return self.ldata
##############################################
## Omnibase
#############################################
#
# correctOdometer
#
def OmniRobot_correctOdometer(self, x, z, alpha):
pass
#
# getBasePose
#
def OmniRobot_getBasePose(self):
#
# implementCODE
#
x = self.bState.x
z = self.bState.z
alpha = self.bState.alpha
return [x, z, alpha]
#
# getBaseState
#
def OmniRobot_getBaseState(self):
return self.bState
#
# resetOdometer
#
def OmniRobot_resetOdometer(self):
pass
#
# setOdometer
#
def OmniRobot_setOdometer(self, state):
pass
#
# setOdometerPose
#
def OmniRobot_setOdometerPose(self, x, z, alpha):
pass
#
# setSpeedBase
#
def OmniRobot_setSpeedBase(self, advx, advz, rot):
self.speed_robot = self.convert_base_speed_to_radians(advz, advx, rot)
#
# stopBase
#
def OmniRobot_stopBase(self):
pass
# ===================================================================
# CoppeliaUtils
# ===================================================================
def CoppeliaUtils_addOrModifyDummy(self, type, name, pose):
if not Dummy.exists(name):
dummy = Dummy.create(0.1)
# one color for each type of dummy
if type == RoboCompCoppeliaUtils.TargetTypes.Info:
pass
if type == RoboCompCoppeliaUtils.TargetTypes.Hand:
pass
if type == RoboCompCoppeliaUtils.TargetTypes.HeadCamera:
pass
dummy.set_name(name)
else:
dummy = Dummy(name)
parent_frame_object = None
if type == RoboCompCoppeliaUtils.TargetTypes.HeadCamera:
parent_frame_object = Dummy("viriato_head_camera_pan_tilt")
#print("Coppelia ", name, pose.x/1000, pose.y/1000, pose.z/1000)
dummy.set_position(
[pose.x / 1000., pose.y / 1000., pose.z / 1000.],
parent_frame_object)
dummy.set_orientation([pose.rx, pose.ry, pose.rz],
parent_frame_object)
class YouBotEnv(object):
def __init__(self):
self.pr = PyRep()
self.pr.launch(SCENE_FILE, headless=False)
self.pr.start()
self.agent = YouBot()
self.agent.set_motor_locked_at_zero_velocity(True)
self.target = Shape.create(type=PrimitiveShape.SPHERE,
size=[0.05, 0.05, 0.05],
color=[1.0, 0.1, 0.1],
static=True,
respondable=False)
self.target_position = None
def _get_state(self):
# Return state containing arm joint angles/velocities & target position
return (self.target.get_position())
def reset(self):
# Set a random starting position
#start_position_x, start_position_y = [-0.5, 0], [0.5, 0]
#start_pose = list(np.random.uniform(start_position_min, start_position_max))
start_pose = list([0, -1, 0])
self.agent.set_2d_pose(start_pose)
# Get a random target position
target_position_min, target_position_max = [-0.5, 1,
0.1], [0.5, 1.5, 0.1]
target_pose = list(
np.random.uniform(target_position_min, target_position_max))
self.target.set_position(target_pose)
return self._get_state()
def step(self, action):
linear_vel_x = 0.5 * (action[0] + 1.0)
angular_vel = action[1]
self.agent.set_base_angular_velocites((linear_vel_x, 0, angular_vel))
self.pr.step()
tx, ty, tz = self.target.get_position()
x, y, yaw = self.agent.get_2d_pose()
# Reward is negative distance to target
reward = -np.sqrt((x - tx)**2 + (y - ty)**2)
print("REWARD:", reward)
return reward, self._get_state()
def step1(self, action):
x, y, yaw = self.agent.get_2d_pose()
target_x = x + 0.5 * (action[0] + 1.0)
target_y = y + action[1]
target_yaw = yaw + action[2] * 0.1
try:
path = self.agent.get_nonlinear_path(position=(target_x, target_y),
angle=target_yaw)
path.visualize()
done = False
while not done:
done = path.step()
self.pr.step()
path.clear_visualization()
except:
pass
tx, ty, tz = self.target.get_position()
x, y, yaw = self.agent.get_2d_pose()
# Reward is negative distance to target
reward = -np.sqrt((x - tx)**2 + (y - ty)**2)
print("REWARD:", reward)
return reward, self._get_state()
def shutdown(self):
self.pr.stop()
self.pr.shutdown()
def test_get_duplicate_mobile(self):
mobile = YouBot(1)
self.assertIsInstance(mobile, YouBot)
def test_get_nonlinear_path(self):
mobile = YouBot()
waypoint = Dummy('youBot_waypoint')
path = mobile.get_nonlinear_path(waypoint.get_position(),
waypoint.get_orientation()[-1])
self.assertIsNotNone(path)
import numpy as np
import json
import os
import sys
import termios
import fcntl
import time
import pygame
#setup CoppeliaSim and launch the scene file
SCENE_FILE = join(dirname(abspath(__file__)), 'scene.ttt')
pr = PyRep()
pr.launch(SCENE_FILE, headless=False)
pr.start() # Start the simulation
json_dictionary = {}
agent = YouBot()
HEIGHT = 0.1
#joystick setup
pygame.joystick.init()
joystick = pygame.joystick.Joystick(0)
joystick.init()
pygame.init()
#help(agent)
angular_velocities = [0., 0., 0.]
#function for joystick axis movement
def joystick_movement():
global angular_velocities
#pygame code to look for event occurrence