def check_mouse(): mouseEvents = pybullet.getMouseEvents() for e in mouseEvents: eventType = e[0] mousePosX = e[1] mousePosY = e[2] buttonIndex = e[3] buttonState = e[4] button_type = "Unknown" button_id = "Unknown" button_action = "Unknown" if eventType == 1: button_type = "Movement" if eventType == 2: button_type = "Click" if buttonIndex == 0: button_id = "left" if buttonIndex == 1: button_id = "middle" if buttonIndex == 2: button_id = "right" if buttonState == 3: button_action = "down" if buttonState == 4: button_action = "release" if (eventType == 2): # Only log click print("Mouse " + button_id + " " + button_type + " " + button_action + " at: (" + str(mousePosX) + "," \ + str(mousePosY) + ")" + " ") return True return False
def changeCameraOnKeyboard(camDistance, yaw, pitch, x, y): """ Change camera zoom or angle from keyboard """ mouseEvents = p.getMouseEvents() keys = p.getKeyboardEvents() if ord(b'a') in keys: camDistance += 0.01 elif ord(b'd') in keys: camDistance -= 0.01 if ord(b'm') not in keys: if 65297 in keys: pitch += 0.2 if 65298 in keys: pitch -= 0.2 if 65295 in keys: yaw += 0.2 if 65296 in keys: yaw -= 0.2 return camDistance, yaw, pitch, 0, 0
def get_mouse_events(self): events = p.getMouseEvents(physicsClientId=self.sim.id) for event in events: if event[4] == 3: _, x, y, _, _ = event camera_params = p.getDebugVisualizerCamera( physicsClientId=self.sim.id) width = camera_params[0] height = camera_params[1] aspect = width / height pos_view = torch.tensor([ 1.0, aspect * (1.0 - (x / width) * 2), 1.0 - (y / height) * 2 ]) camera_forward = torch.tensor(camera_params[5]) camera_left = -torch.tensor(camera_params[6]) camera_left /= camera_left.norm() camera_up = torch.tensor(camera_params[7]) camera_up /= camera_up.norm() R = torch.stack([camera_forward, camera_left, camera_up], dim=1) camera_target = torch.tensor(camera_params[-1]) target_dist = camera_params[-2] camera_pos_world = camera_target - target_dist * camera_forward pos_world = R @ pos_view + camera_pos_world vec = pos_world - camera_pos_world vec = vec / vec.norm() ray = p.rayTest(rayFromPosition=camera_pos_world.tolist(), rayToPosition=(100.0 * vec + camera_pos_world).tolist(), physicsClientId=self.sim.id) hit_pos = torch.tensor(ray[0][3]) results = {} results['camera_pos'] = camera_pos_world results['target_pos'] = torch.tensor(ray[0][3]) results['target_obj'] = self.object_dict.get(ray[0][0], None) results['target_normal'] = torch.tensor(ray[0][4]) return results
def main(): p.setRealTimeSimulation(1) p.setGravity(0, 0, 0) # we don't use gravity yet # debug colors only for show colors = [[1, 0, 0, 1], [0, 1, 0, 1], [0, 0, 1, 1], [1, 1, 1, 1], [0, 0, 0, 1] ] currentColor = 0 try: body = p.getBodyUniqueId(mantis_robot) EndEffectorIndex = p.getNumJoints(body) -1 print("EndEffectorIndex %i" % EndEffectorIndex) tx = 0 ty = 5 tz = 3 tj = 0 tp = 0 tr = math.pi / 2 orn = p.getQuaternionFromEuler([tj,tp,tr]) jointT = p.calculateInverseKinematics(body, EndEffectorIndex, [tx, ty,tz], orn, jointDamping=jd) p.setJointMotorControlArray(body,[1,2,3,4,5,6] , controlMode=p.POSITION_CONTROL , targetPositions=jointT ) print("x:%f y:%f z:%f j:%f p:%f r:%f " % (tx,ty,tz,tj,tp,tr)) print(jointT) p.resetBasePositionAndOrientation(target, [tx, ty, tz ], orn) loop = 0 hasPrevPose = 0 trailDuration = 15 targetSelected = False distance = 5 yaw = 90 motorDir = [1,-1,1,1,1,1] while (p.isConnected()): time.sleep(1. / 240.) # set to 240 fps p.stepSimulation() # get target positon targetPos, targetOrn = p.getBasePositionAndOrientation(target) # do Inverse Kinematics # jointT = p.calculateInverseKinematics(body, EndEffectorIndex, targetPos, targetOrn) jointT = p.calculateInverseKinematics(body, EndEffectorIndex, targetPos, orn) # set Robot Joints p.setJointMotorControlArray(body,[1,2,3,4,5,6] , controlMode=p.POSITION_CONTROL , targetPositions=jointT ) # reduce the output speed to 1/10 of the simulation speed # ~24 fps loop +=1 if loop > 10: loop = 0 # sends data to the robot over ethernet/UDP # just a simple string with the motor number and the angle positon in 12 bit range (0-4096) data_string = "" for i in range ( 1, EndEffectorIndex): jt = p.getJointState(body, i ) data_string += "motor%i:%i\r\n" % ( i, scale(motorDir[i-1] * jt[0])) transfer_socket.sendto(bytes(data_string, "utf-8"), (transfer_ip, transfer_port)) # generates the trail for debug only # optional ls = p.getLinkState(body, EndEffectorIndex) targetPos, targetOrn = p.getBasePositionAndOrientation(target) if (hasPrevPose): p.addUserDebugLine(prevPose, targetPos, [0, 0, 0.3], 1, trailDuration) p.addUserDebugLine(prevPose1, ls[4], [1, 0, 0], 1, trailDuration) prevPose = targetPos prevPose1 = ls[4] hasPrevPose = 1 # get mouse events # changes color of the object clickt on and prints some info mouseEvents = p.getMouseEvents() for e in mouseEvents: if ((e[0] == 2) and (e[3] == 0) and (e[4] & p.KEY_WAS_RELEASED) and (targetSelected == True) ): # after mouse lost reset posion to prevent the target from floting around targetSelected = False targetPos, targetOrn = p.getBasePositionAndOrientation(target) p.resetBasePositionAndOrientation(target, targetPos, targetOrn) if ((e[0] == 2) and (e[3] == 0) and (e[4] & p.KEY_WAS_TRIGGERED)): mouseX = e[1] mouseY = e[2] rayFrom, rayTo = getRayFromTo(mouseX, mouseY) rayInfo = p.rayTest(rayFrom, rayTo) for l in range(len(rayInfo)): hit = rayInfo[l] objectUid = hit[0] jointUid = hit[1] if( objectUid == target): targetSelected = True if (objectUid >= 1): # this is for debug click on an object to get id # oject will change color this has no effect print("obj %i joint %i" % (objectUid , jointUid)) p.changeVisualShape(objectUid, jointUid, rgbaColor=colors[currentColor]) currentColor += 1 if (currentColor >= len(colors)): currentColor = 0 except KeyboardInterrupt: print("KeyboardInterrupt has been caught.") finally: endProgramm()
eef_orn = p.getEulerFromQuaternion(eef_orn) eef_pos = np.array(eef_pos) eef_orn = np.array(eef_orn) while True: images = p.getCameraImage( width, height, view_matrix, projection_matrix, # shadow=True, renderer=p.ER_BULLET_HARDWARE_OPENGL) # クリックされたobject_uidを取得 object_uid = -1 mouse_events = p.getMouseEvents() for e in mouse_events: if ((e[0] == 2) and (e[3] == 0) and (e[4] & p.KEY_WAS_TRIGGERED)): # 左クリック判定 mouse_x = e[1] mouse_y = e[2] object_uid = get_object_uid( mouse_x, mouse_y) # -1 for no object was clicked # print(object_uid) if object_uid < 0: continue # no object was clicked if object_uid not in block_ids: continue # a block was not clicked b_pos = get_block_pos(object_uid, images) if b_pos is None: continue
baseVisualShapeIndex=visualShapeId, basePosition=[((-rangex / 2) + i) * meshScale[0] * 2, (-rangey / 2 + j) * meshScale[1] * 2, 1], useMaximalCoordinates=True) p.changeVisualShape(bodyUid, -1, textureUniqueId=texUid) p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1) p.stopStateLogging(logId) p.setGravity(0, 0, -10) p.setRealTimeSimulation(1) colors = [[1, 0, 0, 1], [0, 1, 0, 1], [0, 0, 1, 1], [1, 1, 1, 1]] currentColor = 0 while (1): p.getCameraImage(320, 200) mouseEvents = p.getMouseEvents() for e in mouseEvents: if ((e[0] == 2) and (e[3] == 0) and (e[4] & p.KEY_WAS_TRIGGERED)): mouseX = e[1] mouseY = e[2] rayFrom, rayTo = getRayFromTo(mouseX, mouseY) rayInfo = p.rayTest(rayFrom, rayTo) #p.addUserDebugLine(rayFrom,rayTo,[1,0,0],3) for l in range(len(rayInfo)): hit = rayInfo[l] objectUid = hit[0] if (objectUid >= 1): #p.removeBody(objectUid) p.changeVisualShape(objectUid, -1, rgbaColor=colors[currentColor]) currentColor += 1 if (currentColor >= len(colors)):
linkInertialFramePositions=linkInertialFramePositions, linkInertialFrameOrientations=linkInertialFrameOrientations, linkParentIndices=indices, linkJointTypes=jointTypes, linkJointAxis=axis) p.changeDynamics(sphereUid, -1, spinningFriction=0.001, rollingFriction=0.001, linearDamping=0.0) for joint in range(p.getNumJoints(sphereUid)): p.setJointMotorControl2(sphereUid, joint, p.VELOCITY_CONTROL, targetVelocity=1, force=10) p.setGravity(0, 0, -9.81) p.setRealTimeSimulation(1) p.getNumJoints(sphereUid) for i in range(p.getNumJoints(sphereUid)): p.getJointInfo(sphereUid, i) while (1): keys = p.getKeyboardEvents() mouses = p.getMouseEvents() print(keys) time.sleep(0.01)
def main(): p.setRealTimeSimulation(1) p.setGravity(0, 0, 0) # we don't use gravity yet # debug colors only for show colors = [[1, 0, 0, 1], [0, 1, 0, 1], [0, 0, 1, 1], [1, 1, 1, 1], [0, 0, 0, 1]] currentColor = 0 try: print("open SpaceMouse") print(spacenavigator.list_devices()) success = spacenavigator.open() body = p.getBodyUniqueId(mantis_robot) EndEffectorIndex = p.getNumJoints(body) - 1 print("") print("EndEffectorIndex %i" % EndEffectorIndex) # i = 0 # for i in range(0, p.getNumJoints(body)): # n = p.getJointInfo(body,i) # alpha robot orientierung # orn = p.getQuaternionFromEuler([math.pi/2,0,math.pi/2]) # p.resetBasePositionAndOrientation(body, [0, 0, 2 ], orn) tx = 0 ty = 5 tz = 3 tj = 0 tp = 0 tr = math.pi / 2 orn = p.getQuaternionFromEuler([tj, tp, tr]) jointT = p.calculateInverseKinematics(body, EndEffectorIndex, [tx, ty, tz], orn, jointDamping=jd) p.setJointMotorControlArray(body, [1, 2, 3, 4, 5, 6], controlMode=p.POSITION_CONTROL, targetPositions=jointT) print("x:%f y:%f z:%f j:%f p:%f r:%f " % (tx, ty, tz, tj, tp, tr)) print(jointT) p.resetBasePositionAndOrientation(target, [tx, ty, tz], orn) loop = 0 hasPrevPose = 0 trailDuration = 15 distance = 5 yaw = 90 motorDir = [1, -1, 1, 1, 1, 1] while (p.isConnected()): time.sleep(1. / 240.) # set to 40 fps p.stepSimulation() # targetVelocity = p.readUserDebugParameter(targetVelocitySlider) if success: state = spacenavigator.read() # print(state) tx += state[1] * 0.01 ty += state[2] * 0.01 tz += state[3] * 0.01 # tj += state[4]*0.005 tp += state[5] * 0.005 tr += state[6] * -0.005 # print("x:%f y:%f z:%f j:%f p:%f r:%f " % (state[1],state[2],state[3],state[4],state[5],state[6])) # orn = p.getQuaternionFromEuler([tj,tp,tr]) orn = p.getQuaternionFromEuler([tj, tp, tr]) jointT = p.calculateInverseKinematics(body, EndEffectorIndex, [tx, ty, tz], orn) p.setJointMotorControlArray(body, [1, 2, 3, 4, 5, 6], controlMode=p.POSITION_CONTROL, targetPositions=jointT) # print("x:%f y:%f z:%f j:%f p:%f r:%f " % (tx,ty,tz,tj,tp,tr)) p.resetBasePositionAndOrientation(target, [tx, ty, tz], orn) targetPos, targetOrn = p.getBasePositionAndOrientation(target) # p.resetDebugVisualizerCamera(distance, yaw, 20, targetPos) loop += 1 if loop > 10: loop = 0 data_string = "" for i in range(1, EndEffectorIndex): jt = p.getJointState(body, i) data_string += "motor%i:%i\r\n" % ( i, scale(motorDir[i - 1] * jt[0])) transfer_socket.sendto(bytes(data_string, "utf-8"), (transfer_ip, transfer_port)) ls = p.getLinkState(body, EndEffectorIndex) targetPos, targetOrn = p.getBasePositionAndOrientation(target) if (hasPrevPose): p.addUserDebugLine(prevPose, targetPos, [0, 0, 0.3], 1, trailDuration) p.addUserDebugLine(prevPose1, ls[4], [1, 0, 0], 1, trailDuration) prevPose = targetPos prevPose1 = ls[4] hasPrevPose = 1 # get mouse events mouseEvents = p.getMouseEvents() for e in mouseEvents: if ((e[0] == 2) and (e[3] == 0) and (e[4] & p.KEY_WAS_TRIGGERED)): mouseX = e[1] mouseY = e[2] rayFrom, rayTo = getRayFromTo(mouseX, mouseY) rayInfo = p.rayTest(rayFrom, rayTo) for l in range(len(rayInfo)): hit = rayInfo[l] objectUid = hit[0] jointUid = hit[1] if (objectUid >= 1): # this is for debug click on an object to get id # oject will change color this has no effect # changing color real time seems to slow print("obj %i joint %i" % (objectUid, jointUid)) # n = p.getJointInfo(objectUid,jointUid) n = p.getJointState(objectUid, jointUid) print(n) p.changeVisualShape(objectUid, jointUid, rgbaColor=colors[currentColor]) currentColor += 1 if (currentColor >= len(colors)): currentColor = 0 except KeyboardInterrupt: print("KeyboardInterrupt has been caught.") finally: endProgramm()
hand_po[1]) elif k == 65296: #right p.resetBasePositionAndOrientation( hand, (hand_po[0][0], hand_po[0][1] - move, hand_po[0][2]), hand_po[1]) elif k == 44: #< p.resetBasePositionAndOrientation( hand, (hand_po[0][0], hand_po[0][1], hand_po[0][2] + move), hand_po[1]) elif k == 46: #> p.resetBasePositionAndOrientation( hand, (hand_po[0][0], hand_po[0][1], hand_po[0][2] - move), hand_po[1]) #print("key:",k) mouse = p.getMouseEvents() if len(mouse) > 0: pass #print("mouse len",len(mouse)) #print("mouse",mouse) if downCameraOn: viewMatrix = down_view() else: viewMatrix = ahead_view() projectionMatrix = p.computeProjectionMatrixFOV(fov, aspect, nearPlane, farPlane) w, h, img_arr, depths, mask = p.getCameraImage(200, 200, viewMatrix, projectionMatrix, lightDirection, lightColor, renderer=p.ER_TINY_RENDERER)
def get_mouse_events(): return list( MouseEvent(*event) for event in p.getMouseEvents(physicsClientId=CLIENT))
def main(): p.setRealTimeSimulation(1) # p.setGravity(0, 0, -10) # we don't use gravity yet atexit.register(endThreads) for light in lights: print("universe %d" % light.universe) aOutput.startOut(p, lights) mInput.startInput(lights, colliders) wInput.startInput() # debug colors only for show colors = [[1, 0, 0, 1], [0, 1, 0, 1], [0, 0, 1, 1], [1, 1, 1, 1]] currentColor = 0 try: while (p.isConnected()): time.sleep(1. / 40.) # set to 40 fps # reset output timeout, hack for threads... aOutput.alive = 0 mInput.alive = 0 wInput.alive = 0 # update the wheel feedback hlWheel.setRotation(p, wInput.angle) # this is the main part of the sim p.stepSimulation() for col in colliders: col.simulate(p) if col.enabled: for light in lights: light.doCollisionFilter(p, col) # get mouse events mouseEvents = p.getMouseEvents() for e in mouseEvents: if ((e[0] == 2) and (e[3] == 0) and (e[4] & p.KEY_WAS_TRIGGERED)): mouseX = e[1] mouseY = e[2] rayFrom, rayTo = getRayFromTo(mouseX, mouseY) rayInfo = p.rayTest(rayFrom, rayTo) for l in range(len(rayInfo)): hit = rayInfo[l] objectUid = hit[0] jointUid = hit[1] if (objectUid >= 1): # this is for debug click on an object to get id # oject will change color this has no effect # changing color real time seems to slow print("obj %i joint %i" % (objectUid, jointUid)) p.changeVisualShape(objectUid, jointUid, rgbaColor=colors[currentColor]) currentColor += 1 if (currentColor >= len(colors)): currentColor = 0 except KeyboardInterrupt: print("KeyboardInterrupt has been caught.") finally: endThreads()