-
Notifications
You must be signed in to change notification settings - Fork 3
/
quadruped_controller.py
133 lines (100 loc) · 3.96 KB
/
quadruped_controller.py
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
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
import numpy as np
import time
import py_dynamixel.io as io
from sin_controller import SinusoidController
class Quadruped():
def __init__(self, port, ctrl_freq):
self.port = port
self.dxl_io = io.DxlIO(port, baudrate=3000000, use_sync_write=True, use_sync_read=True)
print('Connected!')
self.ctrl_freq = ctrl_freq
self.ids = list()
for i in range(10, 50, 10):
for j in range(1, 4):
self.ids.append(i + j)
print("ids", self.ids)
# this is always the trajectory that will be executed
# it is a list of np.arrays the joint angles of every motor
self._traj = []
self.enable_torques()
self.dxl_io.init_sync_read(self.ids)
def enable_torques(self):
self.dxl_io.enable_torque(self.ids)
def disable_torques(self):
self.dxl_io.disable_torque(self.ids)
def shutdown(self):
self.relax()
self.dxl_io.disable_torque(self.ids)
self.dxl_io.close_port()
def relax(self):
"Places hexapod on its belly"
print("#### RELAX POSE CONTROLLER ####")
duration = 2.0
for t in np.arange(0,duration,1.0/self.ctrl_freq):
command = np.array([np.pi, np.pi, np.pi]*6)
# second joint values have to follow a trajectory
a = (duration-t)/duration
b = t/duration
for i in range(1,18,3):
command[i] += (a*(np.pi/4) / 6.0 + b*((np.pi/2)*1.2))
self._traj.append(command)
self._exec_traj()
def neutral_controller(self):
"neutral pose - robot standing up with legs straight"
print("#### NEUTRAL POSE CONTROLLER ####")
duration = 0.1
for t in np.arange(0,duration,1.0/self.ctrl_freq):
command = np.array([np.pi, np.pi, np.pi]*6)
self._traj.append(command)
self._exec_traj()
time.sleep(0.5)
def run_sin_controller(self, ctrl, duration):
controller = SinusoidController(ctrl)
for t in np.arange(0,duration,1.0/self.ctrl_freq):
command = controller.commanded_jointpos(t)
command = command+np.pi # offset differnce from simulator to real world configuration
self._traj.append(command)
self._exec_traj()
time.sleep(0.5)
def _exec_traj(self):
"execeute trajectories that are saved in _traj"
start = time.time()
for i in range(len(self._traj)):
# get current state
cur_jpos = self.dxl_io.get_present_position(self.ids)
cur_jvel = self.dxl_io.get_present_velocity(self.ids)
#print(cur_jpos, cur_jvel)
# get action
joint_pos = self._traj[i]
self.dxl_io.set_goal_position(self.ids, joint_pos)
elapsed = time.time() - start
time.sleep((1.0/self.ctrl_freq) - elapsed)
if ((1.0/self.ctrl_freq) - elapsed) < 0:
print("Control frequency is too high")
start = time.time()
# reset the traj variable
self._traj = []
# add some functions for conversion to hexapod default angles - i.e neutral position is 180 deg absolute
# especially sin controller commands
def main():
## Example to task Hexapod class ##
ports = io.get_available_ports()
print('available ports:', ports)
if not ports:
raise IOError('No port available.')
port = ports[0]
print('Using the first on the list', port)
ctrl_freq = 20
Hexa = Quadruped(port, ctrl_freq)
# TRIPOD GAIT
ctrl = [1, 0, 0.5, 0.25, 0.25, 0.5,
1, 0.5, 0.5, 0.25, 0.75, 0.5,
1, 0, 0.5, 0.25, 0.25, 0.5,
1, 0, 0.5, 0.25, 0.75, 0.5,]
ctrl = np.array(ctrl)
Hexa.neutral_controller()
#Hexa.relax()
Hexa.run_sin_controller(ctrl, duration=3.0)
Hexa.shutdown()
if __name__ == "__main__":
main()