def youbot():
print('Program started')
## Initiate the connection to the simulator.
vrep.simxFinish(-1)
id = vrep.simxStart('127.0.0.1', 19997, True, True, 2000, 5)
if id < 0:
print('Failed connecting to remote API server. Exiting.')
return
print('Connection {} to remote API server open '.format(id))
# Make sure we close the connection whenever the script is interrupted.
atexit.register(cleanup_vrep, vrep=vrep, id=id)
# This will only work in "continuous remote API server service".
# See http://www.v-rep.eu/helpFiles/en/remoteApiServerSide.htm
vrep.simxStartSimulation(id, vrep.simx_opmode_oneshot_wait)
# Retrieve all handles.
h = Youbot(vrep, id)
# Make sure everything is settled before we start (wait for the simulation to start).
time.sleep(.2)
# Read data from the RGB camera.
# This starts the robot's camera to take a 2D picture of what the robot can see.
# Reading an image costs a lot to VREP (it has to simulate the image). It also requires a lot of bandwidth,
# and processing an image will take a long time in MATLAB. In general, you will only want to capture
# an image at specific times, for instance when you believe you're facing one of the tables or a basket.
# Ask the sensor to turn itself on, take A SINGLE IMAGE, and turn itself off again.
# ^^^ ^^^^^^ ^^ ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
# simxSetIntegerSignal 1 simx_opmode_oneshot_wait
# |
# handle_rgb_sensor
res = vrep.simxSetIntegerSignal(id, 'handle_rgb_sensor', 1,
vrep.simx_opmode_oneshot_wait)
#vrchk(vrep, res); # Check the return value from the previous V-REP call (res) and exit in case of error.
# Then retrieve the last picture the camera took. The image must be in RGB (not gray scale).
# ^^^^^^^^^^^^^^^^^^^^^^^^^ ^^^^^^ ^^^
# simxGetVisionSensorImage2 h.rgbSensor 0
# If you were to try to capture multiple images in a row, try other values than vrep.simx_opmode_oneshot_wait.
print('Capturing image...')
res, resolution, image = vrep.simxGetVisionSensorImage(
id, h.RGBDSensor.rgbSensor, 0, vrep.simx_opmode_oneshot_wait)
#vrchk(vrep, res);
width, height = resolution
print('Captured {} pixels ({} x {}).'.format(width * height, width,
height))
image = (
np.reshape(image, (height, width, 3)) + 255
) % 255 # Returns in range [-128, 127]. Matplotlib requires [0, 255]
# Finally, show the image.
plt.imshow(image)
plt.show()
def __init__(self, enables_acturator_dynamics = False):
self.vrep_client_id=vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5) # Connect to V-REP
self.current_goal = 0.
self.path_list = []
self.current_car_position = []
while True:
print('Connected to remote API server')
if self.vrep_client_id != -1:
break
else:
sleep(0.2)
self.path_list = self.readCSV()
self.left_motor_handles = vrep.simxGetObjectHandle(clientID=self.vrep_client_id, objectName='Team12_motorLeft', operationMode=vrep.simx_opmode_blocking)[1]
self.right_motor_handles = vrep.simxGetObjectHandle(clientID=self.vrep_client_id, objectName='Team12_motorRight', operationMode=vrep.simx_opmode_blocking)[1]
self.steeringLeft_handles = vrep.simxGetObjectHandle(clientID=self.vrep_client_id, objectName='Team12_steeringLeft', operationMode=vrep.simx_opmode_blocking)[1]
self.steeringRight_handles = vrep.simxGetObjectHandle(clientID=self.vrep_client_id, objectName='Team12_steeringRight', operationMode=vrep.simx_opmode_blocking)[1]
self.car_handle = vrep.simxGetObjectHandle(clientID=self.vrep_client_id,objectName='Team12_VehicleFrontPose', operationMode=vrep.simx_opmode_blocking)[1]
self.collision_handle = vrep.simxGetCollisionHandle(clientID=self.vrep_client_id,collisionObjectName='Collision', operationMode=vrep.simx_opmode_blocking)[1]
self.goal_number = 0
self.goal_angle = 0
def __init__(self):
try:
vrep.simxFinish(-1)
except:
pass
self.clientID=vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5) # Connect to V-REP
#vrep.simxSynchronous(self.clientID, True)
self.opmode_blocking = vrep.simx_opmode_blocking
self.obs_handle1 = vrep.simxGetObjectHandle(self.clientID, objectName="obs1", operationMode=vrep.simx_opmode_blocking)[1]
self.obs_handle2 = vrep.simxGetObjectHandle(self.clientID, objectName="obs2", operationMode=vrep.simx_opmode_blocking)[1]
self.obs_handle3 = vrep.simxGetObjectHandle(self.clientID, objectName="obs3", operationMode=vrep.simx_opmode_blocking)[1]
self.obs_handle4 = vrep.simxGetObjectHandle(self.clientID, objectName="obs4", operationMode=vrep.simx_opmode_blocking)[1]
self.obs_handle5 = vrep.simxGetObjectHandle(self.clientID, objectName="obs5", operationMode=vrep.simx_opmode_blocking)[1]
self.d_obs_handle = vrep.simxGetObjectHandle(self.clientID, objectName="Obstacle_shape_1", operationMode=vrep.simx_opmode_blocking)[1]
self.robot_handle = vrep.simxGetObjectHandle(self.clientID, objectName="dualarm_mobile", operationMode=vrep.simx_opmode_blocking)[1]
self.x_rand1 = 1.2362 #random.uniform(-2.5, 2.5)
self.x_rand3 = -2.388 #random.uniform(-2.5, 2.5)
self.x_rand4 = 1.8112 #random.uniform(-2.5, 2.5)
self.x_rand5 = -2.5138 #random.uniform(-2.5, 2.5)
vrep.simxSetObjectPosition(self.clientID, self.obs_handle1, -1, [6.75, self.x_rand1, 0.5], self.opmode_blocking)
vrep.simxSetObjectPosition(self.clientID, self.obs_handle3, -1, [1.75, self.x_rand3, 0.5], self.opmode_blocking)
vrep.simxSetObjectPosition(self.clientID, self.obs_handle4, -1, [-2.75, self.x_rand4, 0.5], self.opmode_blocking)
vrep.simxSetObjectPosition(self.clientID, self.obs_handle5, -1, [-5.75, self.x_rand5, 0.5], self.opmode_blocking)
self.alpha = 0.1
self.deviation = 0
self._pos_x = -8
self._pos_y = 0
self._d_x = 4.95
self._d_y = -1.0696
self.graph_switch = True
self.csv_container = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
from api import vrep
import sys
import time
import math
try:
vrep.simxFinish(-1) # just in case, close all opened connections
except:
pass
clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
if clientID != -1:
print("Connected to remote API server")
vrep.simxStopSimulation(clientID, operationMode=vrep.simx_opmode_blocking)
_, handle = vrep.simxGetObjectHandle(clientID, 'dualarm_mobile',
vrep.simx_opmode_blocking)
time.sleep(1)
vrep.simxSetObjectPosition(clientID, handle, -1, [0.0, 0.0, 0.1],
vrep.simx_opmode_blocking)
time.sleep(1)
vrep.simxSetObjectOrientation(clientID, handle, -1,
[math.pi, math.pi / 2, 0.0],
vrep.simx_opmode_blocking)
time.sleep(1)
#vrep.simxFinish(clientID)
#time.sleep(1)
def youbot():
print('Program started')
## Initiate the connection to the simulator.
vrep.simxFinish(-1)
id = vrep.simxStart('127.0.0.1', 19997, True, True, 2000, 5)
if id < 0:
print('Failed connecting to remote API server. Exiting.')
return
print('Connection {} to remote API server open '.format(id))
# Make sure we close the connection whenever the script is interrupted.
atexit.register(cleanup_vrep, vrep=vrep, id=id)
# This will only work in "continuous remote API server service".
# See http://www.v-rep.eu/helpFiles/en/remoteApiServerSide.htm
vrep.simxStartSimulation(id, vrep.simx_opmode_oneshot_wait)
# Retrieve all handles, mostly the Hokuyo.
h = Youbot(vrep, id)
ls = h.LaserScanner
# Make sure everything is settled before we start (wait for the simulation to start).
time.sleep(.2)
if vrep.simxGetConnectionId(id) == -1:
raise ConnectionAbortedError('Lost connection to remote API.')
# Read data from the depth camera (Hokuyo)
# Get the position and orientation of the youBot in the world reference frame (as if with a GPS).
res, youbotPos = vrep.simxGetObjectPosition(id, h.ref, -1,
vrep.simx_opmode_buffer)
#vrchk(vrep, res, true); % Check the return value from the previous V-REP call (res) and exit in case of error.
res, youbotEuler = vrep.simxGetObjectOrientation(id, h.ref, -1,
vrep.simx_opmode_buffer)
#vrchk(vrep, res, true);
# Determine the position of the Hokuyo with global coordinates (world reference frame).
trf = np.linalg.multi_dot((transl(youbotPos), trotx(youbotEuler[0]),
troty(youbotEuler[1]), trotz(youbotEuler[2])))
worldHokuyo1 = homtrans(
trf,
np.array([[ls.hokuyo1Pos[0]], [ls.hokuyo1Pos[1]], [ls.hokuyo1Pos[2]]]))
worldHokuyo2 = homtrans(
trf,
np.array([[ls.hokuyo2Pos[0]], [ls.hokuyo2Pos[1]], [ls.hokuyo2Pos[2]]]))
# Use the sensor to detect the visible points, within the world frame.
pts, contacts = ls.scan(vrep, opmode=vrep.simx_opmode_buffer, trans=trf)
world_points = np.hstack((worldHokuyo1, pts[:], worldHokuyo2))
# Plot this data: delimit the visible area and highlight contact points.
fig, ax = plt.subplots()
ax.plot(pts[0, contacts], pts[1, contacts], '*r')
ax.plot(np.squeeze(np.asarray(world_points[0, :])),
np.squeeze(np.asarray(world_points[1, :])))
ax.set_xlim([-10, 10])
ax.set_ylim([-10, 10])
plt.show()
e, lrf_bin = vrep.simxGetStringSignal(clientID, signalName, opmode)
if e != -1:
return 0, np.array(vrep.simxUnpackFloats(lrf_bin), dtype=float)
else:
return -1, None
if __name__=='__main__':
try:
vrep.simxFinish(-1) # just in case, close all opened connections
except:
pass
opmode_blocking = vrep.simx_opmode_blocking
clientID=vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5) # Connect to V-REP
if clientID != -1:
print('Connected to remote API server')
# enable the synchronous mode on the client:
vrep.simxSynchronous(clientID, True)
# start the simulation:
vrep.simxStartSimulation(clientID,vrep.simx_opmode_blocking)
vrep.simxSynchronousTrigger(clientID) # skip first timestep. LiDAR returns zero-size array at first timestep
# get LiDAR measurement
vrep.simxSynchronousTrigger(clientID)
e, lrf = readLiDAR(clientID, 'measurement', opmode_blocking)
def youbot():
print('Program started')
## Initiate the connection to the simulator.
vrep.simxFinish(-1)
id = vrep.simxStart('127.0.0.1', 19997, True, True, 2000, 5)
if id < 0:
print('Failed connecting to remote API server. Exiting.')
return
print('Connection {} to remote API server open '.format(id))
# Make sure we close the connection whenever the script is interrupted.
atexit.register(cleanup_vrep, vrep=vrep, id=id)
# This will only work in "continuous remote API server service".
# See http://www.v-rep.eu/helpFiles/en/remoteApiServerSide.htm
vrep.simxStartSimulation(id, vrep.simx_opmode_oneshot_wait)
# Retrieve all handles, mostly the Hokuyo.
h = Youbot(vrep, id)
# Make sure everything is settled before we start (wait for the simulation to start).
time.sleep(.2)
if vrep.simxGetConnectionId(id) == -1:
raise ConnectionAbortedError('Lost connection to remote API.')
# Read data from the depth camera (Hokuyo)
# Reading a 3D image costs a lot to VREP (it has to simulate the image). It also requires a lot of
# bandwidth, and processing a 3D point cloud (for instance, to find one of the boxes or cylinders that
# the robot has to grasp) will take a long time in MATLAB. In general, you will only want to capture a 3D
# image at specific times, for instance when you believe you're facing one of the tables.
# Reduce the view angle to pi/8 in order to better see the objects. Do it only once.
# ^^^^^^ ^^^^^^^^^^ ^^^^ ^^^^^^^^^^^^^^^
# simxSetFloatSignal simx_opmode_oneshot_wait
# |
# rgbd_sensor_scan_angle
# The depth camera has a limited number of rays that gather information. If this number is concentrated
# on a smaller angle, the resolution is better. pi/8 has been determined by experimentation.
res = vrep.simxSetFloatSignal(id, 'rgbd_sensor_scan_angle', math.pi / 8,
vrep.simx_opmode_oneshot_wait)
#vrchk(vrep, res); # Check the return value from the previous V-REP call (res) and exit in case of error.
# Ask the sensor to turn itself on, take A SINGLE POINT CLOUD, and turn itself off again.
# ^^^ ^^^^^^ ^^ ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
# simxSetIntegerSignal 1 simx_opmode_oneshot_wait
# |
# handle_xyz_sensor
res = vrep.simxSetIntegerSignal(id, 'handle_xyz_sensor', 1,
vrep.simx_opmode_oneshot_wait)
#vrchk(vrep, res);
# Then retrieve the last point cloud the depth sensor took.
# If you were to try to capture multiple images in a row, try other values than
# vrep.simx_opmode_oneshot_wait.
print('Capturing point cloud...')
pts = h.RGBDSensor.scan(vrep, vrep.simx_opmode_oneshot_wait)
# Each column of pts has [x;y;z;distancetosensor]. However, plot3 does not have the same frame of reference as
# the output data. To get a correct plot, you should invert the y and z dimensions.
# Plot all the points.
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
ax.plot(pts[0, :], pts[2, :], pts[1, :], '*')
# Plot the points of the wall (further away than 1.87 m, which is determined either in the simulator by measuring
# distances or by trial and error) in a different colour. This value is only valid for this robot position, of
# course. This simple test ignores the variation of distance along the wall (distance between a point and several
# points on a line).
ptsWall = pts[0:3, pts[3, :] >= 1.87]
ax.plot(ptsWall[0, :], ptsWall[2, :], ptsWall[1, :], '.r')
plt.show()
def youbot():
print('Program started')
# Initiate the connection to the simulator.
vrep.simxFinish(-1)
id = vrep.simxStart('127.0.0.1', 19997, True, True, 2000, 5)
if id < 0:
print('Failed connecting to remote API server. Exiting.')
return
print('Connection {} to remote API server open '.format(id))
# Make sure we close the connection whenever the script is interrupted.
atexit.register(cleanup_vrep, vrep=vrep, id=id)
# This will only work in "continuous remote API server service".
# See http://www.v-rep.eu/helpFiles/en/remoteApiServerSide.htm
vrep.simxStartSimulation(id, vrep.simx_opmode_oneshot_wait)
# Retrieve all handles, mostly the Hokuyo.
h = Youbot(vrep, id)
# Make sure everything is settled before we start (wait for the simulation to start).
time.sleep(.2)
# The time step the simulator is using (your code should run close to it).
timestep = .05
# Preset values for the demo.
# Definition of the starting pose of the arm (the angle to impose at each joint to be in the rest position).
startingJoints = [
-90, 30.91 * math.pi / 180, 52.42 * math.pi / 180,
72.68 * math.pi / 180, 0
]
# Set the arm to its starting configuration. Several orders are sent sequentially, but they must be followed
# by the simulator without being interrupted by other computations (hence the enclosing simxPauseCommunication
# calls).
res = vrep.simxPauseCommunication(
id, True) # Send order to the simulator through vrep object.
#vrchk(vrep, res); # Check the return value from the previous V-REP call (res) and exit in case of error.
for i in range(5):
res = vrep.simxSetJointTargetPosition(id, h.Arm.armJoints[i],
startingJoints[i],
vrep.simx_opmode_oneshot)
#vrchk(vrep, res, true);
res = vrep.simxPauseCommunication(id, False)
#vrchk(vrep, res);
# Initialise the state machine.
fsm = 'noIK'
## Start the demo.
while True:
t = time.time() # See end of loop to see why it's useful.
if vrep.simxGetConnectionId(id) == -1:
raise ConnectionAbortedError('Lost connection to remote API.')
## Apply the state machine.
# First example: move the arm without using the IK (inverse kinematics) solver, by explicitly setting the value
# for each of the joints.
if fsm == 'noIK':
# Define each angle of the robot arm.
chooseAngle = [
180 * math.pi / 180, -25 * math.pi / 180, 75 * math.pi / 180,
-17 * math.pi / 180, 90 * math.pi / 180
]
# Apply the value to each articulation.
for i in range(5):
res = vrep.simxSetJointTargetPosition(id, h.Arm.armJoints[i],
chooseAngle[i],
vrep.simx_opmode_oneshot)
# vrchk(vrep, res, true)
# Wait until the robot arm is in position.
time.sleep(5)
fsm = 'useIK'
# Second example: move the arm by using the IK solver, so that you only have to specify the position of
# the arm tip. With this, you can move the point between the two tongs of the gripper so that it coincides with
# the object to grasp (the tip): the gripper will magically be at the right position.
elif fsm == 'useIK':
# Get the arm tip position.
[res,
tpos] = vrep.simxGetObjectPosition(id, h.Arm.ptip, h.Arm.armRef,
vrep.simx_opmode_buffer)
#vrchk(vrep, res, true)
# Set the inverse kinematics (IK) mode to position AND orientation (km_mode = 2). The solver will decide the
# values for the joint angles so that the arm tip is at the given position with the given orientation (given
# with a later simulator call).
res = vrep.simxSetIntegerSignal(id, 'km_mode', 2,
vrep.simx_opmode_oneshot_wait)
#vrchk(vrep, res, true);
# Set the new position to the expected one for the gripper (predetermined value).
tpos = [tpos[0] - 0.1, tpos[1] + 0.3, tpos[2] - 0.3]
res = vrep.simxSetObjectPosition(id, h.Arm.ptarget, h.Arm.armRef,
tpos, vrep.simx_opmode_oneshot)
#vrchk(vrep, res, true);
# You could do the same with the orientation using vrep.simxSetObjectOrientation. The details are in the
# documentation:
# http://www.coppeliarobotics.com/helpFiles/en/remoteApiFunctionsMatlab.htm#simxSetObjectOrientation
# Wait long enough so that the tip is at the right position and go on to the next state.
time.sleep(5)
fsm = 'rotGrip'
# Rotate the tip. It can be actuated as any joint of the arm.
elif fsm == 'rotGrip':
# Remove the inverse kinematics (IK) mode so that joint angles can be set individually (it was reenabled by
# the previous state).
res = vrep.simxSetIntegerSignal(id, 'km_mode', 0,
vrep.simx_opmode_oneshot_wait)
#vrchk(vrep, res, true);
# Set the new gripper angle to 0°.
res = vrep.simxSetJointTargetPosition(id, h.Arm.armJoints[4], 0,
vrep.simx_opmode_oneshot)
#vrchk(vrep, res, true);
# Make MATLAB wait long enough so that the tip is at the right position and go on to the next state.
# This value was determined by experiments.
time.sleep(5)
fsm = 'grasp'
# Close the gripper. It is not possible to determine the force to apply and the object will sometimes slip
# from the gripper!
elif fsm == 'grasp':
res = vrep.simxSetIntegerSignal(id, 'gripper_open', 0,
vrep.simx_opmode_oneshot_wait)
#vrchk(vrep, res);
# Make MATLAB wait for the gripper to be closed. This value was determined by experiments.
time.sleep(3)
fsm = 'release'
# Open the gripper.
elif fsm == 'release':
res = vrep.simxSetIntegerSignal(id, 'gripper_open', 1,
vrep.simx_opmode_oneshot_wait)
#vrchk(vrep, res);
# Make MATLAB wait for the gripper to be opened. This value was determined by experiments.
time.sleep(5)
fsm = 'finished'
# Demo done: exit the function.
elif fsm == 'finished':
time.sleep(3)
break
else:
raise ValueError('Unknown state: ' + fsm)
# Make sure that we do not go faster than the physics simulation (it is useless to go faster).
elapsed = time.time() - t
timeleft = timestep - elapsed
if (timeleft > 0):
time.sleep(min(timeleft, .01))
def youbot():
print('Program started')
## Initiate the connection to the simulator.
vrep.simxFinish(-1)
id = vrep.simxStart('127.0.0.1', 19997, True, True, 2000, 5)
if id < 0:
print('Failed connecting to remote API server. Exiting.')
return
print('Connection {} to remote API server open '.format(id))
# Make sure we close the connection whenever the script is interrupted.
atexit.register(cleanup_vrep, vrep=vrep, id=id)
# This will only work in "continuous remote API server service".
# See http://www.v-rep.eu/helpFiles/en/remoteApiServerSide.htm
vrep.simxStartSimulation(id, vrep.simx_opmode_oneshot_wait)
# Retrieve all handles, mostly the Hokuyo.
h = Youbot(vrep, id)
# Make sure everything is settled before we start (wait for the simulation to start).
time.sleep(.2)
# The time step the simulator is using (your code should run close to it).
timestep = .05
## Preset values for the demo.
# Parameters for controlling the youBot's wheels: at each iteration, those values will be set for the wheels.
# They are adapted at each iteration by the code.
forwBackVel = 0 # Move straight ahead.
rightVel = 0 # Go sideways.
rotateRightVel = 0 # Rotate.
# First state of state machine
fsm = 'forward'
print('Switching to state: ', fsm)
## Start the demo.
while True:
t = time.time() # See end of loop to see why it's useful.
if vrep.simxGetConnectionId(id) == -1:
raise ConnectionAbortedError('Lost connection to remote API.')
# Get the position and the orientation of the robot.
res, youbotPos = vrep.simxGetObjectPosition(id, h.ref, -1,
vrep.simx_opmode_buffer)
#vrchk(vrep, res, true) # Check the return value from the previous V-REP call (res) and exit in case of error.
res, youbotEuler = vrep.simxGetObjectOrientation(
id, h.ref, -1, vrep.simx_opmode_buffer)
#vrchk(vrep, res, true)
## Apply the state machine.
if fsm == 'forward':
# Make the robot drive with a constant speed (very simple controller, likely to overshoot).
# The speed is - 1 m/s, the sign indicating the direction to follow. Please note that the robot has
# limitations and cannot reach an infinite speed.
forwBackVel = -1
# Stop when the robot is close to y = - 6.5. The tolerance has been determined by experiments: if it is too
# small, the condition will never be met (the robot position is updated every 50 ms); if it is too large,
# then the robot is not close enough to the position (which may be a problem if it has to pick an object,
# for example).
if abs(youbotPos[1] + 6.5) < .01:
forwBackVel = 0 # Stop the robot.
fsm = 'backward'
print('Switching to state: ', fsm)
elif fsm == 'backward':
# A speed which is a function of the distance to the destination can also be used. This is useful to avoid
# overshooting: with this controller, the speed decreases when the robot approaches the goal.
# Here, the goal is to reach y = -4.5.
forwBackVel = -2 * (youbotPos[1] + 4.5)
# ^^^ ^^^^^^^^^^^^^^^^^^^^
# | distance to goal
# influences the maximum speed
# Stop when the robot is close to y = 4.5.
if abs(youbotPos[1] + 4.5) < .01:
forwBackVel = 0 # Stop the robot.
fsm = 'right'
print('Switching to state: ', fsm)
elif fsm == 'right':
# Move sideways, again with a proportional controller (goal: x = - 4.5).
rightVel = -2 * (youbotPos[0] + 4.5)
# Stop at x = - 4.5
if abs(youbotPos[0] + 4.5) < .01:
rightVel = 0 # Stop the robot.
fsm = 'rotateRight'
print('Switching to state: ', fsm)
elif fsm == 'rotateRight':
# Rotate until the robot has an angle of -pi/2 (measured with respect to the world's reference frame).
# Again, use a proportional controller. In case of overshoot, the angle difference will change sign,
# and the robot will correctly find its way back (e.g.: the angular speed is positive, the robot overshoots,
# the anguler speed becomes negative).
# youbotEuler(3) is the rotation around the vertical axis.
rotateRightVel = -math.pi / 2 - youbotEuler[
2] # angdiff ensures the difference is between -pi and pi.
# Stop when the robot is at an angle close to -pi/2.
if abs(-math.pi / 2 - youbotEuler[2]) < .002:
rotateRightVel = 0
fsm = 'finished'
print('Switching to state: ', fsm)
elif fsm == 'finished':
time.sleep(3)
break
else:
raise ValueError('Unknown state: ' + fsm)
# Update wheel velocities.
h.drive(vrep, forwBackVel, rightVel, rotateRightVel)
# What happens if you do not update the velocities? The simulator always considers the last speed you gave it,
# until you set a new velocity. If you perform computations for several seconds in a row without updating the
# speeds, the robot will continue to move --- even if it bumps into a wall.
# Make sure that we do not go faster than the physics simulation (it is useless to go faster).
elapsed = time.time() - t
timeleft = timestep - elapsed
if (timeleft > 0):
time.sleep(min(timeleft, .01))
def youbot():
print('Program started')
# Initiate the connection to the simulator.
vrep.simxFinish(-1)
id = vrep.simxStart('127.0.0.1', 19997, True, True, 2000, 5)
if id < 0:
print('Failed connecting to remote API server. Exiting.')
return
print('Connection {} to remote API server open '.format(id))
# Make sure we close the connection whenever the script is interrupted.
atexit.register(cleanup_vrep, vrep=vrep, id=id)
# This will only work in "continuous remote API server service".
# See http://www.v-rep.eu/helpFiles/en/remoteApiServerSide.htm
vrep.simxStartSimulation(id, vrep.simx_opmode_oneshot_wait)
# Retrieve all handles, mostly the Hokuyo.
h = Youbot(vrep, id)
# Make sure everything is settled before we start (wait for the simulation to start).
time.sleep(.2)
# The time step the simulator is using (your code should run close to it).
timestep = .05
# Minimum and maximum angles for all joints. Only useful to implement custom IK.
armJointRanges = [[-2.9496064186096, 2.9496064186096],
[-1.5707963705063, 1.308996796608],
[-2.2863812446594, 2.2863812446594],
[-1.7802357673645, 1.7802357673645],
[-1.5707963705063, 1.5707963705063]]
# Definition of the starting pose of the arm (the angle to impose at each joint to be in the rest position).
startingJoints = [
0, 30.91 * math.pi / 180, 52.42 * math.pi / 180, 72.68 * math.pi / 180,
0
]
# Preset values for the demo.
print('Starting robot')
# Define the preset pickup pose for this demo.
pickupJoints = [
90 * math.pi / 180, 19.6 * math.pi / 180, 113 * math.pi / 180,
-41 * math.pi / 180, 0
]
# Parameters for controlling the youBot's wheels: at each iteration, those values will be set for the wheels.
# They are adapted at each iteration by the code.
forwBackVel = 0 # Move straight ahead.
rightVel = 0 # Go sideways.
rotateRightVel = 0 # Rotate.
prevOrientation = 0 # Previous angle to goal (easy way to have a condition on the robot's angular speed).
prevPosition = 0 # Previous distance to goal (easy way to have a condition on the robot's speed).
# Set the arm to its starting configuration. Several orders are sent sequentially, but they must be followed
# by the simulator without being interrupted by other computations (hence the enclosing simxPauseCommunication
# calls).
res = vrep.simxPauseCommunication(
id, True) # Send order to the simulator through vrep object.
#vrchk(vrep, res); # Check the return value from the previous V-REP call (res) and exit in case of error.
for i in range(5):
res = vrep.simxSetJointTargetPosition(id, h.Arm.armJoints[i],
startingJoints[i],
vrep.simx_opmode_oneshot)
#vrchk(vrep, res, true);
res = vrep.simxPauseCommunication(id, False)
#vrchk(vrep, res);
# Initialise the plot.
plotData = False
if plotData:
# Prepare the plot area to receive three plots: what the Hokuyo sees at the top (2D map), the point cloud and
# the image of what is in front of the robot at the bottom.
ax = plt.subplot(211)
# Create a 2D mesh of points, stored in the vectors X and Y. This will be used to display the area the robot can
# see, by selecting the points within this mesh that are within the visibility range.
X, Y = np.meshgrid(
np.linspace(-5, 5, 40), np.linspace(-5.5, 2.5, 32)
) # Values selected for the area the robot will explore for this demo.
X = np.reshape(X, (-1, ), order='F') # Make a vector of the matrix X.
Y = np.reshape(Y, (-1, ), order='F')
# Make sure everything is settled before we start.
time.sleep(2)
# Retrieve the position of the gripper.
res, homeGripperPosition = vrep.simxGetObjectPosition(
id, h.Arm.ptip, h.Arm.armRef, vrep.simx_opmode_buffer)
# vrchk(vrep, res, true)
# Initialise the state machine.
fsm = 'rotate'
## Start the demo.
while True:
t = time.time() # See end of loop to see why it's useful.
if vrep.simxGetConnectionId(id) == -1:
raise ConnectionAbortedError('Lost connection to remote API.')
# Get the position and the orientation of the robot.
res, youbotPos = vrep.simxGetObjectPosition(id, h.ref, -1,
vrep.simx_opmode_buffer)
#vrchk(vrep, res, true)
res, youbotEuler = vrep.simxGetObjectOrientation(
id, h.ref, -1, vrep.simx_opmode_buffer)
#vrchk(vrep, res, true)
## Plot something if required.
if plotData:
# Read data from the depth sensor, more often called the Hokuyo (if you want to be more precise about
# the way you control the sensor, see later for the details about this line or the file
# focused/youbot_3dpointcloud.m).
# This function returns the set of points the Hokuyo saw in pts. contacts indicates, for each point, if it
# corresponds to an obstacle (the ray the Hokuyo sent was interrupted by an obstacle, and was not allowed to
# go to infinity without being stopped).
pts, contacts = h.LaserScanner.scan(vrep, vrep.simx_opmode_buffer)
# Select the points in the mesh [X, Y] that are visible, as returned by the Hokuyo (it returns the area that
# is visible, but the visualisation draws a series of points that are within this visible area).
points = np.hstack(
(h.LaserScanner.hokuyo1Pos, pts[:], h.LaserScanner.hokuyo2Pos))
polygon = path.Path(np.squeeze(np.asarray(points[0, :])),
np.squeeze(np.asarray(points[1, :])))
in_mask = polygon.contains_points(X, Y)
# Plot those points. Green dots: the visible area for the Hokuyo. Red starts: the obstacles. Red lines: the
# visibility range from the Hokuyo sensor.
# The youBot is indicated with two dots: the blue one corresponds to the rear, the red one to the Hokuyo
# sensor position.
plt.subplot(211)
plt.plot(X[in_mask], Y[in_mask], '.g')
plt.plot(pts[0, contacts], pts[1, contacts], '*r')
plt.plot(np.squeeze(np.asarray(points[0, :])),
np.squeeze(np.asarray(points[1, :])), 'r')
plt.plot(0, 0, 'ob')
plt.plot(h.LaserScanner.hokuyo1Pos[0], h.hokuyo1Pos[1], 'or')
plt.plot(h.LaserScanner.hokuyo2Pos[0], h.hokuyo2Pos[1], 'or')
plt.set_xlim([-5.5, 5.5])
plt.set_ylim([-5.5, 2.5])
angl = -math.pi / 2
## Apply the state machine.
if fsm == 'rotate':
# First, rotate the robot to go to one table.
# The rotation velocity depends on the difference between the current angle and the target.
rotateRightVel = angl - youbotEuler[2]
# When the rotation is done (with a sufficiently high precision), move on to the next state.
if ((abs(angl - youbotEuler[2]) < .1 / 180 * math.pi) and
(abs(prevOrientation - youbotEuler[2]) < .01 / 180 * math.pi)):
rotateRightVel = 0
fsm = 'drive'
prevOrientation = youbotEuler[2]
elif fsm == 'drive':
# Then, make it move straight ahead until it reaches the table (x = 3.167 m).
# The further the robot, the faster it drives. (Only check for the first dimension.)
# For the project, you should not use a predefined value, but rather compute it from your map.
# Here, the goal is to reach y = -4.5.
forwBackVel = -2 * (youbotPos[0] + 3.167)
# If the robot is sufficiently close and its speed is sufficiently low, stop it and move its arm to
# a specific location before moving on to the next state.
if (youbotPos[0] + 3.167 <
.001) and (abs(youbotPos[0] - prevPosition) < .001):
forwBackVel = 0
# Change the orientation of the camera to focus on the table (preparation for next state).
vrep.simxSetObjectOrientation(id, h.RGBDSensor.rgbdCasing,
h.ref, [0, 0, math.pi / 4],
vrep.simx_opmode_oneshot)
# Move the arm to the preset pose pickupJoints (only useful for this demo; you should compute it based
# on the object to grasp).
for i in range(5):
res = vrep.simxSetJointTargetPosition(
id, h.Arm.armJoints[i], pickupJoints[i],
vrep.simx_opmode_oneshot)
#vrchk(vrep, res, true)
fsm = 'snapshot'
prevPosition = youbotPos[0]
elif fsm == 'snapshot':
# Read data from the depth camera (Hokuyo)
# Reading a 3D image costs a lot to VREP (it has to simulate the image). It also requires a lot of
# bandwidth, and processing a 3D point cloud (for instance, to find one of the boxes or cylinders that
# the robot has to grasp) will take a long time in MATLAB. In general, you will only want to capture a 3D
# image at specific times, for instance when you believe you're facing one of the tables.
# Reduce the view angle to pi/8 in order to better see the objects. Do it only once.
# ^^^^^^ ^^^^^^^^^^ ^^^^ ^^^^^^^^^^^^^^^
# simxSetFloatSignal simx_opmode_oneshot_wait
# |
# rgbd_sensor_scan_angle
# The depth camera has a limited number of rays that gather information. If this number is concentrated
# on a smaller angle, the resolution is better. pi/8 has been determined by experimentation.
res = vrep.simxSetFloatSignal(id, 'rgbd_sensor_scan_angle',
math.pi / 8,
vrep.simx_opmode_oneshot_wait)
#vrchk(vrep, res); # Check the return value from the previous V-REP call (res) and exit in case of error.
# Ask the sensor to turn itself on, take A SINGLE POINT CLOUD, and turn itself off again.
# ^^^ ^^^^^^ ^^ ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
# simxSetIntegerSignal 1 simx_opmode_oneshot_wait
# |
# handle_xyz_sensor
res = vrep.simxSetIntegerSignal(id, 'handle_xyz_sensor', 1,
vrep.simx_opmode_oneshot_wait)
#vrchk(vrep, res);
# Then retrieve the last point cloud the depth sensor took.
# If you were to try to capture multiple images in a row, try other values than
# vrep.simx_opmode_oneshot_wait.
print('Capturing point cloud...')
pts = h.RGBDSensor.scan(vrep, vrep.simx_opmode_oneshot_wait)
# Each column of pts has [x;y;z;distancetosensor]. However, plot3 does not have the same frame of reference as
# the output data. To get a correct plot, you should invert the y and z dimensions.
# Here, we only keep points within 1 meter, to focus on the table.
pts = pts[:3, pts[3, :] < 1]
if plotData:
ax = plt.subplot(223, projection='3d')
ax.plot(pts[0, :], pts[2, :], pts[1, :], '*')
#ax. view([-169 -46])
# Read data from the RGB camera.
# This starts the robot's camera to take a 2D picture of what the robot can see.
# Reading an image costs a lot to VREP (it has to simulate the image). It also requires a lot of bandwidth,
# and processing an image will take a long time in MATLAB. In general, you will only want to capture
# an image at specific times, for instance when you believe you're facing one of the tables or a basket.
# Ask the sensor to turn itself on, take A SINGLE IMAGE, and turn itself off again.
# ^^^ ^^^^^^ ^^ ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
# simxSetIntegerSignal 1 simx_opmode_oneshot_wait
# |
# handle_rgb_sensor
res = vrep.simxSetIntegerSignal(id, 'handle_rgb_sensor', 1,
vrep.simx_opmode_oneshot_wait)
#vrchk(vrep, res); # Check the return value from the previous V-REP call (res) and exit in case of error.
# Then retrieve the last picture the camera took. The image must be in RGB (not gray scale).
# ^^^^^^^^^^^^^^^^^^^^^^^^^ ^^^^^^ ^^^
# simxGetVisionSensorImage2 h.rgbSensor 0
# If you were to try to capture multiple images in a row, try other values than vrep.simx_opmode_oneshot_wait.
print('Capturing image...')
res, resolution, image = vrep.simxGetVisionSensorImage(
id, h.RGBDSensor.rgbSensor, 0, vrep.simx_opmode_oneshot_wait)
#vrchk(vrep, res);
width, height = resolution
print('Captured {} pixels ({} x {}).'.format(
width * height, width, height))
image = np.reshape(image, (height, width, 3))
# Finally, show the image.
if plotData:
plt.imshow(image)
# Next state.
fsm = 'extend'
elif fsm == 'extend':
# Move the arm to face the object.
# Get the arm position.
res, tpos = vrep.simxGetObjectPosition(id, h.Arm.ptip,
h.Arm.armRef,
vrep.simx_opmode_buffer)
#vrchk(vrep, res, true)
# If the arm has reached the wanted position, move on to the next state.
# Once again, your code should compute this based on the object to grasp.
if np.linalg.norm(
np.array(tpos) -
np.array([0.3259, -0.0010, 0.2951])) < .002:
# Set the inverse kinematics (IK) mode to position AND orientation (km_mode = 2).
res = vrep.simxSetIntegerSignal(id, 'km_mode', 2,
vrep.simx_opmode_oneshot_wait)
#vrchk(vrep, res, true);
fsm = 'reachout'
elif fsm == 'reachout':
# Move the gripper tip along a line so that it faces the object with the right angle.
# Get the arm tip position. The arm is driven only by the position of the tip, not by the angles of
# the joints, except if IK is disabled.
# Following the line ensures the arm attacks the object with the right angle.
res, tpos = vrep.simxGetObjectPosition(id, h.Arm.ptip,
h.Arm.armRef,
vrep.simx_opmode_buffer)
#vrchk(vrep, res, true);
# If the tip is at the right position, go on to the next state. Again, this value should be computed based
# on the object to grasp and on the robot's position.
if tpos[0] > .39:
fsm = 'grasp'
# Move the tip to the next position along the line.
tpos[0] = tpos[0] + .01
res = vrep.simxSetObjectPosition(id, h.Arm.ptarget, h.Arm.armRef,
tpos, vrep.simx_opmode_oneshot)
#vrchk(vrep, res, true)
elif fsm == 'grasp':
# Grasp the object by closing the gripper on it.
# Close the gripper. Please pay attention that it is not possible to adjust the force to apply:
# the object will sometimes slip from the gripper!
res = vrep.simxSetIntegerSignal(id, 'gripper_open', 0,
vrep.simx_opmode_oneshot_wait)
#vrchk(vrep, res);
# Make MATLAB wait for the gripper to be closed. This value was determined by experiments.
time.sleep(2)
# Disable IK; this is used at the next state to move the joints manually.
res = vrep.simxSetIntegerSignal(id, 'km_mode', 0,
vrep.simx_opmode_oneshot_wait)
#vrchk(vrep, res)
fsm = 'backoff'
elif fsm == 'backoff':
for i in range(5):
res = vrep.simxSetJointTargetPosition(id, h.Arm.armJoints[i],
startingJoints[i],
vrep.simx_opmode_oneshot)
#vrchk(vrep, res, true)
# Get the gripper position and check whether it is at destination (the original position).
res, tpos = vrep.simxGetObjectPosition(id, h.Arm.ptip,
h.Arm.armRef,
vrep.simx_opmode_buffer)
#vrchk(vrep, res, true)
if np.linalg.norm(
np.array(tpos) - np.array(homeGripperPosition)) < .02:
# Open the gripper when the arm is above its base.
res = vrep.simxSetIntegerSignal(
id, 'gripper_open', 1, vrep.simx_opmode_oneshot_wait)
#vrchk(vrep, res);
if np.linalg.norm(
np.array(tpos) - np.array(homeGripperPosition)) < .002:
fsm = 'finished'
elif fsm == 'finished':
# Demo done: exit the function.
time.sleep(3)
break
else:
raise ValueError('Unknown state: ' + fsm)
# Update wheel velocities using the global values (whatever the state is).
h.drive(vrep, forwBackVel, rightVel, rotateRightVel)
# Make sure that we do not go faster than the physics simulation (it is useless to go faster).
elapsed = time.time() - t
timeleft = timestep - elapsed
if (timeleft > 0):
time.sleep(min(timeleft, .01))
