def spiral_move(radius, height): err, Qpos = vrep.simxGetObjectPosition(clientID, QuadricopterT, -1, vrep.simx_opmode_oneshot_wait) x = Qpos[0] y = Qpos[1] z = Qpos[2] go_to_z(height) z = height theta = 0 go_to_x(x + radius * np.cos(theta)) time.sleep(2) theta = 0 while True: err = vrep.simxSetObjectPosition( clientID, QuadricopterT, -1, (x + radius * np.cos(theta), y + radius * np.sin(theta), z), vrep.simx_opmode_oneshot_wait) time.sleep(0.01) theta += np.pi / (90 * radius + 1) radius /= 1.001 err, Qpos = vrep.simxGetObjectPosition(clientID, QuadricopterT, -1, vrep.simx_opmode_oneshot_wait) print(Qpos[0]**2 + Qpos[1]**2)
def go_to_y(y1): global x, y, z err, Qpos = vrep.simxGetObjectPosition(clientID, QuadricopterT, -1, vrep.simx_opmode_oneshot_wait) x = Qpos[0] y = Qpos[1] z = Qpos[2] if y < y1: sign = -1 elif y > y1: sign = 1 else: return 0 while abs(y - y1) > 0.01: err = vrep.simxSetObjectPosition(clientID, QuadricopterT, -1, (x, y, z), vrep.simx_opmode_oneshot_wait) time.sleep(0.2) y -= sign * 0.01 print(y) print('\n') err = vrep.simxSetObjectPosition(clientID, QuadricopterT, -1, (x, y1, z), vrep.simx_opmode_oneshot_wait) time.sleep(0.2) err, Qpos = vrep.simxGetObjectPosition(clientID, QuadricopterT, -1, vrep.simx_opmode_oneshot_wait) print(Qpos) y = y1
def go_to_z(z1): global x, y, z err, Qpos = vrep.simxGetObjectPosition(clientID, QuadricopterT, -1, vrep.simx_opmode_oneshot_wait) x = Qpos[0] y = Qpos[1] z = Qpos[2] if z < z1: sign = -1 elif z > z1: sign = 1 else: return 0 while abs(z - z1) > 0.01: err = vrep.simxSetObjectPosition(clientID, QuadricopterT, -1, (x, y, z), vrep.simx_opmode_oneshot_wait) time.sleep(0.2) z -= sign * 0.01 print(z) print('\n') err = vrep.simxSetObjectPosition(clientID, QuadricopterT, -1, (x, y, z1), vrep.simx_opmode_oneshot_wait) time.sleep(0.2) err, Qpos = vrep.simxGetObjectPosition(clientID, QuadricopterT, -1, vrep.simx_opmode_oneshot_wait) print(Qpos) z = z1
def get_forward_kinematic(self): fk_list = [] res, tran = sim.simxGetObjectPosition(self.clientID, self.joints[0], -1, sim.simx_opmode_blocking) res, rot = sim.simxGetObjectOrientation(self.clientID, self.joints[0], -1, sim.simx_opmode_blocking) fk_list.append(util.matrix_transform(np.array(rot), np.array(tran))) for idx in range(1, 7): # transformation to parent frame res, tran = sim.simxGetObjectPosition(self.clientID, self.joints[idx], self.joints[idx - 1], sim.simx_opmode_blocking) res, rot = sim.simxGetObjectOrientation(self.clientID, self.joints[idx], self.joints[idx - 1], sim.simx_opmode_blocking) fk_list.append(util.matrix_transform(rot, np.array(tran))) # endeffector res, tran = sim.simxGetObjectPosition( self.clientID, self.endEffector, self.joints[len(self.joints) - 1], sim.simx_opmode_blocking) res, rot = sim.simxGetObjectOrientation( self.clientID, self.endEffector, self.joints[len(self.joints) - 1], sim.simx_opmode_blocking) fk_list.append(util.matrix_transform(rot, np.array(tran))) self.fk = np.array(fk_list)
def forward_kinematics_test(self): self.get_forward_kinematic(self) self.get_current_pose(self) for i in range(100): config = np.random.rand(6) self.apply_forward_kinematics(config) self.apply_control(self.control) self.compare_pose() time.sleep(1) return # taget position targetID, targetPos, targetRot = self.determine_target('target') res, targetPos = sim.simxGetObjectPosition(self.clientID, targetID, self.worldCoord, sim.simx_opmode_oneshot) res, targetRot = sim.simxGetObjectOrientation(self.clientID, targetID, self.worldCoord, sim.simx_opmode_oneshot) x, y, z = targetPos rx, ry, rz = targetRot target = np.array([[x, y, z, rx, ry, rz]]) # get endeffector res, tran = sim.simxGetObjectPosition(self.clientID, self.endEffector, self.worldCoord, sim.simx_opmode_oneshot) res, rot = sim.simxGetObjectOrientation(self.clientID, self.endEffector, self.worldCoord, sim.simx_opmode_oneshot) end_pos = np.array([tran, rot]).flatten()
def compute_jacobian(self, targetPos, targetRot): # target position in global frame x, y, z = targetPos rx, ry, rz = targetRot target = np.array([[x, y, z, rx, ry, rz]]) # end effector position in global frame res, endEffPos = sim.simxGetObjectPosition(self.clientID, self.endEffector, self.worldCoord, sim.simx_opmode_blocking) x, y, z = endEffPos res, endEffRot = sim.simxGetObjectOrientation(self.clientID, self.endEffector, self.worldCoord, sim.simx_opmode_blocking) rx, ry, rz = endEffRot endEffectorPos = np.array([[x, y, z, rx, ry, rz]]) error = target - endEffectorPos # start computing jacobian jacobian = [] joint_q = [] res, trans1 = sim.simxGetObjectPosition( self.clientID, self.endEffector, self.worldCoord, sim.simx_opmode_blocking) # end effector in world for idx in range(len(self.joints)): # compute local jacobian local_rot_axis = self.rot_axis[idx] # find the current joint configuration res, local_config = sim.simxGetJointPosition( self.clientID, self.joints[idx], sim.simx_opmode_blocking) joint_q.append((local_config)) # find transform matrix res, rot = sim.simxGetObjectOrientation(self.clientID, self.joints[idx], self.worldCoord, sim.simx_opmode_blocking) rot_mat = util.get_rotation_matrix(rot) # rotation axis in world coordinate local_rot_axis = rot_mat.dot(local_rot_axis).flatten() # joint position in world res, trans2 = sim.simxGetObjectPosition(self.clientID, self.joints[idx], self.worldCoord, sim.simx_opmode_blocking) # find the translation from local position to world position zi = np.array(trans1) - np.array(trans2) j1 = np.cross(local_rot_axis, zi) local_jacobian = np.hstack([j1, local_rot_axis]) jacobian.append(local_jacobian) jacobian = np.array(jacobian).T joint_q = np.array([joint_q]) # INVERSE JACOBIAN return jacobian, error, joint_q
def calc_distance(self): """Calculates the distance between the end effector and a target position Args: Returns: right_distance left_distance """ error_code, self.right_xyz_hand = vrep.simxGetObjectPosition( self.clientID, self.right_hand, -1, vrep.simx_opmode_buffer) # right hand error_code, self.left_xyz_hand = vrep.simxGetObjectPosition( self.clientID, self.left_hand, -1, vrep.simx_opmode_buffer) # left hand # removed getting right_target location each time- using the static initial location # error_code, self.right_xyz_target = vrep.simxGetObjectPosition(self.clientID, self.right_target, -1, # vrep.simx_opmode_buffer) # TODO set this to main target, find static point. # need to check if this formula is calculating distance properly right_distance = 1 / math.sqrt( pow((self.right_xyz_hand[0] - self.right_xyz_target[0]), 2) + pow((self.right_xyz_hand[1] - self.right_xyz_target[1]), 2) + pow((self.right_xyz_hand[2] - self.right_xyz_target[2]), 2)) left_distance = 1 / math.sqrt( pow((self.left_xyz_hand[0] + self.left_xyz_target[0]), 2) + pow((self.left_xyz_hand[1] + self.left_xyz_target[1]), 2) + pow((self.left_xyz_hand[2] + self.left_xyz_target[2]), 2)) return right_distance, left_distance
def step(self, action, reset): if sys.version_info[0] == 3: _ = sim.simxSetObjectPosition(self.clientID, self.target, -1, self.target_position, sim.simx_opmode_oneshot) sensor_data = np.zeros((self.total_sensors), dtype=np.float32) vel_reading = np.zeros((2), dtype=np.float32) angular_reading = 0 collision = np.zeros((2), dtype=np.float32) target_location = np.zeros((3), dtype=np.float32) target_location = np.round_( np.subtract(np.array(self.position[:2]), np.array(self.target_position[:2])), 3) if (reset == False): if (self.flag): _, target_location = sim.simxGetObjectPosition( self.clientID, self.target, -1, sim.simx_opmode_buffer) _, bot_location = sim.simxGetObjectPosition( self.clientID, self.bot, -1, sim.simx_opmode_buffer) target_location = np.round_([ bot_location[0] - target_location[0], bot_location[1] - target_location[1] ], 3) self.flag = True speed = (self.velocity * action[0]) / 100 turn = (self.angular_velocity * action[1]) / 100 l_wheel_vel = round( (speed - self.wheel_basis * turn) / self.wheel_radius, 4) r_wheel_vel = round( (speed + self.wheel_basis * turn) / self.wheel_radius, 4) _ = sim.simxSetJointTargetVelocity(self.clientID, self.left_wheel, l_wheel_vel, sim.simx_opmode_streaming) _ = sim.simxSetJointTargetVelocity(self.clientID, self.right_wheel, r_wheel_vel, sim.simx_opmode_streaming) #Collision _, collision[0] = sim.simxGetIntegerSignal( self.clientID, "collision_wall", sim.simx_opmode_buffer) _, collision[1] = sim.simxGetIntegerSignal( self.clientID, "collision_target", sim.simx_opmode_buffer) sensor_data = self._readsensor_continue() vel_reading, angular_reading = self._get_velocity_reading_continue( ) else: self._create(self.position) #_ = sim.simxSetObjectPosition(self.clientID, self.target, self.bot, self.target_position, sim.simx_opmode_oneshot) #target_location = self.position - self.target_position sim.simxSynchronousTrigger(self.clientID) return sensor_data, vel_reading, angular_reading, target_location, collision else: raw_input('Press <enter> key to step the !')
def andar_em_metros(d, v, m): # d = 1 , andar para frente # d =-1 , andar para trás # v = velocidade # m = valor em metros erro, a_inicial = sim.simxGetObjectPosition(clientID, robo, -1, sim.simx_opmode_blocking) x_inicial = a_inicial[0] y_inicial = a_inicial[1] while (True): erro, a = sim.simxGetObjectPosition(clientID, robo, -1, sim.simx_opmode_blocking) x = a[0] y = a[1] #print(x,y) if (abs(x - x_inicial) >= m or abs(y - y_inicial) >= m): break sim.simxPauseCommunication(clientID, True) sim.simxSetJointTargetVelocity(clientID, robotRightMotor, d * v, sim.simx_opmode_oneshot) sim.simxSetJointTargetVelocity(clientID, robotLeftMotor, d * v, sim.simx_opmode_oneshot) sim.simxPauseCommunication(clientID, False) Stop()
def get_b_parameter(self): _, rr = sim.simxGetObjectPosition(self.client_id, self.right_motor_handle, -1, sim.simx_opmode_oneshot_wait) _, rl = sim.simxGetObjectPosition(self.client_id, self.left_motor_handle, -1, sim.simx_opmode_oneshot_wait) return np.sqrt((rr[0] - rl[0])**2 + (rr[1] - rl[1])**2 + (rr[2] - rl[2])**2) / 2
def ResetSimulationScene(self): errorCode, resolution, image = sim.simxGetVisionSensorImage( self.clientID, self.camera, 0, sim.simx_opmode_streaming) pos = sim.simxGetObjectPosition(self.clientID, self.khepera, -1, sim.simx_opmode_streaming) ori_body = sim.simxGetObjectOrientation(self.clientID, self.khepera, -1, sim.simx_opmode_streaming) tar = sim.simxGetObjectPosition(self.clientID, self.target, -1, sim.simx_opmode_streaming)
def drone_position(args): drones = [[] for i in range(3)] drones_names = [ 'Quadricopter_base', 'Quadricopter_base#0', 'Quadricopter_base#1' ] nodes = [] data = [[] for i in range(3)] if len(args) > 1: for n in range(1, len(args)): nodes.append(args[n]) else: info("No nodes defined") exit() info('Program started') sim.simxFinish(-1) # just in case, close all opened connections clientID = sim.simxStart('127.0.0.1', 19999, True, True, 5000, 5) # Connect to CoppeliaSim if clientID != -1: res = None info('Connected to remote API server') # Getting the ID of the drones from the simulation for i in range(0, len(drones)): [res, drones[i]] = sim.simxGetObjectHandle(clientID, drones_names[i], sim.simx_opmode_oneshot_wait) if res == sim.simx_return_ok: info('Connected with CoppeliaSim') else: info('Remote API function call returned with error code: ', res) time.sleep(2) # Starting the getPosition function streaming for i in range(0, len(drones)): sim.simxGetObjectPosition(clientID, drones[i], -1, sim.simx_opmode_streaming) while True: # Getting the positions as buffers for i in range(0, len(drones)): # Try to retrieve the streamed data returnCode, data[i] = sim.simxGetObjectPosition( clientID, drones[i], -1, sim.simx_opmode_buffer) # Storing the position in data files for i in range(0, len(data)): send_file(data[i], nodes[i]) time.sleep(1) # Now close the connection to CoppeliaSim: sim.simxFinish(clientID) else: info('Failed connecting to remote API server') info('Program ended')
def init_robot(self): for h in self.handles: sim.simxSetJointPosition(self.clientID, h, 0, sim.simx_opmode_oneshot) print("termino de inicializar") _, pos = sim.simxGetObjectPosition(self.clientID, self.j2, -1, sim.simx_opmode_streaming) _, pos = sim.simxGetObjectPosition(self.clientID, self.j1, -1, sim.simx_opmode_streaming)
def __init__(self, queue_dict, sync=True): """ this class loads coppeliasim and is responsible for communication between the sim and python... api functions can be found https://www.coppeliarobotics.com/helpFiles/en/remoteApiFunctionsPython.htm """ # open queues self.queues = queue_dict # close all connection that are remaining sim.simxFinish(-1) self.clientID = -1 attempt_num = 0 while self.clientID == -1: print("attempting to connect to VREP...") self.clientID = sim.simxStart('127.0.0.1', 19999, True, True, 5000, 5) attempt_num += 1 if attempt_num >= 3: print("could not connect to vrep") return print("successful connection!") self.sync = sync if self.sync: # set the simulation to synchronise with api sim.simxSynchronous(self.clientID, True) # get coppeliasim object handles self.motor = {'left': self.getHandle("Pioneer_p3dx_leftMotor"), 'right': self.getHandle("Pioneer_p3dx_rightMotor")} self.camHandle = self.getHandle("camera") #self.gripperHandle = {"left": self.getHandle("left_joint"), # "right": self.getHandle("right_joint")} # init camera data stream with rgb and depth sim.simxGetVisionSensorImage(self.clientID, self.camHandle, 0, sim.simx_opmode_streaming) sim.simxGetVisionSensorDepthBuffer(self.clientID, self.camHandle, sim.simx_opmode_streaming) # init position data stream with cartesian position and euler orientation sim.simxGetObjectOrientation(self.clientID, self.camHandle, -1, sim.simx_opmode_streaming) sim.simxGetObjectPosition(self.clientID, self.camHandle, -1, sim.simx_opmode_streaming) # setting motor speed self.speed = 1 self.lr = 0.3 self.images = None # remote start simulation if not already sim.simxStartSimulation(self.clientID, sim.simx_opmode_oneshot) # used for making keyboard switches self.keyboard_key = Clicker("c", activated=True) self.keyboard_controlled = self.keyboard_key.activated self.gripper_key = Clicker("g", activated=False) self.gripper_activated = self.gripper_key.activated
def track(self): print("start tracking") while True: if (self.pos[0] - self.pickupPath[-1][0])**2 + ( self.pos[1] - self.pickupPath[-1][1])**2 < 2: self.robot.setLeftVelocity(0) self.robot.setRightVelocity(0) break lookpoint = self.lookhead(self.pos, self.pickupPath) curv = self.curvature(lookpoint) wheels = self.turn(curv, self.width) self.robot.setLeftVelocity(wheels[0]) self.robot.setRightVelocity(-wheels[1]) self.pos = self.robot.getPos()[0:2] self.angle = self.robot.getAngle() self.socketio.sleep(0) self.reset() print("start picking") print(self.robot) err, platform = sim.simxGetObjectHandle( self.warehouse.client, "platform_" + self.robot.id, sim.simx_opmode_blocking, ) err, pos = sim.simxGetObjectPosition(self.warehouse.client, platform, -1, sim.simx_opmode_blocking) pos[2] = pos[2] + 0.1 self.warehouse.movePackage(self.pkg, pos, platform) self.warehouse.movePackage(self.pkg, pos, platform) self.warehouse.movePackage(self.pkg, pos, platform) while True: if (self.pos[0] - self.dropPath[-1][0])**2 + ( self.pos[1] - self.dropPath[-1][1])**2 < 2: self.robot.setLeftVelocity(0) self.robot.setRightVelocity(0) break lookpoint = self.lookhead(self.pos, self.dropPath) curv = self.curvature(lookpoint) wheels = self.turn(curv, self.width) self.robot.setLeftVelocity(wheels[0]) self.robot.setRightVelocity(-wheels[1]) self.pos = self.robot.getPos()[0:2] self.angle = self.robot.getAngle() self.socketio.sleep(0) err, pos = sim.simxGetObjectPosition(self.warehouse.client, platform, -1, sim.simx_opmode_blocking) pos[2] = 0.1 pos[0] = pos[0] - 0.3 pos[1] = pos[1] - 0.3 self.warehouse.movePackage(self.pkg, pos, -1) self.warehouse.movePackage(self.pkg, pos, -1)
def compare_pose(self): # get the current 6-D position of robot for idx in range(len(self.joints)): res, tran = sim.simxGetObjectPosition(self.clientID, self.joints[idx], self.worldCoord, sim.simx_opmode_blocking) print(np.array(util.homo_coord(tran)) - self.cur_pos[idx]) res, tran = sim.simxGetObjectPosition(self.clientID, self.endEffector, self.worldCoord, sim.simx_opmode_blocking) print(np.array(util.homo_coord(tran)) - self.cur_pos[6])
def ResetSimulationScene(self): errorCode, resolution, image = sim.simxGetVisionSensorImage( self.clientID, self.camera, 0, sim.simx_opmode_streaming) pos = sim.simxGetObjectPosition(self.clientID, self.khepera, -1, sim.simx_opmode_streaming) ori_body = sim.simxGetObjectOrientation(self.clientID, self.khepera, -1, sim.simx_opmode_streaming) tar = sim.simxGetObjectPosition(self.clientID, self.target, -1, sim.simx_opmode_streaming) for i in range(8): handle = self.sensor[i] errorCode, detectionState, detectedPoint, detectedObjectHandle, detectedSurfaceNormalVector = sim.simxReadProximitySensor( self.clientID, handle, sim.simx_opmode_streaming)
def step(self, action): if action <=5: x,y,z = self.moves[action] self.S = [self.S[0] + x, self.S[1] + y, self.S[2] + z] if self.S[0]<0 or self.S[1]<0 or self.S[2]<0: self.S = [max(0, self.S[0]), max(0, self.S[1]), max(0, self.S[2])] return self.S,-10,False,{} if self.S[0]>=5 or self.S[1]>=5 or self.S[2]>=10: self.S = [min(self.S[0], 5 - 1), min(self.S[1], 5 - 1), min(self.S[2], 10 - 1)] return self.S,-10,False,{} else: sim.simxSetIntegerSignal(self.clientID,'actuateGripperSignal',0,sim.simx_opmode_oneshot) sim.simxSetObjectPosition(self.clientID, self.target,-1, [0+0.1*self.S[0],0+0.1*self.S[1],0+0.1*self.S[2]],sim.simx_opmode_oneshot) #starting point is (0.3,0.3,0.3) while True: distance = sim.simxGetObjectPosition(self.clientID,self.copter,self.target,sim.simx_opmode_oneshot_wait) if np.abs(np.linalg.norm(np.array(distance[1])))<0.02: time.sleep(2) break # if (sim.simxGetCollisionHandle(self.clientID,)) return self.S,-1,False,{} elif action==6: # pdb.set_trace() gripperAngle = 150 angle_displacement = (gripperAngle/2-45)*math.pi/180 err, joint_6 = sim.simxGetObjectHandle(self.clientID, "Fourbar_joint6", sim.simx_opmode_oneshot) # sim.simxClearIntegerSignal(self.clientID,'actuateGripperSignal',sim.simx_opmode_oneshot) # sim.simxClearIntegerSignal(self.clientID,'gripperAngle',sim.simx_opmode_oneshot) sim.simxSetIntegerSignal(self.clientID,'actuateGripperSignal',1,sim.simx_opmode_oneshot) sim.simxSetIntegerSignal(self.clientID,'gripperAngle',gripperAngle,sim.simx_opmode_oneshot) while True: current_joint6_angle = sim.simxGetJointPosition(self.clientID, joint_6, sim.simx_opmode_oneshot) if abs(abs(current_joint6_angle[1])-abs(angle_displacement))<0.05*math.pi/180: time.sleep(5) break oldPos = sim.simxGetObjectPosition(self.clientID, self.sphere, -1, sim.simx_opmode_oneshot) sim.simxSetObjectPosition(self.clientID, self.target, self.copter, [0,0,0.5], sim.simx_opmode_oneshot) while True: distance = sim.simxGetObjectPosition(self.clientID,self.copter,self.target,sim.simx_opmode_oneshot_wait) if np.abs(np.linalg.norm(np.array(distance[1])))<0.02: time.sleep(2) break newPos = sim.simxGetObjectPosition(self.clientID, self.sphere, -1, sim.simx_opmode_oneshot) if np.linalg.norm(np.array(newPos[1])-np.array(oldPos[1]))>0.4 and np.linalg.norm(np.array(newPos)-np.array(oldPos))<0.5: return self.S,100,True,{} else: return self.S,-100,True,{}
def get_current_pose(self): cur_pos = [] # get the current 6-D position of robot for idx in range(len(self.joints)): res, tran = sim.simxGetObjectPosition(self.clientID, self.joints[idx], -1, sim.simx_opmode_blocking) cur_pos.append(np.array(util.homo_coord(tran))) # get the curretn end effector position res, tran = sim.simxGetObjectPosition(self.clientID, self.endEffector, -1, sim.simx_opmode_blocking) cur_pos.append(np.array(util.homo_coord(tran))) self.cur_pos = np.array(cur_pos)
def check_suction_distance(self): error_code, self.right_xyz_hand = vrep.simxGetObjectPosition( self.clientID, self.right_hand, -1, vrep.simx_opmode_buffer) # right hand error_code, self.left_xyz_hand = vrep.simxGetObjectPosition( self.clientID, self.left_hand, -1, vrep.simx_opmode_buffer) # left hand distance = math.sqrt( pow((self.right_xyz_hand[0] - self.left_xyz_hand[0]), 2) + pow((self.right_xyz_hand[1] - self.left_xyz_hand[1]), 2) + pow((self.right_xyz_hand[2] - self.left_xyz_hand[2]), 2)) return distance
def _getFinalPos(self, initialize=True): if initialize: ret, pos = sim.simxGetObjectPosition(self.clientID, self.testDummyHandle, -1, sim.simx_opmode_streaming) else: ret, pos = sim.simxGetObjectPosition(self.clientID, self.testDummyHandle, -1, sim.simx_opmode_buffer) if ret == sim.simx_return_ok: return pos else: # print('[WARNING] problem in getting tip position.') return -1
def calcDistance(self): errorCode, self.xyz_hand = vrep.simxGetObjectPosition( self.clientID, self.hand, -1, vrep.simx_opmode_buffer) errorCode, self.xyz_target = vrep.simxGetObjectPosition( self.clientID, self.target, -1, vrep.simx_opmode_buffer) # need to check if this formula is calculating distance properly distance = math.sqrt( pow((self.xyz_hand[0] - self.xyz_target[0]), 2) + pow((self.xyz_hand[1] - self.xyz_target[1]), 2) + pow((self.xyz_hand[2] - self.xyz_target[2]), 2)) return distance
def step(self, action): assert self.action_space.contains(action) joint = int(action / 3) handle = self.joint_handles[joint] value = self.action_map[action % 3] if joint in self.faulty_joints: value = 0 #for angle, handle in zip(action, self.joint_handles) cur_pos = sim.simxGetJointPosition(self.clientID, handle, buffer)[1] sim.simxSetJointTargetPosition(self.clientID, handle, cur_pos + value * (3.14 / 180), oneshot) # give reward for touching the box reward, done = self.detect_collision() box_distance = sim.simxGetObjectPosition(self.clientID, self.lamp_end, self.green_box, buffer)[1] state = [d for d in box_distance] for handle in self.joint_handles: state.append( sim.simxGetJointPosition(self.clientID, handle, buffer)[1]) return np.array(state), reward, done, {}
def get_position(self) -> list: """Retrieves the orientation. @rtype: Vec3 """ code, pos = s.simxGetObjectPosition(self._id, self._handle, -1, self._def_op_mode) return pos
def __init__(self, clientID): self.clientID = clientID self.pioneer3DX_array = [None] * 19 self.position_coordX = [None] * 3 self.position_coordXc = [None] * 3 self.orientation = None self.velocity = [None]* 2 self.ultrasonic = np.zeros((16, 3)) self.name = "Pioneer_p3dx" self.left_motor = 'Pioneer_p3dx_leftMotor' self.right_motor = 'Pioneer_p3dx_rightMotor' self.ultrasonic_sensors = "Pioneer_p3dx_ultrasonicSensor" ### Load Mobile Robot Pioneer parameters # self.pioneer3DX_array[0] represents the entire mobile robot block error,self.pioneer3DX_array[0] = sim.simxGetObjectHandle(self.clientID,self.name,sim.simx_opmode_blocking) # self.pioneer3DX_array[1] represents only the left motor error,self.pioneer3DX_array[1] = sim.simxGetObjectHandle(self.clientID,self.left_motor,sim.simx_opmode_blocking) # self.pioneer3DX_array[2] represents only the right motor error,self.pioneer3DX_array[2] = sim.simxGetObjectHandle(self.clientID,self.right_motor,sim.simx_opmode_blocking) # self.pioneer3DX_array[3:18] represents the 16 ultrasonic sensors num = 1 while num < 17: error,self.pioneer3DX_array[2+num] = sim.simxGetObjectHandle(self.clientID,self.ultrasonic_sensors+str(num),sim.simx_opmode_blocking) error, DetectionState, Points ,detectedObjectHandle, Vector = sim.simxReadProximitySensor(self.clientID,self.pioneer3DX_array[2+num],sim.simx_opmode_streaming) num+=1 error, linearVelocity, angularVelocity = sim.simxGetObjectVelocity(self.clientID,self.pioneer3DX_array[0], sim.simx_opmode_streaming) error, position = sim.simxGetObjectPosition(self.clientID,self.pioneer3DX_array[0],-1, sim.simx_opmode_streaming) error, angle = sim.simxGetObjectOrientation(self.clientID,self.pioneer3DX_array[0],-1,sim.simx_opmode_streaming) print("Pioneer 3DX loaded")
def get_position(self): error, position = sim.simxGetObjectPosition(self.clientId, self.pioneerHandle, -1, sim.simx_opmode_buffer) if error == sim.simx_return_novalue_flag: print(str(error) + "! simxGetObjectPosition_buffer") return position
def get_label(sequence_sample, converter, clientID, Body): sequence_sample = sequence_sample[0] for duration in [0.02, 0.05]: for idx_f in range(sequence_sample.shape[0]): angle_recon = converter.frameRecon(sequence_sample[idx_f]) # angles: LSP, LSR, LEY, LER, RSP, RSR, REY, RER, LHP, LHR, LHYP, LKP, RHP, RHR, RHYP, RKP, LAP, LAR, RAP, RAR angles = converter.generateWholeJoints(angle_recon) assert (len(angles) == 20) transmit(clientID, Body, angles) time.sleep(duration) returnCode, position = sim.simxGetObjectPosition( clientID, Body['HeadYaw'], -1, sim.simx_opmode_buffer) if position[2] < 0.4 and position[2] > 0: #fall down print('fall') sim.simxStopSimulation(clientID, sim.simx_opmode_oneshot) print('stop') time.sleep(.1) sim.simxStartSimulation(clientID, sim.simx_opmode_oneshot) print('start') time.sleep(.1) #returnCode, position=sim.simxGetObjectPosition(clientID, Body['HeadYaw'], -1, sim.simx_opmode_streaming) return torch.tensor([0]).long() sim.simxStopSimulation(clientID, sim.simx_opmode_oneshot) print('go on: stop') time.sleep(.1) sim.simxStartSimulation(clientID, sim.simx_opmode_oneshot) print('go on: start') time.sleep(.1) #returnCode, position=sim.simxGetObjectPosition(clientID, Body['HeadYaw'], -1, sim.simx_opmode_streaming) return torch.tensor([1]).long()
def getSlist(clientID, joint_handles, base_handle): q = np.zeros((6, 3)) for i in range(6): q[i] = np.array( sim.simxGetObjectPosition(clientID, joint_handles[i], base_handle, sim.simx_opmode_blocking)[1]) w = np.zeros((6, 3)) w[0] = np.array([0, 0, 1]).T w[1] = np.array([-1, 0, 0]).T w[2] = np.array([-1, 0, 0]).T w[3] = np.array([-1, 0, 0]).T w[4] = np.array([0, 0, 1]).T w[5] = np.array([-1, 0, 0]).T v = np.zeros((6, 3)) for i in range(6): v[i] = np.cross(-w[i], q[i]) Slist = np.zeros((6, 6)) for i in range(6): Slist[i, :3] = w[i] Slist[i, 3:] = v[i] return Slist
def __init__(self): try: sim.simxFinish(-1) #close all opened connections clientID = sim.simxStart('127.0.0.1', 19999, True, True, 5000, 5) # Connect to CoppeliaSim if clientID != -1: print('connect successfully') else: sys.exit("connect error") except: print('Check if CoppeliaSim is open') _, Quadcopter_target = sim.simxGetObjectHandle( clientID, 'Quadricopter_target', sim.simx_opmode_blocking) _, targetPosition = sim.simxGetObjectPosition(clientID, Quadcopter_target, -1, sim.simx_opmode_buffer) print(targetPosition) ArrayPosition = [-0.18570, 0.99366, 0.615] sim.simxSetObjectPosition(clientID, Quadcopter_target, -1, ArrayPosition, sim.simx_opmode_oneshot) self.clientID = clientID self.Quadcopter_target = Quadcopter_target self.targetPosition = targetPosition
def scan(self): #Method for getting the coordinates of a cloud of detected points in absolute/world reference xy = [] for sonar in self.sonarHandles[0:16]: #For all sonars do # Ping the sonar res, detectionState, detectedPoint, detectedObjectHandle, detectedSurfaceNormalVector = sim.simxReadProximitySensor( clientID, sonar, sim.simx_opmode_buffer) # If a point is detected: if detectionState == 1: # Get the sensor orientation in Euler Angles for the sensor res, eulerAngle = sim.simxGetObjectOrientation( clientID, sonar, -1, sim.simx_opmode_blocking) # Get the position for the sonar vs the World frame (-1 value sees to that) res, position = sim.simxGetObjectPosition( clientID, sonar, -1, sim.simx_opmode_blocking) # Rotate the coordinates of the found point to receive absolute coordinates (minus the offset of the sensor location) v1 = rotate(detectedPoint[0], detectedPoint[1], detectedPoint[2], eulerAngle[0], eulerAngle[1], eulerAngle[2]) # Append the x and y coordinates of the detected poing plus the location of the sensor to get absolute coordinates xy.append((position[0] + v1[0], position[1] + v1[1])) return xy