time.sleep(0.5)
x = 90
y = 100
z = 300
r = 0
p = 0
ya = 0
# print('::get_angles()')
# print('==> degrees: {}\n'.format(mycobot.get_angles()))
# time.sleep(0.5)
# print('::get_angles_of_radian()')
# print('==> radians: {}\n'.format(mycobot.get_angles_of_radian()))
# time.sleep(0.5)
while 1:
state = spacenavigator.read()
x += state.x * 15
y += state.y * 15
z += state.z * 15
r += state.roll * 15
p += state.pitch * 15
ya += state.yaw * 15
print(x, y, z, r, p, ya)
data = {}
data["Position"] = [
state.x, state.y, state.z, state.roll, state.pitch, state.yaw
]
mycobot.send_coords([x, y, z, r, p, ya], 70, 0)
print("next step")
# mycobot.send_angles([state.x*160,state.y*50,state.z*90,state.roll*90,state.pitch*90,state.yaw*90], 80)
# print(state.x*160, state.y*50, state.z*90,state.roll*90,state.pitch*90,state.yaw*90)
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()