def __init__(self, clientid): self.clientID = clientid res, self.rollJoint = vrep.simxGetObjectHandle( self.clientID, 'RJ', vrep.simx_opmode_blocking) res, self.gripperJoint1 = vrep.simxGetObjectHandle( self.clientID, 'SJ1', vrep.simx_opmode_blocking) res, self.gripperJoint2 = vrep.simxGetObjectHandle( self.clientID, 'SJ2', vrep.simx_opmode_blocking) vrep.simxSetJointPosition(self.clientID, self.rollJoint, 0, vrep.simx_opmode_streaming) vrep.simxSetJointPosition(self.clientID, self.gripperJoint1, 0, vrep.simx_opmode_streaming) vrep.simxSetJointPosition(self.clientID, self.gripperJoint2, 0, vrep.simx_opmode_streaming) res, pos1 = vrep.simxGetJointPosition(self.clientID, self.gripperJoint1, vrep.simx_opmode_streaming) res, pos2 = vrep.simxGetJointPosition(self.clientID, self.gripperJoint2, vrep.simx_opmode_streaming) res, pos3 = vrep.simxGetJointPosition(self.clientID, self.rollJoint, vrep.simx_opmode_streaming)
def send_gear_commands(clientID, target_degree1, target_degree2, gearHandle1, gearHandle2): RAD = 180 / math.pi lastCmdTime = vrep.simxGetLastCmdTime(clientID) vrep.simxSynchronousTrigger(clientID) count = 0 while vrep.simxGetConnectionId(clientID) != -1: count += 1 if count > 20: break currCmdTime = vrep.simxGetLastCmdTime(clientID) dt = currCmdTime - lastCmdTime ret, gear_pos1 = vrep.simxGetJointPosition(clientID, gearHandle1, vrep.simx_opmode_buffer) ret, gear_pos2 = vrep.simxGetJointPosition(clientID, gearHandle2, vrep.simx_opmode_buffer) degree1 = round(gear_pos1 * RAD, 2) degree2 = round(gear_pos2 * RAD, 2) print(degree1, degree2) if abs(degree1 - target_degree1) < 0.5 and abs(degree2 - target_degree2) < 0.5: break vrep.simxPauseCommunication(clientID, True) vrep.simxSetJointTargetPosition(clientID, gearHandle1, target_degree1 / RAD, vrep.simx_opmode_oneshot) vrep.simxSetJointTargetPosition(clientID, gearHandle2, target_degree2 / RAD, vrep.simx_opmode_oneshot) vrep.simxPauseCommunication(clientID, False) lastCmdTime = currCmdTime vrep.simxSynchronousTrigger(clientID)
def run(self): global left_wheel_angle, right_wheel_angle, pos_x, pos_y, angle _, position = vrep.simxGetObjectPosition(self.clientID, self.robot_handle, -1, vrep.simx_opmode_buffer) _, eulerAngles = vrep.simxGetObjectOrientation(self.clientID, self.robot_handle, -1, vrep.simx_opmode_buffer) _, new_right_wheel_angle = vrep.simxGetJointPosition( self.clientID, self.right_motor_handle, vrep.simx_opmode_streaming) _, new_left_wheel_angle = vrep.simxGetJointPosition( self.clientID, self.left_motor_handle, vrep.simx_opmode_streaming) pos_x = position[0] pos_y = position[1] angle = eulerAngles[2] new_left_wheel_angle = (new_left_wheel_angle + 2 * PI) % (2 * PI) new_right_wheel_angle = (new_right_wheel_angle + 2 * PI) % (2 * PI) left_wheel_angle = new_left_wheel_angle right_wheel_angle = new_right_wheel_angle while True: self.update() time.sleep(0.1)
def step(self, action): action = np.clip(action, *self.action_bound) self.jointConfig1 = np.zeros((self.jointNum, )) self.jointConfig2 = np.zeros((self.jointNum, )) for i in range(self.jointNum): _, jpos = vrep.simxGetJointPosition(self.clientID, self.robot1_jointHandle[i], vrep.simx_opmode_blocking) self.jointConfig1[i] = jpos for i in range(self.jointNum): _, jpos = vrep.simxGetJointPosition(self.clientID, self.robot2_jointHandle[i], vrep.simx_opmode_blocking) self.jointConfig2[i] = jpos # print(self.jointConfig1) self.jointConfig1 += action[:3] * self.dt self.jointConfig2 += action[-3:] * self.dt # self.jointConfig1 %= np.pi*2 # self.jointConfig2 %= np.pi*2 self.jointConfig1 = np.clip(self.jointConfig1, *self.joint_bound) self.jointConfig2 = np.clip(self.jointConfig2, *self.joint_bound) self.moveto(self.jointConfig1, self.jointConfig2) # time.sleep(0.1) s_ = self.getState_v1() r = self._r_func() return s_, r, self.get_point, self.get_obstacles
def handle_output(self): # return whatever state information you need to get the error you want # this will get you the information for the center of the object. If you # want something like the position of the end of an arm, you will need # to do some calculations, or just make a dummy object, attach it to # the point you want, and get the position of the dummy object #err, hand_pos = vrep.simxGetObjectPosition(self.cid, self.hand, -1, # vrep.simx_opmode_oneshot) #err, hand_ori = vrep.simxGetObjectOrientation(self.cid, self.hand, -1, # vrep.simx_opmode_oneshot) err, hand_ori = vrep.simxGetJointPosition(self.cid, self.hand_joint, vrep.simx_opmode_oneshot) err, arm_ori = vrep.simxGetJointPosition(self.cid, self.arm_joint, vrep.simx_opmode_oneshot) err, hand_vel = vrep.simxGetObjectFloatParameter(self.cid, self.hand_joint, 2012, vrep.simx_opmode_oneshot) err, arm_vel = vrep.simxGetObjectFloatParameter(self.cid, self.arm_joint, 2012, vrep.simx_opmode_oneshot) err, hand_pos = vrep.simxGetObjectPosition(self.cid, self.hand, -1, vrep.simx_opmode_oneshot) err, target_pos = vrep.simxGetObjectPosition(self.cid, self.target, -1, vrep.simx_opmode_oneshot) return [arm_ori, hand_ori, arm_vel, hand_vel, hand_pos[0], hand_pos[2], target_pos[0], target_pos[1]]
def open_hand( self): #this should work to place gripper at initial open position _, dist = vrep.simxGetJointPosition(self.client_id, self.motor_handle, \ vrep.simx_opmode_blocking) vrep.simxSetJointForce(self.client_id, self.motor_handle, 20, vrep.simx_opmode_blocking) vrep.simxSetJointTargetVelocity(self.client_id, self.motor_handle, -0.5, \ vrep.simx_opmode_blocking) vrep.simxSynchronousTrigger(self.client_id) vrep.simxGetPingTime(self.client_id) # start_time = time.time() while dist > -1e-06: #8.22427227831e-06:#-1.67e-06: # Block until gripper is fully open _, dist = vrep.simxGetJointPosition(self.client_id, self.motor_handle, \ vrep.simx_opmode_blocking) vrep.simxSetJointTargetVelocity(self.client_id, self.motor_handle, -0.5, \ vrep.simx_opmode_blocking) vrep.simxSynchronousTrigger(self.client_id) vrep.simxGetPingTime(self.client_id) # end_time = time.time() # if (end_time-start_time > 2): # self.restart() vrep.simxSetJointTargetVelocity(self.client_id, self.motor_handle, 0.0, \ vrep.simx_opmode_blocking) vrep.simxSynchronousTrigger(self.client_id) vrep.simxGetPingTime(self.client_id)
def __init__(self, clientID): ''' get handles set initial state ''' self.clientID = clientID self.jointHandles = [-1, -1, -1, -1] #self.collisionHandles = [-1,-1] #self.state = [False,False] jointInd = [0, 1, 2, 3] jointName = ['uarm_motor' + str(i + 1) for i in range(4)] self.jointDict = dict(zip(jointInd, jointName)) # joint handle for i in range(4): res, self.jointHandles[i] = vrep.simxGetObjectHandle( clientID, self.jointDict[i], vrep.simx_opmode_blocking) if res != 0: print('Failed to find: {}'.format(self.jointDict[i])) # collision handle # for i in range(2): # self.returnCode,self.collisionHandles[i] = vrep.simxGetCollisionHandle(clientID,'Collision'+str(i+1),vrep.simx_opmode_blocking) print('handles get') # initialize for i in range(4): vrep.simxGetJointPosition(self.clientID, self.jointHandles[i], vrep.simx_opmode_streaming) print('initialzied')
def on_press(key): try: if key == keyboard.Key.left: velEsq = -0.5 velDir = 0.5 elif key == keyboard.Key.right: velEsq = 0.5 velDir = -0.5 elif key == keyboard.Key.space: velEsq = 0 velDir = 0 elif key == keyboard.Key.esc: # Stop listener sys.exit(0) return False vrep.simxSetJointTargetVelocity(clientID, rightMotorHandle, velDir, vrep.simx_opmode_streaming) vrep.simxSetJointTargetVelocity(clientID, leftMotorHandle, velEsq, vrep.simx_opmode_streaming) thetaDir = vrep.simxGetJointPosition(clientID, rightMotorHandle, vrep.simx_opmode_streaming)[1] thetaEsq = vrep.simxGetJointPosition(clientID, leftMotorHandle, vrep.simx_opmode_streaming)[1] localizacao.setAngulos(thetaDir, thetaEsq) except AttributeError: print('special key {0} pressed'.format(key))
def testGripper(gripper_handle, gripperID, gripper_Pos, clientID): time.sleep(1) # Close the gripper # vrep.simxSetJointTargetPosition(clientID, gripper_handle, -gripper_Pos, vrep.simx_opmode_oneshot) # time.sleep(1) result, theta = vrep.simxGetJointPosition(clientID, gripper_handle, vrep.simx_opmode_blocking) print("Theta is {}".format(theta)) # Open the gripper vrep.simxSetJointTargetPosition(clientID, gripper_handle, theta - np.pi, vrep.simx_opmode_oneshot) time.sleep(1) result, theta = vrep.simxGetJointPosition(clientID, gripper_handle, vrep.simx_opmode_blocking) print("Theta is {}".format(theta)) # Hold the gripper vrep.simxSetJointTargetPosition(clientID, gripper_handle, 0, vrep.simx_opmode_oneshot) time.sleep(1)
def wait_till_stream(self): ret = vrep.simx_return_illegal_opmode_flag while ret != vrep.simx_return_ok: ret, ori = vrep.simxGetObjectOrientation(self.client_id, self.body_handle, -1, vrep.simx_opmode_buffer) self.logger.info("orient:%s" % str(ori)) ret = vrep.simx_return_illegal_opmode_flag while ret != vrep.simx_return_ok: ret, _ = vrep.simxGetObjectPosition(self.client_id, self.body_handle, -1, vrep.simx_opmode_buffer) ret = vrep.simx_return_illegal_opmode_flag while ret != vrep.simx_return_ok: ret, _ = vrep.simxGetJointPosition(self.client_id, self.left_joint, vrep.simx_opmode_buffer) ret = vrep.simx_return_illegal_opmode_flag while ret != vrep.simx_return_ok: ret, _ = vrep.simxGetJointPosition(self.client_id, self.right_joint, vrep.simx_opmode_buffer) ret = vrep.simx_return_illegal_opmode_flag while ret != vrep.simx_return_ok: ret, _ = vrep.simxGetObjectFloatParameter(self.client_id, self.right_joint, 2012, vrep.simx_opmode_buffer) ret = vrep.simx_return_illegal_opmode_flag while ret != vrep.simx_return_ok: ret, _ = vrep.simxGetObjectFloatParameter(self.client_id, self.right_joint, 2012, vrep.simx_opmode_buffer)
def __init__(self, client_id, name): self.client_id = client_id self.name = name err, self.handle = vrep.simxGetObjectHandle( client_id, name, vrep.simx_opmode_oneshot_wait) vrep.simxGetJointPosition(self.client_id, self.handle, vrep.simx_opmode_streaming)
def reset(self): self.get_point = False self.grab_counter = 0 self.get_obstacles = False self.obstacles_counter = 0 _ = vrep.simxSetObjectPosition(self.clientID,self.ball1Handle, -1,self.pos1 , vrep.simx_opmode_oneshot) _ = vrep.simxSetObjectPosition(self.clientID,self.ball2Handle , -1,self.pos2 , vrep.simx_opmode_oneshot) # for i in range(self.jointNum): # vrep.simxSetJointPosition(self.clientID,self.robot1_jointHandle[i], self.config1[i], vrep.simx_opmode_oneshot) # for i in range(self.jointNum): # vrep.simxSetJointPosition(self.clientID,self.robot2_jointHandle[i], self.config2[i],vrep.simx_opmode_oneshot) time.sleep(0.1) jointConfig1 = np.zeros((self.jointNum,)) jointConfig2 = np.zeros((self.jointNum,)) for i in range(self.jointNum): _, jpos = vrep.simxGetJointPosition(self.clientID, self.robot1_jointHandle[i], vrep.simx_opmode_blocking) jointConfig1[i] = jpos for i in range(self.jointNum): _, jpos = vrep.simxGetJointPosition(self.clientID, self.robot2_jointHandle[i], vrep.simx_opmode_blocking) jointConfig2[i] = jpos s = np.hstack((jointConfig1, jointConfig2)) _, end1 = vrep.simxGetObjectPosition(self.clientID, self.end1_Handle,self.base1Handle, vrep.simx_opmode_blocking) _, end2 = vrep.simxGetObjectPosition(self.clientID, self.end2_Handle,self.base2Handle, vrep.simx_opmode_blocking) del end1[2] del end2[2] end1 = np.array(end1) end2 = np.array(end2) d1 = self.goal_1-end1 d2 = self.goal_2-end2 s = np.hstack((s, np.hstack((d1,d2)))) return s
def virar(angulo): thetaInicial = localizacao.getOrientacao() print thetaInicial if(angulo > 0): #motorDir.rotate((int) (ang * (DISTANCIA_RODAS / RAIO_RODA))); v_Left = -0.5 v_Right = 0.5 while(abs(thetaInicial - localizacao.getOrientacao()) < abs(angulo)): vrep.simxSetJointTargetVelocity(clientID, rightMotorHandle, v_Right, vrep.simx_opmode_streaming) vrep.simxSetJointTargetVelocity(clientID, leftMotorHandle, v_Left, vrep.simx_opmode_streaming) thetaDir = vrep.simxGetJointPosition(clientID, rightMotorHandle, vrep.simx_opmode_streaming)[1] thetaEsq = vrep.simxGetJointPosition(clientID, leftMotorHandle, vrep.simx_opmode_streaming)[1] localizacao.setAngulos(thetaDir, thetaEsq) else: v_Left = 0.5 v_Right = -0.5 while(abs(thetaInicial - localizacao.getOrientacao()) < abs(angulo)): vrep.simxSetJointTargetVelocity(clientID, rightMotorHandle, v_Right, vrep.simx_opmode_streaming) vrep.simxSetJointTargetVelocity(clientID, leftMotorHandle, v_Left, vrep.simx_opmode_streaming) thetaDir = vrep.simxGetJointPosition(clientID, rightMotorHandle, vrep.simx_opmode_streaming)[1] thetaEsq = vrep.simxGetJointPosition(clientID, leftMotorHandle, vrep.simx_opmode_streaming)[1] localizacao.setAngulos(thetaDir, thetaEsq)
def retrieve_state(self): pos = np.zeros(2) vel = np.zeros(2) tip = np.zeros(3) _, pos[0] = vrep.simxGetJointPosition(self.clientID, self.jointHandles[0], vrep.simx_opmode_blocking) _, pos[1] = vrep.simxGetJointPosition(self.clientID, self.jointHandles[1], vrep.simx_opmode_blocking) _, vel[0] = vrep.simxGetObjectFloatParameter(self.clientID, self.jointHandles[0], 2012, vrep.simx_opmode_blocking) _, vel[1] = vrep.simxGetObjectFloatParameter(self.clientID, self.jointHandles[1], 2012, vrep.simx_opmode_blocking) _, tip_list = vrep.simxGetObjectPosition(self.clientID, self.tipHandle, -1, vrep.simx_opmode_blocking) tip = np.asarray(tip_list) return {1: pos, 2: vel, 3: tip}
def startDataStreaming(): if (not clientID): print('connect first') elif (clientID[0] == -1): print('connect first') else: #Position for i in Objects_handle: vrep.simxGetObjectPosition(clientID[0], i, UR3_handle[0], vrep.simx_opmode_streaming) #Dummy position vrep.simxGetObjectPosition(clientID[0], end_effector[0], UR3_handle[0], vrep.simx_opmode_streaming) #vision for i in vision_sensor: vrep.simxGetVisionSensorImage(clientID[0], i, 0, vrep.simx_opmode_streaming) #collision for i in collision_handle: vrep.simxReadCollision(clientID[0], i, vrep.simx_opmode_streaming) #UR3 Joint for i in UR3_joint_handle: vrep.simxGetJointPosition(clientID[0], i, vrep.simx_opmode_streaming)
def run_sim_data(subject, clientID): vrep.simxStartSimulation(clientID,vrep.simx_opmode_oneshot_wait) # start #RhipX,RhipY,RhipZ,LhipX,LhipY,LhipZ,RKneeZ,LKneeZ,RAnkleX,RAnkleZ,LAnkleX,LAnkleZ legJoints = [0,0,0,0,0,0,0,0,0,0,0,0] #RhipX,RhipY,RhipZ,LhipX,LhipY,LhipZ,RKneeZ,LKneeZ,RAnkleX,RAnkleZ,LAnkleX,LAnkleZ legPositions = [0,0,0,0,0,0,0,0,0,0,0,0] legJointInv = [1,1,1,-1,-1,1,1,1,1,1,-1,1] hipJointInv = [1,1,1,1,1,1,1,1,1,1,1,1] returnCode01, legJoints[0] = vrep.simxGetObjectHandle(clientID, "rightLegJoint0", vrep.simx_opmode_blocking) returnCode02, legJoints[1] = vrep.simxGetObjectHandle(clientID, "rightLegJoint1", vrep.simx_opmode_blocking) returnCode03, legJoints[2] = vrep.simxGetObjectHandle(clientID, "rightLegJoint2", vrep.simx_opmode_blocking) returnCode04, legJoints[6] = vrep.simxGetObjectHandle(clientID, "rightLegJoint3", vrep.simx_opmode_blocking) returnCode05, legJoints[8] = vrep.simxGetObjectHandle(clientID, "rightLegJoint5", vrep.simx_opmode_blocking) returnCode06, legJoints[9] = vrep.simxGetObjectHandle(clientID, "rightLegJoint4", vrep.simx_opmode_blocking) returnCode07, legJoints[3] = vrep.simxGetObjectHandle(clientID, "leftLegJoint0", vrep.simx_opmode_blocking) returnCode08, legJoints[4] = vrep.simxGetObjectHandle(clientID, "leftLegJoint1", vrep.simx_opmode_blocking) returnCode09, legJoints[5] = vrep.simxGetObjectHandle(clientID, "leftLegJoint2", vrep.simx_opmode_blocking) returnCode010, legJoints[7] = vrep.simxGetObjectHandle(clientID, "leftLegJoint3", vrep.simx_opmode_blocking) returnCode011, legJoints[10] = vrep.simxGetObjectHandle(clientID, "leftLegJoint5", vrep.simx_opmode_blocking) returnCode012, legJoints[11] = vrep.simxGetObjectHandle(clientID, "leftLegJoint4", vrep.simx_opmode_blocking) returnCode013, head = vrep.simxGetObjectHandle(clientID, "Asti", vrep.simx_opmode_blocking) headLocation = [1,1,1]; returncode, headLocation = vrep.simxGetObjectPosition(clientID, head, -1, vrep.simx_opmode_streaming) for i in range(12): returnCode, legPositions[i] = vrep.simxGetJointPosition(clientID, legJoints[i], vrep.simx_opmode_streaming) #RhipX,RhipY,RhipZ,LhipX,LhipY,LhipZ,RKneeZ,LKneeZ,RAnkleX,RAnkleZ,LAnkleX,LAnkleZ start_time = int(round(time.time())) while vrep.simxGetConnectionId(clientID) != -1 and ((headLocation[2] > 0.3 or headLocation[2] == 0.0) and ((int(round(time.time())) - start_time) < 100)): for j in range(((subject * 101)),((subject * 101) + 202)): for i in range(12): returnCode, legPositions[i] = vrep.simxGetJointPosition(clientID, legJoints[i], vrep.simx_opmode_buffer) joint_data = model_iterator.getActual(j) newJointPositions = [0,0,0,0,0,0,0,0,0,0,0,0] for i in range(len(newJointPositions)): joint_data[i] = joint_data[i] * math.pi / 180 newJointPositions[i] = joint_data[i] * legJointInv[i] * hipJointInv[i] print(j) print(newJointPositions) print() vrep.simxPauseCommunication(clientID,1) for i in range(12): vrep.simxSetJointTargetPosition(clientID,legJoints[i],newJointPositions[i],vrep.simx_opmode_oneshot) vrep.simxPauseCommunication(clientID,0) time.sleep(0.01) vrep.simxStopSimulation(clientID,vrep.simx_opmode_oneshot_wait) time.sleep(0.25) return
def getState_v1(self): #版本二的状态定义,维度是14 jointConfig1 = np.zeros((self.jointNum, )) jointConfig2 = np.zeros((self.jointNum, )) for i in range(self.jointNum): _, jpos = vrep.simxGetJointPosition(self.clientID, self.robot1_jointHandle[i], vrep.simx_opmode_blocking) jointConfig1[i] = jpos for i in range(self.jointNum): _, jpos = vrep.simxGetJointPosition(self.clientID, self.robot2_jointHandle[i], vrep.simx_opmode_blocking) jointConfig2[i] = jpos s = np.hstack((jointConfig1, jointConfig2)) _, pos1 = vrep.simxGetObjectPosition(self.clientID, self.end1_Handle, -1, vrep.simx_opmode_blocking) _, pos2 = vrep.simxGetObjectPosition(self.clientID, self.end2_Handle, -1, vrep.simx_opmode_blocking) del pos1[2] del pos2[2] pos1 = np.array(pos1) pos2 = np.array(pos2) pos = np.hstack((pos1, pos2)) s = np.hstack((s, pos)) _, d1 = vrep.simxReadDistance(self.clientID, self.robot1_goal_Handle, vrep.simx_opmode_blocking) _, d2 = vrep.simxReadDistance(self.clientID, self.robot2_goal_Handle, vrep.simx_opmode_blocking) s = np.hstack((s, np.array([d1, d2]))) # collision1, collision2 = self.getCollisonStates() # danger = np.array([1. if collision1 else 0., 1. if collision2 else 0.]) # s = np.hstack((s, danger)) return s
def on_for_degrees(self, left_speed, right_speed, degrees, brake=True, block=True): if brake is not True: print(f'{__class__.__name__} break not implemented') if block is not True: print(f'{__class__.__name__} block not implemented') # deg to rad if degrees == 'full': degrees = 360 if degrees == 'half': degrees = 180 if degrees == 'right': degrees = 90 rad = degrees * math.pi / 180 _clientID = self.motors['Left motor port'].kwargs['clientID'] _left_motor = self.motors['Left motor port'] _right_motor = self.motors['Right motor port'] left_speed, right_speed = self._unpack_speeds_to_native_units( left_speed, right_speed ) rCL, ini_pos_L = _left_motor.initialize_motor() rCR, ini_pos_R = _right_motor.initialize_motor() if VERBOSE: print(ini_pos_L, ini_pos_R) pos_L, pos_R = ini_pos_L, ini_pos_R while ((abs(pos_L - ini_pos_L) <= rad) and (abs(pos_R - ini_pos_R) <= rad)): err_vL = vrep.simxSetJointTargetVelocity( _clientID, _left_motor.kwargs.get('motor'), left_speed, vrep.simx_opmode_oneshot_wait ) err_vR = vrep.simxSetJointTargetVelocity( _clientID, _right_motor.kwargs.get('motor'), right_speed, vrep.simx_opmode_oneshot_wait ) rC, pos_L = vrep.simxGetJointPosition( _clientID, _left_motor.kwargs.get('motor'), vrep.simx_opmode_streaming) rC, pos_R = vrep.simxGetJointPosition( _clientID, _right_motor.kwargs.get('motor'), vrep.simx_opmode_streaming) if VERBOSE: print(pos_L - ini_pos_L, pos_R - ini_pos_R) self.motors['Left motor port'].full_stop() self.motors['Right motor port'].full_stop()
def connect(self, address='127.0.0.1', port=19999): vrep.simxFinish(-1) # just in case, close all opened connections self._clientID = vrep.simxStart(address, port, True, True, 5000, 5) # Connect to V-REP if self._clientID >= 0: # and clientID_0 != -1: print('Connected to remote API server: client id {}'.format(self._clientID)) else: raise VREPCommunicationError('Failed connecting to remote API server') self._RightMotor = self._vrep_get_object_handle('Right_Motor{}'.format(self._value_number), vrep.simx_opmode_blocking) self._LeftMotor = self._vrep_get_object_handle('Left_Motor{}'.format(self._value_number), vrep.simx_opmode_blocking) self._Robobo = self._vrep_get_object_handle('Robobo{}'.format(self._value_number), vrep.simx_opmode_blocking) self._IrBackC = self._vrep_get_object_handle('Ir_Back_C{}'.format(self._value_number), vrep.simx_opmode_blocking) self._IrFrontC = self._vrep_get_object_handle('Ir_Front_C{}'.format(self._value_number), vrep.simx_opmode_blocking) self._IrFrontLL = self._vrep_get_object_handle('Ir_Front_LL{}'.format(self._value_number), vrep.simx_opmode_blocking) self._IrFrontRR = self._vrep_get_object_handle('Ir_Front_RR{}'.format(self._value_number), vrep.simx_opmode_blocking) self._IrBackL = self._vrep_get_object_handle('Ir_Back_L{}'.format(self._value_number), vrep.simx_opmode_blocking) self._IrBackLFloor = self._vrep_get_object_handle('Ir_Back_L_Floor{}'.format(self._value_number), vrep.simx_opmode_blocking) self._IrBackR = self._vrep_get_object_handle('Ir_Back_R{}'.format(self._value_number), vrep.simx_opmode_blocking) self._IrBackRFloor = self._vrep_get_object_handle('Ir_Back_R_Floor{}'.format(self._value_number), vrep.simx_opmode_blocking) self._IrFrontL = self._vrep_get_object_handle('Ir_Front_L{}'.format(self._value_number), vrep.simx_opmode_blocking) self._IrFrontLFloor = self._vrep_get_object_handle('Ir_Front_L_Floor{}'.format(self._value_number), vrep.simx_opmode_blocking) self._IrFrontR = self._vrep_get_object_handle('Ir_Front_R{}'.format(self._value_number), vrep.simx_opmode_blocking) self._IrFrontRFloor = self._vrep_get_object_handle('Ir_Front_R_Floor{}'.format(self._value_number), vrep.simx_opmode_blocking) self._TiltMotor = self._vrep_get_object_handle('Tilt_Motor{}'.format(self._value_number), vrep.simx_opmode_blocking) self._PanMotor = self._vrep_get_object_handle('Pan_Motor{}'.format(self._value_number), vrep.simx_opmode_blocking) self._FrontalCamera = self._vrep_get_object_handle('Frontal_Camera{}'.format(self._value_number), vrep.simx_opmode_blocking) # read a first value in streaming mode self._vrep_read_proximity_sensor_ignore_error(self._IrFrontC) self._vrep_read_proximity_sensor_ignore_error(self._IrBackC) self._vrep_read_proximity_sensor_ignore_error(self._IrFrontLL) self._vrep_read_proximity_sensor_ignore_error(self._IrFrontRR) self._vrep_read_proximity_sensor_ignore_error(self._IrBackL) self._vrep_read_proximity_sensor_ignore_error(self._IrBackLFloor) self._vrep_read_proximity_sensor_ignore_error(self._IrBackR) self._vrep_read_proximity_sensor_ignore_error(self._IrBackRFloor) self._vrep_read_proximity_sensor_ignore_error(self._IrFrontR) self._vrep_read_proximity_sensor_ignore_error(self._IrFrontRFloor) self._vrep_read_proximity_sensor_ignore_error(self._IrFrontL) self._vrep_read_proximity_sensor_ignore_error(self._IrFrontLFloor) # setup join positions vrep.simxGetJointPosition(self._clientID, self._RightMotor, vrep.simx_opmode_buffer) vrep.simxGetJointPosition(self._clientID, self._LeftMotor, vrep.simx_opmode_buffer) vrep.simxGetObjectPosition(self._clientID, self._Robobo, -1, vrep.simx_opmode_buffer) # read a first value in buffer mode self._vrep_get_vision_sensor_image_ignore_error(self._FrontalCamera, vrep.simx_opmode_streaming) self._vrep_get_ping_time() return self
def final_move(clientID, jointsdict, joint_list, collision_library, arm): if arm == "left": handle = jointsdict['Baxter_leftArm_joint1']['Joint Handler'] res, theta_i = vrep.simxGetJointPosition(clientID, handle, vrep.simx_opmode_blocking) elif arm == "right": handle = jointsdict['Baxter_rightArm_joint1']['Joint Handler'] res, theta_i = vrep.simxGetJointPosition(clientID, handle, vrep.simx_opmode_blocking) return
def deinitGripper(self): res, pos = vrep.simxGetJointPosition(self.clientID, self.rollJoint, vrep.simx_opmode_buffer) while pos < 0: vrep.simxSetJointPosition(self.clientID, self.rollJoint, pos + math.pi / 50, vrep.simx_opmode_oneshot) res, pos = vrep.simxGetJointPosition(self.clientID, self.rollJoint, vrep.simx_opmode_buffer) time.sleep(0.01)
def search_object_2(): print('Searching...') vrep.simxSetStringSignal(clientID, 'IKCommands', 'LookPos', vrep.simx_opmode_oneshot) # "Looking" position center_res = 10 tic_so = time.clock() objectFound = False while objectFound is False: toc_so = time.clock() errorCode2, resolution, image = vrep.simxGetVisionSensorImage( clientID, cam_Handle, 0, vrep.simx_opmode_buffer) im = np.array(image, dtype=np.uint8) if len(resolution) > 0: im.resize([resolution[0], resolution[1], 3]) obj_list = image_process_2(im, resolution) if len(obj_list) > 0: print(obj_list[0]) if len(obj_list[0]) > 1: im_x = obj_list[0][1] im_y = obj_list[0][2] print('Object Found!') full_center(im_x, im_y, center_res, resolution) return 1 errorCode, joint_ang_0 = vrep.simxGetJointPosition( clientID, joint_handles[0], vrep.simx_opmode_buffer) errorCode, joint_ang_4 = vrep.simxGetJointPosition( clientID, joint_handles[4], vrep.simx_opmode_buffer) if joint_ang_0 > 70 * math.pi / 180: vrep.simxSetJointTargetVelocity(clientID, joint_handles[0], 0, vrep.simx_opmode_streaming) vrep.simxSetJointTargetVelocity(clientID, joint_handles[4], -0.2, vrep.simx_opmode_streaming) if joint_ang_4 < 100 * math.pi / 180: vrep.simxSetJointTargetVelocity(clientID, joint_handles[0], -0.2, vrep.simx_opmode_streaming) vrep.simxSetJointTargetVelocity(clientID, joint_handles[4], 0, vrep.simx_opmode_streaming) if joint_ang_0 < -70 * math.pi / 180: vrep.simxSetJointTargetVelocity(clientID, joint_handles[0], 0, vrep.simx_opmode_streaming) vrep.simxSetJointTargetVelocity(clientID, joint_handles[4], 0.2, vrep.simx_opmode_streaming) if joint_ang_4 > 118 * math.pi / 180: vrep.simxSetJointTargetVelocity(clientID, joint_handles[0], 0.2, vrep.simx_opmode_streaming) vrep.simxSetJointTargetVelocity(clientID, joint_handles[4], 0, vrep.simx_opmode_streaming) if toc_so - tic_so > 20: return 6
def run360(): print "connecting to vrep with address", IPADDRESS, "on port", PORT clientID = vrep.simxStart(IPADDRESS, PORT, True, True, 5000, 5) print "hello vrep! ", clientID if clientID == -1: print "failed to connect to vrep server" return # get the motor joint handle error, jointHandle = vrep.simxGetObjectHandle( clientID, "plantStandMotor", vrep.simx_opmode_oneshot_wait) print "starting the motor..." # set the velocity of the joint vrep.simxSetJointTargetVelocity(clientID, jointHandle, 1.0, vrep.simx_opmode_oneshot_wait) print "spinning 360 degrees..." # set up to stream joint positions vrep.simxGetJointPosition(clientID, jointHandle, vrep.simx_opmode_streaming) passed90Degrees = False # The control loop: while vrep.simxGetConnectionId( clientID) != -1: # while we are connected to the server.. (error, position) = vrep.simxGetJointPosition(clientID, jointHandle, vrep.simx_opmode_buffer) # Fetch the newest joint value from the inbox (func. returns immediately (non-blocking)): if error == vrep.simx_error_noerror: # here we have the newest joint position in variable jointPosition! # break when we've done a 360 if passed90Degrees and position >= 0 and position < math.pi / 2.0: break elif not passed90Degrees and position > math.pi / 2.0: passed90Degrees = True print "stoppping..." vrep.simxSetJointTargetVelocity(clientID, jointHandle, 0.0, vrep.simx_opmode_oneshot_wait) if clientID != -1: vrep.simxFinish(clientID) print "done!"
def signal_values(self): vrep.simxGetObjectOrientation(self.client_id, self.body_handle, -1, vrep.simx_opmode_streaming) vrep.simxGetObjectPosition(self.client_id, self.body_handle, -1, vrep.simx_opmode_streaming) for handle_idx in range(0, self.joint_count): vrep.simxGetObjectFloatParameter(self.client_id, self.handles[handle_idx], 2012, vrep.simx_opmode_streaming) for handle_idx in range(0, self.joint_count): vrep.simxGetJointPosition(self.client_id, self.handles[handle_idx], vrep.simx_opmode_streaming)
def on_release(key): vrep.simxSetJointTargetVelocity(clientID, rightMotorHandle, v_Right, vrep.simx_opmode_streaming) vrep.simxSetJointTargetVelocity(clientID, leftMotorHandle, v_Left, vrep.simx_opmode_streaming) thetaDir = vrep.simxGetJointPosition(clientID, rightMotorHandle, vrep.simx_opmode_streaming)[1] thetaEsq = vrep.simxGetJointPosition(clientID, leftMotorHandle, vrep.simx_opmode_streaming)[1] localizacao.setAngulos(thetaDir, thetaEsq)
def get_joint_position(self, object_handle): """ get joint position of an object """ try: err_code, angle = vrep.simxGetJointPosition( self.clientID, object_handle, vrep.simx_opmode_streaming) while err_code != vrep.simx_return_ok: err_code, angle = vrep.simxGetJointPosition( self.clientID, object_handle, vrep.simx_opmode_buffer) return angle except Exception as e: raise e
def getCurrentBaseJointPos(self): res, jointHandle = vrep.simxGetObjectHandle( self._clientID, GB_CBASE_JOINT_NAME, vrep.simx_opmode_oneshot_wait) res, pos = vrep.simxGetJointPosition(self._clientID, jointHandle, vrep.simx_opmode_streaming) # Wait until the first data has arrived (just any blocking funtion): vrep.simxGetPingTime(self._clientID) # Now you can read the data that is being continuously streamed: res, pos = vrep.simxGetJointPosition(self._clientID, jointHandle, vrep.simx_opmode_buffer) return pos
def getJointPositionNonBlock(self, clientID, joint, firstTime): error = False value = 0 handle = self.jointHandleMapping[joint] if (handle == 0): error, handle=vrep.simxGetObjectHandle(clientID,joint,vrep.simx_opmode_oneshot) self.jointHandleMapping[joint]=handle if not error: if firstTime: error, value = vrep.simxGetJointPosition(clientID,handle,vrep.simx_opmode_streaming) else: error, value = vrep.simxGetJointPosition(clientID,handle,vrep.simx_opmode_buffer) return error, value
def run360(): print "connecting to vrep with address", IPADDRESS, "on port", PORT clientID = vrep.simxStart(IPADDRESS, PORT, True, True, 5000, 5) print "hello vrep! ", clientID if clientID == -1: print "failed to connect to vrep server" return # get the motor joint handle error,jointHandle = vrep.simxGetObjectHandle(clientID, "plantStandMotor", vrep.simx_opmode_oneshot_wait) print "starting the motor..." # set the velocity of the joint vrep.simxSetJointTargetVelocity(clientID, jointHandle, 1.0, vrep.simx_opmode_oneshot_wait); print "spinning 360 degrees..." # set up to stream joint positions vrep.simxGetJointPosition(clientID, jointHandle, vrep.simx_opmode_streaming); passed90Degrees = False # The control loop: while vrep.simxGetConnectionId(clientID) != -1 : # while we are connected to the server.. (error,position) = vrep.simxGetJointPosition( clientID, jointHandle, vrep.simx_opmode_buffer ) # Fetch the newest joint value from the inbox (func. returns immediately (non-blocking)): if error==vrep.simx_error_noerror : # here we have the newest joint position in variable jointPosition! # break when we've done a 360 if passed90Degrees and position >= 0 and position < math.pi/2.0: break elif not passed90Degrees and position > math.pi/2.0: passed90Degrees = True print "stoppping..." vrep.simxSetJointTargetVelocity(clientID, jointHandle, 0.0, vrep.simx_opmode_oneshot_wait); if clientID != -1: vrep.simxFinish(clientID) print "done!"
def read_FS(): global gripper_handles penalty = 0 returnCode, state, forceVector0, torqueVector0 = vrep.simxReadForceSensor( clientID, FS_handles[0], vrep.simx_opmode_buffer) returnCode, state, forceVector1, torqueVector1 = vrep.simxReadForceSensor( clientID, FS_handles[1], vrep.simx_opmode_buffer) returnCode, state, forceVector2, torqueVector2 = vrep.simxReadForceSensor( clientID, FS_handles[2], vrep.simx_opmode_buffer) # print forceVector0, torqueVector0 # print forceVector1, torqueVector1 # print forceVector2, torqueVector2 force0 = (forceVector0[0]**2 + forceVector0[1]**2 + forceVector0[2]**2)**(0.5) force1 = (forceVector1[0]**2 + forceVector1[1]**2 + forceVector1[2]**2)**(0.5) force2 = (forceVector2[0]**2 + forceVector2[1]**2 + forceVector2[2]**2)**(0.5) # print force0, force1, force2 av_force = np.mean([force0, force1, force2]) force_dist = (abs(force0 - force1) + abs(force1 - force2) + abs(force2 - force0)) / 3 force_dist_cost = 3 * (1 - np.exp(force_dist - 3)) if force_dist_cost < -20: force_dist_cost = -20 print(av_force, force_dist, force_dist_cost) returnCode, hand_joint_0 = vrep.simxGetJointPosition( clientID, gripper_handles[2], vrep.simx_opmode_buffer) returnCode, hand_joint_1 = vrep.simxGetJointPosition( clientID, gripper_handles[3], vrep.simx_opmode_buffer) returnCode, hand_joint_2 = vrep.simxGetJointPosition( clientID, gripper_handles[4], vrep.simx_opmode_buffer) if hand_joint_0 > 100 * math.pi / 180 or hand_joint_0 < 40 * math.pi / 180: penalty = penalty + 10 if hand_joint_1 > 100 * math.pi / 180 or hand_joint_1 < 40 * math.pi / 180: penalty = penalty + 10 if hand_joint_2 > 100 * math.pi / 180 or hand_joint_2 < 40 * math.pi / 180: penalty = penalty + 10 return [av_force, force_dist_cost, penalty]
def getJointValues(self): #Store values of waiter error, rLeg = vrep.simxGetJointPosition(clientID, self.rightLegID, vrep.simx_opmode_streaming) error, lLeg = vrep.simxGetJointPosition(clientID, self.leftLegID, vrep.simx_opmode_streaming) error, rKnee = vrep.simxGetJointPosition(clientID, self.rightKneeID, vrep.simx_opmode_streaming) error, lKnee = vrep.simxGetJointPosition(clientID, self.leftKneeID, vrep.simx_opmode_streaming) error, rAnkle = vrep.simxGetJointPosition(clientID, self.rightAnkleID, vrep.simx_opmode_streaming) error, lAnkle = vrep.simxGetJointPosition(clientID, self.leftAnkleID, vrep.simx_opmode_streaming) error, rElbow = vrep.simxGetJointPosition(clientID, self.rightElbowID, vrep.simx_opmode_streaming) error, lElbow = vrep.simxGetJointPosition(clientID, self.leftElbowID, vrep.simx_opmode_streaming) error, rShoulder = vrep.simxGetJointMatrix(clientID, self.rightArmID, vrep.simx_opmode_streaming) error, lShoulder = vrep.simxGetJointMatrix(clientID, self.leftArmID, vrep.simx_opmode_streaming) error, neck = vrep.simxGetJointMatrix(clientID, self.neckID, vrep.simx_opmode_buffer) return (rLeg, lLeg, rKnee, lKnee, rAnkle, lAnkle, rElbow, lElbow, rShoulder, lShoulder, neck)
def get_angles_firsttime(): for i in range(12): print('handler is') print(int(angles_handler[i])) trash, ang = vrep.simxGetJointPosition(clientID, int(angles_handler[i]), vrep.simx_opmode_streaming) res = vrep.simx_return_novalue_flag while res != vrep.simx_return_ok: res, ang1 = vrep.simxGetJointPosition(clientID, int(angles_handler[i]), vrep.simx_opmode_buffer) print("Angle x IS") print(ang1)
def initialize_handles(self): self._RightMotor = self._vrep_get_object_handle('Right_Motor{}'.format(self._value_number), vrep.simx_opmode_blocking) self._LeftMotor = self._vrep_get_object_handle('Left_Motor{}'.format(self._value_number), vrep.simx_opmode_blocking) self._Robobo = self._vrep_get_object_handle('Robobo{}'.format(self._value_number), vrep.simx_opmode_blocking) self._IrBackC = self._vrep_get_object_handle('Ir_Back_C{}'.format(self._value_number), vrep.simx_opmode_blocking) self._IrFrontC = self._vrep_get_object_handle('Ir_Front_C{}'.format(self._value_number), vrep.simx_opmode_blocking) self._IrFrontLL = self._vrep_get_object_handle('Ir_Front_LL{}'.format(self._value_number), vrep.simx_opmode_blocking) self._IrFrontRR = self._vrep_get_object_handle('Ir_Front_RR{}'.format(self._value_number), vrep.simx_opmode_blocking) self._IrBackL = self._vrep_get_object_handle('Ir_Back_L{}'.format(self._value_number), vrep.simx_opmode_blocking) self._IrBackLFloor = self._vrep_get_object_handle('Ir_Back_L_Floor{}'.format(self._value_number), vrep.simx_opmode_blocking) self._IrBackR = self._vrep_get_object_handle('Ir_Back_R{}'.format(self._value_number), vrep.simx_opmode_blocking) self._IrBackRFloor = self._vrep_get_object_handle('Ir_Back_R_Floor{}'.format(self._value_number), vrep.simx_opmode_blocking) self._IrFrontL = self._vrep_get_object_handle('Ir_Front_L{}'.format(self._value_number), vrep.simx_opmode_blocking) self._IrFrontLFloor = self._vrep_get_object_handle('Ir_Front_L_Floor{}'.format(self._value_number), vrep.simx_opmode_blocking) self._IrFrontR = self._vrep_get_object_handle('Ir_Front_R{}'.format(self._value_number), vrep.simx_opmode_blocking) self._IrFrontRFloor = self._vrep_get_object_handle('Ir_Front_R_Floor{}'.format(self._value_number), vrep.simx_opmode_blocking) self._TiltMotor = self._vrep_get_object_handle('Tilt_Motor{}'.format(self._value_number), vrep.simx_opmode_blocking) self._PanMotor = self._vrep_get_object_handle('Pan_Motor{}'.format(self._value_number), vrep.simx_opmode_blocking) self._FrontalCamera = self._vrep_get_object_handle('Frontal_Camera{}'.format(self._value_number), vrep.simx_opmode_blocking) # read a first value in streaming mode self._vrep_read_proximity_sensor_ignore_error(self._IrFrontC) self._vrep_read_proximity_sensor_ignore_error(self._IrBackC) self._vrep_read_proximity_sensor_ignore_error(self._IrFrontLL) self._vrep_read_proximity_sensor_ignore_error(self._IrFrontRR) self._vrep_read_proximity_sensor_ignore_error(self._IrBackL) self._vrep_read_proximity_sensor_ignore_error(self._IrBackLFloor) self._vrep_read_proximity_sensor_ignore_error(self._IrBackR) self._vrep_read_proximity_sensor_ignore_error(self._IrBackRFloor) self._vrep_read_proximity_sensor_ignore_error(self._IrFrontR) self._vrep_read_proximity_sensor_ignore_error(self._IrFrontRFloor) self._vrep_read_proximity_sensor_ignore_error(self._IrFrontL) self._vrep_read_proximity_sensor_ignore_error(self._IrFrontLFloor) # setup join positions vrep.simxGetJointPosition(self._clientID, self._RightMotor, vrep.simx_opmode_buffer) vrep.simxGetJointPosition(self._clientID, self._LeftMotor, vrep.simx_opmode_buffer) vrep.simxGetObjectPosition(self._clientID, self._Robobo, -1, vrep.simx_opmode_buffer) # read a first value in buffer mode self._vrep_get_vision_sensor_image_ignore_error(self._FrontalCamera, vrep.simx_opmode_streaming) self.wait_for_ping()
def printJointPositions(self, clientID): error, handlers, intData, floatData, stringData=vrep.simxGetObjectGroupData(clientID,vrep.sim_appobj_object_type,0,vrep.simx_opmode_oneshot_wait) itemHandle=0 print stringData for name in stringData: error, position = vrep.simxGetJointPosition(clientID,handlers[itemHandle], vrep.simx_opmode_streaming) itemHandle=itemHandle+1 print name + ":" + str(position)
def handle_output(self): err, arm_ori = vrep.simxGetJointPosition(self.cid, self.arm_joint, vrep.simx_opmode_oneshot) #2012 is the code for joint velocity err, arm_vel = vrep.simxGetObjectFloatParameter(self.cid, self.arm_joint, 2012, vrep.simx_opmode_oneshot) return [arm_ori, arm_vel]
def get_joint_angles(self, initial=False): if initial: mode = vrep.simx_opmode_streaming else: mode = vrep.simx_opmode_buffer angles = [] for joint in self.joints: rc, angle = vrep.simxGetJointPosition(self.client_id, joint, mode) assert rc == (1 if initial else 0), rc angles.append(angle) return np.array(angles)
# Here, we use maths.radians to convert degrees into radians. # http://www.coppeliarobotics.com/helpFiles/en/remoteApiFunctionsPython.htm#simxSetJointTargetPosition msg = "Motors target positions: " + str(ashoulder) + " " + str(aelbow) + " " + str(awrist) print msg logging.info(msg) vrep.simxSetJointTargetPosition(clientID, wristHandle, math.radians(awrist), opmode) vrep.simxSetJointTargetPosition(clientID, elbowHandle, math.radians(aelbow), opmode) vrep.simxSetJointTargetPosition(clientID, shoulderHandle, math.radians(ashoulder), opmode) # Wait in order to let the motors finish their movements # Tip: there must be a more efficient way to do it... time.sleep(3) # Get the motors effective positions after the movement sequence # http://www.coppeliarobotics.com/helpFiles/en/remoteApiFunctionsPython.htm#simxGetJointPosition pwrist = vrep.simxGetJointPosition(clientID, wristHandle, opmode) pelbow = vrep.simxGetJointPosition(clientID, elbowHandle, opmode) pshoulder = vrep.simxGetJointPosition(clientID, shoulderHandle, opmode) msg = "Motors reached positions: " + str(pshoulder) + " " + str(pelbow) + " " + str(pwrist) #print msg # Get the robot position after the movement sequence pret, lPos = vrep.simxGetObjectPosition(clientID, leftWheelHandle, -1, vrep.simx_opmode_streaming) pret, rPos = vrep.simxGetObjectPosition(clientID, rightWheelHandle, -1, vrep.simx_opmode_streaming) msg = "2w1a position after move: leftWheel = " + str(lPos) +\ ", rightWheel = " + str(rPos) + ")" print msg logging.info(msg) # find out the fitness robot moved rs = fitness(lOriginPos, rOriginPos, lPos, rPos)
vrep.simxStartSimulation(clientID, opmode) theTime = time.time() # Start getting the robot position pret, robotPos = vrep.simxGetObjectPosition(clientID, robotHandle, -1, vrep.simx_opmode_streaming) pret4, leftWheelPos = vrep.simxGetObjectPosition(clientID, leftWheelHandle, -1, vrep.simx_opmode_streaming) pret5, rightWheelPos = vrep.simxGetObjectPosition(clientID, rightWheelHandle, -1, vrep.simx_opmode_streaming) print "2w1a position: (x = " + str(robotPos[0]) + ", y = " + str(robotPos[1]) + ")" # Start getting the robot orientation oret, robotOrient = vrep.simxGetObjectOrientation(clientID, robotHandle, -1, vrep.simx_opmode_streaming) print "2w1a orientation: (x = " + str(robotOrient[0]) + \ ", y = " + str(robotOrient[1]) +\ ", z = " + str(robotOrient[2]) + ")" for gene in individual.genes: vrep.simxSetJointTargetPosition(clientID, gene.type, math.radians(gene.action), opmode) pgene = vrep.simxGetJointPosition(clientID, gene.type, opmode) #time.sleep(0.05) # Wait in order to let the motors finish their movements # timer = time.time() # while True: # pgene = vrep.simxGetJointPosition(clientID, gene.type, opmode) # if round(math.degrees(pgene[1]), 0) == round(gene.action, 0): # break # else: # time.sleep(0.01) # timer += 0.01 # if time.time() >= timer + 3: # theTime = 0
initialCall=True print ('VREP Simulation Program') vrep.simxFinish(-1) # just in case, close all opened connections clientID=vrep.simxStart('127.0.0.1',19999,True,True,5000,5) # Connect to V-REP if clientID!=-1: print ('Connected to remote API server') else: print('Connection Failure') sys.exit('Abort Connection') # Object handle _,quadBase=vrep.simxGetObjectHandle(clientID,'Quadricopter_base',vrep.simx_opmode_oneshot_wait) _,jointPole=vrep.simxGetObjectHandle(clientID,'Pole_joint',vrep.simx_opmode_oneshot_wait) while True: # Code for testing... if initialCall: mode = vrep.simx_opmode_streaming initialCall = False else: mode = vrep.simx_opmode_buffer errorFlag,jointPos=vrep.simxGetJointPosition(clientID,jointPole,mode) if errorFlag == vrep.simx_return_ok: print(str(jointPos)) else: print('Measurements Awaiting') time.sleep(1.0)
target_index += 1 # get the (x,y,z) position of the target _, target_xyz = vrep.simxGetObjectPosition(clientID, target_handle, -1, # retrieve absolute, not relative, position vrep.simx_opmode_blocking) if _ !=0 : raise Exception() track_target.append(np.copy(target_xyz)) # store for plotting target_xyz = np.asarray(target_xyz) for ii,joint_handle in enumerate(joint_handles): old_q = np.copy(q) # get the joint angles _, q[ii] = vrep.simxGetJointPosition(clientID, joint_handle, vrep.simx_opmode_blocking) if _ !=0 : raise Exception() # get the joint velocity _, dq[ii] = vrep.simxGetObjectFloatParameter(clientID, joint_handle, 2012, # parameter ID for angular velocity of the joint vrep.simx_opmode_blocking) if _ !=0 : raise Exception() # update end-effector position L = np.array([0, .35]) # segment lengths associated with each joint s = np.sin(q) c = np.cos(q)
def get_joint_values(clientID,Body): i = 0 angles = [0] * 25 angles[0] = vrep.simxGetJointPosition(clientID,Body[0][i],vrep.simx_opmode_oneshot_wait)[1] angles[1] = vrep.simxGetJointPosition(clientID,Body[1][i], vrep.simx_opmode_oneshot_wait)[1] #Left Leg angles[8] = vrep.simxGetJointPosition(clientID,Body[2][i],vrep.simx_opmode_oneshot_wait)[1] angles[9] = vrep.simxGetJointPosition(clientID,Body[3][i],vrep.simx_opmode_oneshot_wait)[1] angles[10] = vrep.simxGetJointPosition(clientID,Body[4][i],vrep.simx_opmode_oneshot_wait)[1] angles[11] = vrep.simxGetJointPosition(clientID,Body[5][i],vrep.simx_opmode_oneshot_wait)[1] angles[12] = vrep.simxGetJointPosition(clientID,Body[6][i],vrep.simx_opmode_oneshot_wait)[1] angles[13] = vrep.simxGetJointPosition(clientID,Body[7][i],vrep.simx_opmode_oneshot_wait)[1] #Right Leg angles[14] = vrep.simxGetJointPosition(clientID,Body[8][i],vrep.simx_opmode_oneshot_wait)[1] angles[15] = vrep.simxGetJointPosition(clientID,Body[9][i],vrep.simx_opmode_oneshot_wait)[1] angles[16] = vrep.simxGetJointPosition(clientID,Body[10][i],vrep.simx_opmode_oneshot_wait)[1] angles[17] = vrep.simxGetJointPosition(clientID,Body[11][i],vrep.simx_opmode_oneshot_wait)[1] angles[18] =vrep.simxGetJointPosition(clientID,Body[12][i],vrep.simx_opmode_oneshot_wait)[1] angles[19] =vrep.simxGetJointPosition(clientID,Body[13][i],vrep.simx_opmode_oneshot_wait)[1] #Left Arm angles[2] = vrep.simxGetJointPosition(clientID,Body[14][i],vrep.simx_opmode_oneshot_wait)[1] angles[3] =vrep.simxGetJointPosition(clientID,Body[15][i],vrep.simx_opmode_oneshot_wait)[1] angles[4] = vrep.simxGetJointPosition(clientID,Body[16][i],vrep.simx_opmode_oneshot_wait)[1] angles[5] = vrep.simxGetJointPosition(clientID,Body[17][i],vrep.simx_opmode_oneshot_wait)[1] angles[6] =vrep.simxGetJointPosition(clientID,Body[18][i],vrep.simx_opmode_oneshot_wait)[1] #Right Arm angles[20] = vrep.simxGetJointPosition(clientID,Body[19][i],vrep.simx_opmode_oneshot_wait)[1] angles[21] = vrep.simxGetJointPosition(clientID,Body[20][i],vrep.simx_opmode_oneshot_wait)[1] angles[22] = vrep.simxGetJointPosition(clientID,Body[21][i],vrep.simx_opmode_oneshot_wait)[1] angles[23] = vrep.simxGetJointPosition(clientID,Body[22][i],vrep.simx_opmode_oneshot_wait)[1] angles[24] = vrep.simxGetJointPosition(clientID,Body[23][i],vrep.simx_opmode_oneshot_wait)[1] #angles[7] = 1.0 - vrep.simxGetJointPosition(clientID,Body[25][i][0],vrep.simx_opmode_oneshot_wait)[1] #angles[25] = 1.0 - vrep.simxGetJointPosition(clientID,Body[25][i][0],vrep.simx_opmode_oneshot_wait)[1] return angles
e = vrep.simxSetJointTargetPosition(client_id, joint_1, 0, vrep.simx_opmode_streaming) # this line is necessary to enable position recording e, body_pos = vrep.simxGetObjectPosition(client_id, body, -1, vrep.simx_opmode_streaming) # Wait until the robot is settled to the default position time.sleep(0.3) # enable synchronous mode vrep.simxSynchronous(client_id, True) position_history = [] e, body_pos = vrep.simxGetObjectPosition(client_id, body, -1, vrep.simx_opmode_buffer) position_history.append(body_pos) joint_pos_history = [] e, joint_0_pos = vrep.simxGetJointPosition(client_id, joint_0, vrep.simx_opmode_streaming) e, joint_1_pos = vrep.simxGetJointPosition(client_id, joint_1, vrep.simx_opmode_streaming) joint_pos_history.append([joint_0_pos, joint_1_pos]) with contexttimer.Timer() as timer: for i in range(4): e = vrep.simxSetJointTargetPosition(client_id, joint_0, 0.5, vrep.simx_opmode_streaming) for t in range(3): vrep.simxSynchronousTrigger(client_id) e, body_pos = vrep.simxGetObjectPosition(client_id, body, -1, vrep.simx_opmode_buffer) position_history.append(body_pos) e, joint_0_pos = vrep.simxGetJointPosition(client_id, joint_0, vrep.simx_opmode_buffer) e, joint_1_pos = vrep.simxGetJointPosition(client_id, joint_1, vrep.simx_opmode_buffer) joint_pos_history.append([joint_0_pos, joint_1_pos]) e = vrep.simxSetJointTargetPosition(client_id, joint_1, 2.5, vrep.simx_opmode_streaming)
def getJointObjectPosition(self, obj): return vrep.simxGetJointPosition(self.clientID, obj, self.opmode)
rand_hand_bend = numpy.random.uniform(10,90) *math.pi/180 # instruct the arm to move (all joints at once) vrep.simxPauseCommunication(clientID,True) vrep.simxSetJointPosition(clientID,armJoint0Handle, rand_base_rotate,vrep.simx_opmode_streaming) # base - rotates the entire arm - [-40..40] vrep.simxSetJointPosition(clientID,armJoint1Handle, rand_base_lower ,vrep.simx_opmode_streaming) # base - lowers the arm - [5..35] vrep.simxSetJointPosition(clientID,armJoint2Handle, rand_elbow_bend ,vrep.simx_opmode_streaming) # elbow - bends the arm - [0..50] vrep.simxSetJointPosition(clientID,armJoint3Handle, rand_hand_bend ,vrep.simx_opmode_streaming) # hand - bends the lower part of the arm - [0..90] vrep.simxSetJointPosition(clientID,armJoint4Handle, 0.0,vrep.simx_opmode_streaming) # wrist - turns the gripper vrep.simxPauseCommunication(clientID,False) # give the arm some time to move time.sleep(1) # verify that the arm has approximately reached its target position err0 = vrep.simxGetJointPosition(clientID,armJoint0Handle,vrep.simx_opmode_streaming)[1] - rand_base_rotate err1 = vrep.simxGetJointPosition(clientID,armJoint1Handle,vrep.simx_opmode_streaming)[1] - rand_base_lower err2 = vrep.simxGetJointPosition(clientID,armJoint2Handle,vrep.simx_opmode_streaming)[1] - rand_elbow_bend err3 = vrep.simxGetJointPosition(clientID,armJoint3Handle,vrep.simx_opmode_streaming)[1] - rand_hand_bend err4 = vrep.simxGetJointPosition(clientID,armJoint4Handle,vrep.simx_opmode_streaming)[1] - 0.0 joint_pos_err = abs(err0) + abs(err1) + abs(err2) + abs(err3) + abs(err4) print "joint positioning error =", joint_pos_err # get image from vision sensor 1 err,res,img = vrep.simxGetVisionSensorImage(clientID,visionSensor1Handle,0,vrep.simx_opmode_oneshot_wait) colval1 = img_to_numpy(res,img) # get the binary image marking the cyan ball in Youbot's gripper binval1 = img_binarise(colval1,0.0,0.15,0.7,0.99,0.9,1.0) # get the x- and y-coordinates of the cyan ball binval1 = numpy.reshape(binval1,(res[1],res[0])) pos_img1 = img_find_cm(binval1)
errorCode,joint_Handle11=vrep.simxGetObjectHandle(clientID,"LBR4p_joint1",vrep.simx_opmode_oneshot_wait) errorCode,joint_Handle5=vrep.simxGetObjectHandle(clientID,"BarrettHand_jointB_1",vrep.simx_opmode_oneshot_wait) errorCode,joint_Handle6=vrep.simxGetObjectHandle(clientID,"BarrettHand_jointC_1",vrep.simx_opmode_oneshot_wait) errorCode,joint_Handle7=vrep.simxGetObjectHandle(clientID,"BarrettHand_jointB_0",vrep.simx_opmode_oneshot_wait) errorCode,joint_Handle8=vrep.simxGetObjectHandle(clientID,"BarrettHand_jointC_0",vrep.simx_opmode_oneshot_wait) errorCode,joint_Handle9=vrep.simxGetObjectHandle(clientID,"BarrettHand_jointB_2",vrep.simx_opmode_oneshot_wait) errorCode,joint_Handle10=vrep.simxGetObjectHandle(clientID,"BarrettHand_jointC_2",vrep.simx_opmode_oneshot_wait) errorCode=vrep.simxSetJointTargetPosition(clientID,joint_Handle1,math.radians(65),vrep.simx_opmode_oneshot_wait) errorCode=vrep.simxSetJointTargetPosition(clientID,joint_Handle2,math.radians(170),vrep.simx_opmode_oneshot_wait) errorCode=vrep.simxSetJointTargetPosition(clientID,joint_Handle3,math.radians(90),vrep.simx_opmode_oneshot_wait) errorCode=vrep.simxSetJointTargetPosition(clientID,joint_Handle12,math.radians(90),vrep.simx_opmode_oneshot_wait)#gives position to the joint5 vrep.simxSetIntegerSignal(clientID,"closing_signal",1,vrep.simx_opmode_oneshot_wait)# sends signal to child script errorCode,joint=vrep.simxGetJointPosition(clientID,joint_Handle7,vrep.simx_opmode_oneshot_wait)# get the position of the joint7 time.sleep(5) while joint >= 0.0104653835297: errorCode=vrep.simxSetJointTargetPosition(clientID,joint_Handle1,math.radians(5),vrep.simx_opmode_oneshot_wait) errorCode=vrep.simxSetJointTargetPosition(clientID,joint_Handle11,math.radians(90),vrep.simx_opmode_oneshot_wait) break; time.sleep(2) errorCode=vrep.simxSetJointTargetPosition(clientID,joint_Handle4,math.radians(10),vrep.simx_opmode_oneshot_wait) errorCode=vrep.simxSetJointTargetPosition(clientID,joint_Handle1,math.radians(35),vrep.simx_opmode_oneshot_wait) vrep.simxSetIntegerSignal(clientID,"closing_signal",0,vrep.simx_opmode_oneshot_wait)
def joint_angle(cid, handle): err, ret = vrep.simxGetJointPosition(cid, handle, vrep_mode) return [ret]
def _read_sensors(self): """ This method is used for read the ePuck's sensors. Don't use directly, instead use 'step()' """ # Read differents sensors for s in self._sensors_to_read: if s == 'a': # Accelerometer sensor in a non filtered way if self._accelerometer_filtered: parameters = ('A', 12, '@III') else: parameters = ('a', 6, '@HHH') self._debug('WARNING: Accelerometer not yet implemented!') elif s == 'n': # Proximity sensors res, prox = vrep.simxGetStringSignal(self._clientID, 'EPUCK_proxSens', vrep.simx_opmode_streaming) if res != vrep.simx_return_ok: self._debug("WARNING: Proximity sensors readout failed: ", res) else: proxVals = vrep.simxUnpackFloats(prox) # TODO: Find out actual needed scaling factor proxVals = [int(x * 1000) for x in proxVals] self._proximity = tuple(proxVals) elif s == 'm': # Floor sensors res, floor1 = vrep.simxGetFloatSignal(self._clientID, 'EPUCK_mylightSens_0', vrep.simx_opmode_streaming) if res != vrep.simx_return_ok: self._debug("WARNING: Floor 1 sensor readout failed: ", res) res, floor2 = vrep.simxGetFloatSignal(self._clientID, 'EPUCK_mylightSens_1', vrep.simx_opmode_streaming) if res != vrep.simx_return_ok: self._debug("WARNING: Floor 2 sensor readout failed: ", res) res, floor3 = vrep.simxGetFloatSignal(self._clientID, 'EPUCK_mylightSens_2', vrep.simx_opmode_streaming) if res != vrep.simx_return_ok: self._debug("WARNING: Floor 3 sensor readout failed: ", res) # Scale returned values to mimic real robot; current factor is just guessed self._floor_sensors = (floor1*1800, floor2*1800, floor3*1800) elif s == 'q': # Motor position sensor # First: Get the handles of both motor joints res, leftMotor = vrep.simxGetObjectHandle(self._clientID, 'ePuck_leftJoint', vrep.simx_opmode_oneshot_wait) if res != vrep.simx_return_ok: self._debug("WARNING: Unable to get handle of left motor: ", res) continue res, rightMotor = vrep.simxGetObjectHandle(self._clientID, 'ePuck_rightJoint', vrep.simx_opmode_oneshot_wait) if res != vrep.simx_return_ok: self._debug("WARNING: Unable to get handle of right motor: ", res) continue # Second: Get the actual motor position (in radians) res, leftPos = vrep.simxGetJointPosition(self._clientID, leftMotor, vrep.simx_opmode_oneshot_wait) if res != vrep.simx_return_ok: self._debug("WARNING: Readout of left motor failed: ", res) continue res, rightPos = vrep.simxGetJointPosition(self._clientID, rightMotor, vrep.simx_opmode_streaming) if res != vrep.simx_return_ok: self._debug("WARNING: Readout of left motor failed: ", res) continue self._motor_position = (leftPos, rightPos) elif s == 'o': # Light sensors parameters = ('O', 16, '@HHHHHHHH') self._debug('WARNING: Light sensors not yet implemented!') elif s == 'u': # Microphone parameters = ('u', 6, '@HHH') self._debug('WARNING: Microphones not yet implemented!') elif s == 'e': # Motor Speed parameters = ('E', 4, '@HH') self._debug('WARNING: Motor speed not yet implemented!') elif s == 'i': # Do nothing for the camera, is an independent process pass else: self._debug('Unknow type of sensor to read')
def transform(pos_wrtworld): transmation_matrix = np.array([[-1,0,0,2.23],[0,-1,0,-0.775],[0,0,1,0.0771],[0,0,0,1]]) pos_wrt_robot = np.linalg.solve(transmation_matrix,np.array(pos_wrtworld)) return pos_wrt_robot if __name__ == "__main__": rospy.init_node('points_talker', anonymous=True) error_collect = [] vrep.simxFinish(-1) # just in case, close all opened connections clientID=vrep.simxStart('127.0.0.1',19997,True,True,5000,5) # Connect to V-REP joints = [1]*3 angles = np.zeros(3) for i in range(3): errorCode,joints[i] = vrep.simxGetObjectHandle(clientID,'IRB140_joint'+str(i+1),vrep.simx_opmode_blocking) angles[i] =(vrep.simxGetJointPosition(clientID,joints[i],vrep.simx_opmode_streaming)[1]) epsilon =6e-2 for i in range(3): vrep.simxSetJointTargetVelocity(clientID,joints[i],0.,vrep.simx_opmode_oneshot) vrep.simxSetJointForce(clientID,joints[i],9999.,vrep.simx_opmode_oneshot) # Kp = np.abs(error/np.sum(np.abs(error))) #Kp = 2000 #Kv = 100 # kp = [2000, 2000,2000] # kv = [100,100,100] res, floor_handle = vrep.simxGetObjectHandle(clientID, 'floor_handle', vrep.simx_opmode_blocking) res, s_handle = vrep.simxGetObjectHandle(clientID, 'kinect', vrep.simx_opmode_blocking) position = vrep.simxGetObjectPosition(clientID, s_handle, vrep.sim_handle_parent, vrep.simx_opmode_blocking) orientation = vrep.simxGetObjectOrientation(clientID, s_handle, vrep.sim_handle_parent, vrep.simx_opmode_buffer) print('-'*8+'Calculating Trajectory'+'-'*8) # print(position)
def callback(data): print('inside callback') data = data.data #print(data) #a,b,c = calc_parabola_vertex(data[0], data[1], data[2], data[3], data[4], data[5]) a,b = calc_straight_line(data[0], data[1], data[4], data[5]) print([a,b]) z_req_vision= ((a*(x_req))+(b))/2 # z_req_vision= (a*(x_req**2))+(b*x_req)+c print('z required from vision'+str(z_req_vision)) print('x required from vision'+str(x_req)) print('-'*8+'Checking if ball can be blocked'+'-'*8) transformation = np.array([[1,0,0,x_req*1000],[0,1,0,0],[0,0,1,z_req_vision*1000],[0,0,0,1]]) desired_angles = robot.inverseKinematics(transformation) if desired_angles: print('-'*8+'Moving'+'-'*8) error = desired_angles-angles #error = np.array(error) error_collect.append(error) #error_collect.append(error.reshape(3,1)) sum = np.sum(np.abs(error)) Kp = [1000*(sum-np.abs(i))/sum for i in error] Kv = 100 velocity_error = np.array([0 ,0 ,0]) desired_velocity = np.copy(velocity_error) for i in range(3): while abs(error[i])>epsilon: vrep.simxSetJointTargetVelocity(clientID,joints[i],error[i]*9999/abs(error[i]),vrep.simx_opmode_oneshot) # if error[i]>0.5: # vrep.simxSetJointForce(clientID,joints[i],abs(Kp*0.5)+(Kv*velocity_error[i]),vrep.simx_opmode_oneshot) # else:#+(Kv*velocity_error[i]) if abs(error[i])<epsilon: vrep.simxSetJointTargetVelocity(clientID,joints[i],0.,vrep.simx_opmode_oneshot) vrep.simxSetJointForce(clientID,joints[i],9999.,vrep.simx_opmode_oneshot) vrep.simxSetJointForce(clientID,joints[i],abs(Kp[i]*(error[i]))+Kv*velocity_error[i],vrep.simx_opmode_oneshot) error[i] = desired_angles[i] - (vrep.simxGetJointPosition(clientID,joints[i],vrep.simx_opmode_streaming)[1]) #print(vrep.simxGetJointPosition(clientID,joints[i],vrep.simx_opmode_streaming)[1]) if abs(error[i])<epsilon: vrep.simxSetJointTargetVelocity(clientID,joints[i],0.,vrep.simx_opmode_oneshot) vrep.simxSetJointForce(clientID,joints[i],9999.,vrep.simx_opmode_oneshot) res,velocity = vrep.simxGetObjectFloatParameter(clientID,joints[i],2012,vrep.simx_opmode_blocking) velocity_error[i] = desired_velocity[i] - velocity #error_collect.append(error) #print(error) # while np.sum(np.abs(error))>epsilon: # for i in range(6): # if abs(error[i])<epsilon/6.: # vrep.simxSetJointTargetVelocity(clientID,joints[i],0.,vrep.simx_opmode_oneshot) # vrep.simxSetJointForce(clientID,joints[i],9999.,vrep.simx_opmode_oneshot) # else: # vrep.simxSetJointTargetVelocity(clientID,joints[i],error[i]*9999/abs(error[i]),vrep.simx_opmode_oneshot) # vrep.simxSetJointForce(clientID,joints[i],abs(Kp*(error[i])),vrep.simx_opmode_oneshot) # error[i] = desired_angles[i] - (vrep.simxGetJointPosition(clientID,joints[i],vrep.simx_opmode_streaming)[1]) a = [] for i in range(3): vrep.simxSetJointTargetVelocity(clientID,joints[i],0,vrep.simx_opmode_oneshot) vrep.simxSetJointForce(clientID,joints[i],9999,vrep.simx_opmode_oneshot) a.append(vrep.simxGetJointPosition(clientID,joints[i],vrep.simx_opmode_streaming)[1]) # print(a) else: for i in range(3): vrep.simxSetJointTargetVelocity(clientID,joints[i],0.,vrep.simx_opmode_oneshot) vrep.simxSetJointForce(clientID,joints[i],9999.,vrep.simx_opmode_oneshot) print('hi') sub_MTML.unregister()