-
Notifications
You must be signed in to change notification settings - Fork 1
/
jaceProg.py
112 lines (90 loc) · 3.19 KB
/
jaceProg.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
import sys
import jaceCommands as jc
sys.path.append('../StanfordQuadruped')
import numpy as np
import time
import os
from src.IMU import IMU
from src.Controller import Controller
from src.JoystickInterface import JoystickInterface
from src.State import BehaviorState, State
from MangDang.mini_pupper.HardwareInterface import HardwareInterface
from MangDang.mini_pupper.Config import Configuration
from pupper.Kinematics import four_legs_inverse_kinematics, leg_explicit_inverse_kinematics
from MangDang.mini_pupper.display import Display
from src.MovementScheme import MovementScheme
from src.danceSample import MovementLib
from UDPComms import Publisher
def main(use_imu=False):
"""Main program
"""
# Create config
config = Configuration()
hardware_interface = HardwareInterface()
disp = Display()
disp.show_ip()
# Create imu handle
if use_imu:
imu = IMU(port="/dev/ttyACM0")
imu.flush_buffer()
# Create controller and user input handles
controller = Controller(
config,
four_legs_inverse_kinematics,
)
state = State()
print("Creating joystick listener...")
joystick_interface = JoystickInterface(config)
print("Done.")
#Create movement group scheme instance and set a default false state
movementCtl = MovementScheme(MovementLib)
dance_active_state = False
last_loop = time.time()
print("Summary of gait parameters:")
print("overlap time: ", config.overlap_time)
print("swing time: ", config.swing_time)
print("z clearance: ", config.z_clearance)
print("x shift: ", config.x_shift)
store = jc.stand()
state.joint_angles = store
jc.set_servos(hardware_interface, state.joint_angles)
time.sleep(1)
while True:
## Stand
storeDefault = jc.stand()
storeA = jc.stand(5)
storeB = jc.stand(245)
jc.keyframe(1, storeDefault, storeA, hardware_interface)
jc.keyframe(1, storeA, storeB, hardware_interface)
jc.keyframe(1, storeB, storeDefault, hardware_interface)
time.sleep(1)
## Walk forward, right, backward, left
print("forward")
# jc.walk_control(0, 0.75, 4, hardware_interface)
# time.sleep(1)
print("right")
jc.walk_control(0.25, 0.75, 4, hardware_interface)
time.sleep(1)
print("backward")
# jc.walk_control(0.5, 0.75, 4, hardware_interface)
# time.sleep(1)
print("left")
jc.walk_control(0.75, 0.75, 4, hardware_interface)
time.sleep(1)
## Look around
store1 = jc.look_around(127, 1)
store2 = jc.look_around(127, -1)
store3 = jc.look_around(127, 0)
jc.keyframe(0.5, store3, store1, hardware_interface)
jc.keyframe(1, store1, store2, hardware_interface)
jc.keyframe(0.5, store2, store3, hardware_interface)
time.sleep(1)
## Turn a little
jc.turn_around(127, 1, hardware_interface)
time.sleep(0.5)
jc.turn_around(127, -1, hardware_interface)
jc.turn_around(127, -1, hardware_interface)
time.sleep(0.5)
jc.turn_around(127, 1, hardware_interface)
time.sleep(1)
main()