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): 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()
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))