kinova-api-python/monitorforces.py at master · MobileRobots/kinova-api-python · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
#!/usr/bin/python

import kinovapy
import sys
import time

if kinovapy.InitAPI() != kinovapy.NO_ERROR_KINOVA:
  print 'Error initializing Kinova API'
  sys.exit(1)

#deviceCount = kinovapy.GetNumDevices()
#if deviceCount < 0:
#  print 'Error getting device count'
#  sys.exit(2)
#print 'Found %d devices' % deviceCount
#print 'Homing each arm...'
#for i in xrange(0, deviceCount):
#	kinovapy.SetActiveDeviceNum(0)
#	kinovapy.MoveHome()
#	kinovapy.InitFingers()

cmd = kinovapy.TrajectoryPoint()
cmd.InitStruct
cmd.Position.Type = kinovapy.CARTESIAN_POSITION
cmd.Position.CartesianPosition.X = 0.50
cmd.Position.CartesianPosition.Y = 0.50

#print 'Moving each arm 50 cm out (X) and 50 cm forward (Y)...'
#for i in xrange(0, deviceCount):
	#kinovapy.SetActiveDeviceNum(i)

i = 1
kinovapy.SetActiveDeviceNum(1)
kinovapy.SendAdvanceTrajectory(cmd)


while True:
	#for i in xrange(0, deviceCount):
	#  kinovapy.SetActiveDeviceNum(i)
	  clientconfig = kinovapy.ClientConfigurations()
	  kinovapy.GetClientConfigurations(clientconfig)
	  forceinfo = kinovapy.ForcesInfo()
	  kinovapy.GetForcesInfo(forceinfo)

          print 'arm #%d' % (i),
	  if clientconfig.Laterality == kinovapy.LEFTHAND:
	    print ' (left): ',
	  else:
	    print ' (right): ',
	  print forceinfo
	  time.sleep(1.0)


kinovapy.CloseAPI()