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