def close(self): sim.simxAddStatusbarMessage(self.clientID, 'Goodbye CoppeliaSim!', sim.simx_opmode_oneshot) sim.simxGetPingTime(self.clientID) sim.simxStopSimulation(self.clientID, sim.simx_opmode_oneshot) time.sleep(2) sim.simxFinish(self.clientID)
def stop_simulation(self): # Now send some data to CoppeliaSim in a non-blocking fashion: sim.simxAddStatusbarMessage(self.clientID, 'Quit', sim.simx_opmode_oneshot) # Before closing the connection to CoppeliaSim, make sure that the last command sent out had time to arrive. You can guarantee this with (for example): sim.simxGetPingTime(self.clientID) # Now close the connection to CoppeliaSim: sim.simxFinish(self.clientID)
def activate_magnet(teste): status = 'active' if teste else 'deactive' print("Magnet is {}".format(status)) try: err_code, arm_actuator = sim.simxGetObjectHandle( client.id, "suctionPad", sim.simx_opmode_oneshot) if err_code != 0: raise Exception("teste") sim.simxAddStatusbarMessage( client.id, 'Crab position request is: {}'.format(status), sim.simx_opmode_oneshot) err_code = sim.simxSetUserParameter(client.id, arm_actuator, status, sim.simx_opmode_streaming) except Exception: print("Code error is {}".format(err_code))
def move_arm(rotation): radians = (2 * math.pi * float(rotation)) / 100 print("degrees are {}".format(str((360 * radians) / (2 * math.pi)))) try: err_code, arm_actuator = sim.simxGetObjectHandle( client.id, "armActuator", sim.simx_opmode_blocking) if err_code != 0: raise Exception("teste") sim.simxAddStatusbarMessage( client.id, 'Command angle rotation send: {}'.format(str(radians)), sim.simx_opmode_oneshot) err_code = sim.simxSetJointTargetPosition(client.id, arm_actuator, radians, sim.simx_opmode_streaming) except Exception: print("Code error is {}".format(err_code))
def move_hoist(height): final_height = (-0.68 * float(height)) / 100 print("height is {}".format(str(final_height))) try: err_code, arm_actuator = sim.simxGetObjectHandle( client.id, "UpperMass", sim.simx_opmode_blocking) if err_code != 0: raise Exception("teste") sim.simxAddStatusbarMessage( client.id, 'Command angle rotation send: {}'.format(str(final_height)), sim.simx_opmode_oneshot) err_code = sim.simxSetJointTargetPosition(client.id, arm_actuator, final_height, sim.simx_opmode_streaming) except Exception: print("Code error is {}".format(err_code))
def move_crab(position): final_position = (0.61 * float(position)) / 100 print("Position is {}".format(str(final_position))) try: err_code, arm_actuator = sim.simxGetObjectHandle( client.id, "CrabMove", sim.simx_opmode_blocking) if err_code != 0: raise Exception("teste") sim.simxAddStatusbarMessage( client.id, 'Crab position request is: {}'.format(str(final_position)), sim.simx_opmode_oneshot) err_code = sim.simxSetJointTargetPosition(client.id, arm_actuator, final_position, sim.simx_opmode_streaming) except Exception: print("Code error is {}".format(err_code))
def movement(): sim.simxAddStatusbarMessage(clientID, 'Connected and running', sim.simx_opmode_oneshot) vel = 10 print("Forward") sim.simxSetJointTargetVelocity(clientID, left_motor_handle, -vel, sim.simx_opmode_streaming) sim.simxSetJointTargetVelocity(clientID, right_motor_handle, -vel, sim.simx_opmode_streaming) time.sleep(40) print("Left") sim.simxSetJointTargetVelocity(clientID, left_motor_handle, vel, sim.simx_opmode_streaming) time.sleep(10) sim.simxSetJointTargetVelocity(clientID, left_motor_handle, 0, sim.simx_opmode_streaming) sim.simxSetJointTargetVelocity(clientID, right_motor_handle, 0, sim.simx_opmode_streaming) print("Stop")
def main(): np.set_printoptions(precision=3, suppress=True) # Restrict decimals in console output dof = 6 home_position, screw = ur5_params() # Get UR5 parameters theta_0 = np.array([0, 0, 0, 0, 0, 0]) # Initial position theta_d = np.array([0,-m.pi/2, 0, 0, m.pi/2, 0]) # Desired position, converted to x_d in the solver T_0 = compute_T(screw, theta_0, dof, home_position) T_d = compute_T(screw, theta_d, dof, home_position) print("Home position: \n", T_0, "\n") print("Desired position: \n", T_d, "\n") # Find solution to the system theta, delta, n = root_finding(theta_0, theta_d, 20, dof, home_position, screw) T_theta = compute_T(screw, theta, dof, home_position) print('Solution : \n', theta, '\n', 'Transformation : \n', T_theta, '\n') R = T_theta[0:3][0:3] # Get RPY for the solution r_roll = m.atan2(R[2][1],R[2][2]) r_pitch = m.atan2(-R[2][0],m.sqrt(R[2][2]*R[2][2] + R[2][1]*R[2][1])) r_yaw = m.atan2(R[1][0],R[0][0]) # Begin connection with coppeliaSim. # Robot simulation must be running in coppeliaSim to connect sim.simxFinish(-1) # just in case, close all opened connections clientID=sim.simxStart('127.0.0.1',19997,True,True,5000,5) # Connect to CoppeliaSim # clientID stores the ID assingned by coppeliaSim, if the connection failed it will be assigned -1 if clientID!=-1: print ('Connected to remote API server') # Get ID handles for each joint of the robot res,UR5_joint1 =sim.simxGetObjectHandle( clientID, 'UR5_joint1', sim.simx_opmode_blocking) res,UR5_joint2 =sim.simxGetObjectHandle( clientID, 'UR5_joint2', sim.simx_opmode_blocking) res,UR5_joint3 =sim.simxGetObjectHandle( clientID, 'UR5_joint3', sim.simx_opmode_blocking) res,UR5_joint4 =sim.simxGetObjectHandle( clientID, 'UR5_joint4', sim.simx_opmode_blocking) res,UR5_joint5 =sim.simxGetObjectHandle( clientID, 'UR5_joint5', sim.simx_opmode_blocking) res,UR5_joint6 =sim.simxGetObjectHandle( clientID, 'UR5_joint6', sim.simx_opmode_blocking) res,UR5_endEffector =sim.simxGetObjectHandle( clientID, 'UR5_connection', sim.simx_opmode_blocking) # Store the handles as a list UR5 = [UR5_joint1, UR5_joint2, UR5_joint3, UR5_joint4, UR5_joint5, UR5_joint6] # Just grab the first 3 for now to test # Get current coordinates of each joint UR5_q = [] for joint in UR5: res, value = sim.simxGetJointPosition(clientID, joint, sim.simx_opmode_oneshot) UR5_q.append(value) # Store current values # Set new coordinates for the robot in coppeliaSim # Add time delays so the animation can be displayed correctly steps = 100 position_desired = theta for t in range(steps): i = 0 k = 0 for joint in UR5: sim.simxSetJointTargetPosition(clientID, joint, t*(position_desired[i])/steps, sim.simx_opmode_streaming) res, value = sim.simxGetJointPosition(clientID, joint, sim.simx_opmode_oneshot) if t == 0 or t == 99: k += 1 res, position = sim.simxGetObjectPosition(clientID, UR5_endEffector, UR5_joint1, sim.simx_opmode_oneshot) res, orientation = sim.simxGetObjectOrientation(clientID, UR5_endEffector, UR5_joint1, sim.simx_opmode_oneshot) i += 1 time.sleep(2/steps) # Convert robot angles to the 4x4 matrix representation for comparison c1, s1 = np.cos(orientation[0]), np.sin(orientation[0]) c2, s2 = np.cos(orientation[1]), np.sin(orientation[1]) c3, s3 = np.cos(orientation[2]), np.sin(orientation[2]) R_ur5 = np.block([ [c2 * c3, -c2 * s3, s2], [c1 * s3 + c3 * s1 * s2, c1 * c3 - s1 * s2 * s3, -c2 * s1], [s1 * s3 - c1 * c3 * s2, c3 * s1 + c1 * s2 * s3, c1 * c2] ]) p_ur5 = np.array(position).reshape((3,1)) T_ur5 = np.block([ [R_ur5, p_ur5], [0, 0, 0, 1] ]) print('\n Robot coordinates: \n ', T_ur5 ) # print('\n Absolute Error : \n', abs(T_theta - T_ur5)) time.sleep(1) # print('\n Returning to home position...') # for joint in UR5: # sim.simxSetJointTargetPosition(clientID, joint, 0, sim.simx_opmode_streaming) # Now send some data to CoppeliaSim in a non-blocking fashion: sim.simxAddStatusbarMessage(clientID,'Excuting forward kinematics in Python',sim.simx_opmode_oneshot) # Before closing the connection to CoppeliaSim, make sure that the last command sent out had time to arrive. You can guarantee this with (for example): sim.simxGetPingTime(clientID) # Now close the connection to CoppeliaSim: sim.simxFinish(clientID)
import random import time import keyboard import math #from winsound import Beep print('Start') vrep.simxFinish(-1) clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5) if clientID != -1: print('Connected to remote API server') res = vrep.simxAddStatusbarMessage(clientID, "test-25", vrep.simx_opmode_oneshot) if res not in (vrep.simx_return_ok, vrep.simx_return_novalue_flag): print("Could not add a message to the status bar.") opmode = vrep.simx_opmode_oneshot_wait STREAMING = vrep.simx_opmode_streaming vrep.simxStartSimulation(clientID, opmode) ret, joint1 = vrep.simxGetObjectHandle(clientID, "Revolute3_1", opmode) ret, joint2 = vrep.simxGetObjectHandle(clientID, "Revolute3_2", opmode) ret, joint3 = vrep.simxGetObjectHandle(clientID, "Revolute3_3", opmode) ret, joint4 = vrep.simxGetObjectHandle(clientID, "Revolute3_4", opmode) ret, joint5 = vrep.simxGetObjectHandle(clientID, "Revolute3_5", opmode) ret, joint6 = vrep.simxGetObjectHandle(clientID, "Revolute3_6", opmode) ret, joint01 = vrep.simxGetObjectHandle(clientID, "joint0_1", opmode)
sim.simx_opmode_blocking) #print('returncode3=',return_code) return_code = sim.simxSetJointTargetPosition(clientID, UR3_joint4, theta[3], sim.simx_opmode_blocking) #print('returncode4=',return_code) return_code = sim.simxSetJointTargetPosition(clientID, UR3_joint5, theta[4], sim.simx_opmode_blocking) #print('returncode5=',return_code) return_code = sim.simxSetJointTargetPosition(clientID, UR3_joint6, theta[5], sim.simx_opmode_blocking) #print('returncode6=',return_code) #sim.simxPauseCommunication(clientID,False) if (FvI == 'F' or FvI == 'f'): sim.simxSetObjectPosition(clientID, POI, -1, XYZ, sim.simx_opmode_blocking) sim.simxAddStatusbarMessage(clientID, 'End of Code', sim.simx_opmode_oneshot) print('End of Code') res = input('reset? y/n:') #print(res) if (res == 'y' or res == 1): return_code = sim.simxSetJointTargetPosition(clientID, UR3_joint1, 0, sim.simx_opmode_blocking) return_code = sim.simxSetJointTargetPosition(clientID, UR3_joint2, 0, sim.simx_opmode_blocking) return_code = sim.simxSetJointTargetPosition(clientID, UR3_joint3, 0, sim.simx_opmode_blocking) return_code = sim.simxSetJointTargetPosition(clientID, UR3_joint4, 0, sim.simx_opmode_blocking) return_code = sim.simxSetJointTargetPosition(clientID, UR3_joint5, 0, sim.simx_opmode_blocking) return_code = sim.simxSetJointTargetPosition(clientID, UR3_joint6, 0,
def detect_object(sensorHolder): return_value, detectionState, detectionPoint, detectedObjectHandle, detectedSurfaceNormalVector = sim.simxReadProximitySensor( clientID, leftSensorFront, sim.simx_opmode_buffer) print(detectionPoint) print(detectionState) print('Program started') sim.simxFinish(-1) # just in case, close all opened connections clientID = sim.simxStart('127.0.0.1', 19990, True, True, 5000, 5) # Connect to CoppeliaSim if clientID != -1: print('Connected to remote API server') # Now send some data to CoppeliaSim in a non-blocking fashion: sim.simxAddStatusbarMessage(clientID, 'Hello from python!', sim.simx_opmode_oneshot) return_value, leftSensorFront = sim.simxGetObjectHandle( clientID, 'Proximity_sensor_front_left', sim.simx_opmode_blocking) return_value, leftSensorBack = sim.simxGetObjectHandle( clientID, 'Proximity_sensor_back_left', sim.simx_opmode_blocking) return_value, rightSensorFront = sim.simxGetObjectHandle( clientID, 'Proximity_sensor_front_right', sim.simx_opmode_blocking) return_value, rightSensorBack = sim.simxGetObjectHandle( clientID, 'Proximity_sensor_back_right', sim.simx_opmode_blocking) return_value, detectionState, detectionPoint, detectedObjectHandle, detectedSurfaceNormalVector = sim.simxReadProximitySensor( clientID, leftSensorFront, sim.simx_opmode_streaming) while (1): detect_object(leftSensorFront)
import random import time import keyboard import math #from winsound import Beep print('Start') vrep.simxFinish(-1) clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5) if clientID != -1: print('Connected to remote API server') res = vrep.simxAddStatusbarMessage(clientID, "control_1", vrep.simx_opmode_oneshot) if res not in (vrep.simx_return_ok, vrep.simx_return_novalue_flag): print("Could not add a message to the status bar.") opmode = vrep.simx_opmode_oneshot_wait STREAMING = vrep.simx_opmode_streaming vrep.simxStartSimulation(clientID, opmode) ret, joint01 = vrep.simxGetObjectHandle(clientID, "joint1", opmode) ret, joint02 = vrep.simxGetObjectHandle(clientID, "joint2", opmode) ret, joint03 = vrep.simxGetObjectHandle(clientID, "joint3", opmode) ret, joint04 = vrep.simxGetObjectHandle(clientID, "joint4", opmode) ret, joint05 = vrep.simxGetObjectHandle(clientID, "joint5", opmode) ret, joint06 = vrep.simxGetObjectHandle(clientID, "joint6", opmode) degset01 = 0 degset02 = 0
def show_msg(message): """ send a message for printing in V-REP """ sim.simxAddStatusbarMessage(clientID, message, WAIT) return
import sim as vrep import math import random import time import math def moving(x,y): a=0.4 b=0.4 c=math.pow((math.pow(x,2)+math.pow(y,2)),0.5) s=(a+b+c)/2 area=math.pow((s*(s-a)*(s-b)*(s-c)),0.5) h=area/(2*c) deg1_base=math.atan(x/y) if x<0 and y<0 : deg_base1=deg1_base+math.pi deg1_tri=math.asin(h/a) deg1=deg1_base+deg1_tri deg2=math.pi-(0.5*math.pi-deg1_tri)-math.acos(h/b) deg3=deg2-deg1 vrep.simxSetJointTargetPosition(clientID,joint01,deg1,opmode) vrep.simxSetJointTargetPosition(clientID,joint02,-deg2,opmode) vrep.simxSetJointTargetPosition(clientID,joint03,deg3,opmode) print ('Start') vrep.simxFinish(-1) clientID = vrep.simxStart('192.168.0.10', 19997, True, True, 5000, 5) if clientID != -1: print ('Connected to remote API server')
blockLocalDelivery, myDirection) entregar_cubo_colorido(cube) ########################################## print('Program started') sim.simxFinish(-1) # just in case, close all opened connections clientID = sim.simxStart('127.0.0.1', 19999, True, True, 5000, 5) robotname = 'Robot' #targetname = 'Target1' if clientID != -1: sim.simxStartSimulation(clientID, sim.simx_opmode_oneshot_wait) print('Connected to remote API server') sim.simxAddStatusbarMessage(clientID, 'Funcionando...', sim.simx_opmode_oneshot_wait) time.sleep(0.02) # Coletar handles #Robô erro, robot = sim.simxGetObjectHandle(clientID, robotname, sim.simx_opmode_blocking) erro, robo = sim.simxGetObjectHandle(clientID, 'S_Base', sim.simx_opmode_blocking) #Motores [erro, robotLeftMotor] = sim.simxGetObjectHandle(clientID, 'Revolute_joint2', sim.simx_opmode_blocking) [erro, robotRightMotor] = sim.simxGetObjectHandle(clientID, 'Revolute_joint1', sim.simx_opmode_blocking)
print('Program started') vrep.simxFinish(-1) clientID = vrep.simxStart('127.0.0.1', 19999, True, True, 5000, 5) if clientID != -1: print('Connected to remote API server!') res, v0 = vrep.simxGetObjectHandle(clientID, 'Vision_sensor', vrep.simx_opmode_oneshot_wait) res, resolution, imag = vrep.simxGetVisionSensorImage( clientID, v0, 0, vrep.simx_opmode_streaming) imcount = 0 while (vrep.simxGetConnectionId(clientID) != -1): res, resolution, image = vrep.simxGetVisionSensorImage( clientID, v0, 0, vrep.simx_opmode_buffer) if res == vrep.simx_return_ok: imcount = imcount + 1 res, rgb_resolution, rgb_image = vrep.simxGetVisionSensorImage( clientID, v0, 0, vrep.simx_opmode_buffer) rgb_img = np.array(rgb_image, dtype=np.uint8) rgb_img.resize([rgb_resolution[1], rgb_resolution[0], 3]) rgb_img = cv2.flip(rgb_img, 0) cv2.imshow("RGB_Image", rgb_img) vrep.simxAddStatusbarMessage(clientID, 'hello world', vrep.simx_opmode_oneshot) cv2.waitKey(1) print(imcount) else: print('Failed to show rgb and depth image') else: print('Failed to connect to vrep API server!') print('Program ended!')
-0.2, -0.4, -0.6, -0.8, -1, -1.2, -1.4, -1.6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] braitenbergR = [ -1.6, -1.4, -1.2, -1, -0.8, -0.6, -0.4, -0.2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] v0 = 2 # Client Connection clientID = sim.simxStart(serverIP, serverPort, True, True, timeOut, 5) if clientID != -1: # Now send some data to CoppeliaSim in a non-blocking fashion: print('Connected to remote API server!') sim.simxAddStatusbarMessage(clientID, 'Connected to remote API client!', sim.simx_opmode_oneshot) # Motors Initialization (remoteApi) ret, pioneer.leftMotor.Handle = sim.simxGetObjectHandle( clientID, 'Pioneer_p3dx_leftMotor', sim.simx_opmode_oneshot_wait) if ret != 0: print("'Pioneer_p3dx_leftMotor' not found!") else: print("Linked to the 'Pioneer_p3dx_leftMotor' objHandle!") ret, pioneer.rightMotor.Handle = sim.simxGetObjectHandle( clientID, 'Pioneer_p3dx_rightMotor', sim.simx_opmode_oneshot_wait) if ret != 0: print("'Pioneer_p3dx_rightMotor' not found!") else:
def init(_clientID, robotname): global robot, robo, robotLeftMotor, robotRightMotor global clientID global paEsquerda, paDireita global elevador, irRight, irLeft global color_sensor_Left, color_sensor_Right global positionrobot, angLeft, angRight, orientationrobot clientID = _clientID sim.simxStartSimulation(clientID, sim.simx_opmode_oneshot_wait) print('Connected to remote API server') sim.simxAddStatusbarMessage(clientID, 'Funcionando...', sim.simx_opmode_oneshot_wait) time.sleep(0.02) # Coletar handles #Robô erro, robot = sim.simxGetObjectHandle(clientID, robotname, sim.simx_opmode_blocking) erro, robo = sim.simxGetObjectHandle(clientID, 'S_Base', sim.simx_opmode_blocking) #Motores [erro, robotLeftMotor] = sim.simxGetObjectHandle(clientID, 'Revolute_joint2', sim.simx_opmode_blocking) [erro, robotRightMotor] = sim.simxGetObjectHandle(clientID, 'Revolute_joint1', sim.simx_opmode_blocking) #Garra erro, paEsquerda = sim.simxGetObjectHandle(clientID, 'joint_pa_esquerda_garra', sim.simx_opmode_blocking) erro, paDireita = sim.simxGetObjectHandle(clientID, 'joint_pa_direita_garra', sim.simx_opmode_blocking) erro, elevador = sim.simxGetObjectHandle(clientID, 'joint_acoplador_garra', sim.simx_opmode_blocking) #Sensores erro, irRight = sim.simxGetObjectHandle(clientID, 'Sensor_IR_direito', sim.simx_opmode_blocking) erro, irLeft = sim.simxGetObjectHandle(clientID, 'Sensor_IR_esquerdo', sim.simx_opmode_blocking) erro, color_sensor_Left = sim.simxGetObjectHandle(clientID, 'Sensor_cor_esquerda', sim.simx_opmode_blocking) erro, color_sensor_Right = sim.simxGetObjectHandle( clientID, 'Sensor_cor_direita', sim.simx_opmode_blocking) # Criar stream de dados [erro, positionrobot] = sim.simxGetObjectPosition(clientID, robot, -1, sim.simx_opmode_streaming) erro, angLeft = sim.simxGetJointPosition(clientID, robotLeftMotor, sim.simx_opmode_streaming) erro, angRight = sim.simxGetJointPosition(clientID, robotRightMotor, sim.simx_opmode_streaming) [erro, orientationrobot ] = sim.simxGetObjectOrientation(clientID, robot, -1, sim.simx_opmode_streaming) sim.simxReadProximitySensor(clientID, irRight, sim.simx_opmode_streaming) sim.simxReadProximitySensor(clientID, irLeft, sim.simx_opmode_streaming) sim.simxGetVisionSensorImage(clientID, color_sensor_Left, 0, sim.simx_opmode_streaming) sim.simxGetVisionSensorImage(clientID, color_sensor_Right, 0, sim.simx_opmode_streaming)