controller.py 3.95 KB
Newer Older
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
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
#!/usr/bin/python

import argparse
import ethernet
import extctrl2014
import extctrl2014_lc
import labcomm2014
import sys


class Joint(extctrl2014_lc.ext2irb_joint_offset):

    def __init__(self,
                 parKp=0.0,
                 parKv=0.0,
                 parKi=0.0,
                 posOffset=0.0,
                 velOffset=0.0,
                 trqFfwOffset=0.0):
        self.parKp = parKp
        self.parKv = parKv
        self.parKi = parKi
        self.posOffset = posOffset
        self.velOffset = velOffset
        self.trqFfwOffset = trqFfwOffset

    def __repr__(self):
        return 'Joint(posOffset=%f posRef=%f)' % (
            self.posOffset)
    
class Arm(extctrl2014_lc.ext2irb_robot_net):

    def __init__(self, joint, mocgendata):
        self.joint = joint
        self.mocgendata = mocgendata

    def __repr__(self):
        return 'Arm(joint=%s mocgendata=%s)' % (self.joint, self.mocgendata)

class Robot(extctrl2014_lc.ext2irb_net):

    def __init__(self, arms):
        self.robot = arms

    def __repr__(self):
        return 'Robot(arms=%s)' % (self.robot)

if __name__ == "__main__":
    optParser = argparse.ArgumentParser(usage='%(prog)s [options]')
    optParser.add_argument('--listen',
                           type=ethernet.parse_address,
                           action='store',
                           metavar='MAC',
                           required=True,
                           help='ethernet MAC address to listen for')
    optParser.add_argument('--channel',
                           type=lambda s: int(s, 0),
                           action='store',
                           metavar='CHANNEL',
                           required=True,
                           help='CHANNEL to listen for')
    optParser.add_argument('--interface',
                           action='store',
                           metavar='INTERFACE',
                           required=True,
                           help='INTERFACE to use for connection')
    optParser.add_argument('--send-loss',
                           type=float,
                           action='store',
                           metavar='FRACTION',
                           help='FRACTION of send packets to lose [0..1]')
    optParser.add_argument('--recv-loss',
                           type=float,
                           action='store',
                           metavar='FRACTION',
                           help='FRACTION of recv packets to lose [0..1]')
    optParser.add_argument('-v', '--verbose',
                           action='store_true',
                           help='be verbose')

    options = optParser.parse_args(sys.argv[1:])

    eth = ethernet.ETH(options.interface,
                       send_loss=options.send_loss,
                       recv_loss=options.recv_loss)
    extctrl = extctrl2014.ExtCtrl(ethernet=eth,
                                  channel=options.channel,
                                  robot=options.listen)

    encoder = labcomm2014.Encoder(extctrl.writer())
    decoder = labcomm2014.Decoder(extctrl.reader())
    print encoder, decoder
    decoder.add_decl(extctrl2014_lc.irb2ext_net.signature)
    decoder.add_decl(extctrl2014_lc.force_torque_net.signature)
    encoder.add_decl(extctrl2014_lc.ext2irb_net.signature)

    delta = 0.1
    offset = 0.0
    while True:
        value,decl = decoder.decode()
        if value != None and decl == extctrl2014_lc.irb2ext_net.signature:
            offset += delta
            if offset > 1.0:
                delta = -delta
                offset += delta
            elif offset < -1.0:
                delta = -delta
                offset += delta
            print "ROBOT", value
            feedback = Robot([ Arm(joint=[ Joint(posOffset=offset),
                                           Joint(posOffset=offset*2) ],
                        mocgendata=[])])
            encoder.encode(feedback,
                           extctrl2014_lc.ext2irb_net.signature)

    time.sleep(10)