-
Notifications
You must be signed in to change notification settings - Fork 0
/
ros_master.py
118 lines (99 loc) · 3.75 KB
/
ros_master.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
import numpy as np
import time
from src.IMU import IMU
from src.Controller import Controller
from JoystickInterface import JoystickInterface
from src.State import State
from pupper.HardwareInterface import HardwareInterface
from pupper.Config import Configuration
from pupper.Kinematics import four_legs_inverse_kinematics
import math
# from calibrate import app
# Create config
config = Configuration()
hardware_interface = HardwareInterface()
def main(use_imu=False):
"""Main program
"""
# 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.")
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)
# exit()
# Wait until the activate button has been pressed
while True:
print("Waiting for L1 to activate robot.")
while True:
# break
command = joystick_interface.get_command(state, True)
# print(command)
joystick_interface.set_color(config.ps4_deactivated_color)
if command.activate_event == 1:
break
time.sleep(0.1)
print("Robot activated.")
joystick_interface.set_color(config.ps4_color)
while True:
now = time.time()
if now - last_loop < config.dt:
continue
last_loop = time.time()
# Parse the udp joystick commands and then update the robot controller's parameters
command = joystick_interface.get_command(state)
if command.activate_event == 1:
print("Deactivating Robot")
break
# Read imu data. Orientation will be None if no data was available
quat_orientation = (
imu.read_orientation() if use_imu else np.array([1, 0, 0, 0])
)
state.quat_orientation = quat_orientation
# Step the controller forward by dt
controller.run(state, command)
# Update the pwm widths going to the servos
angles = []
for leg in state.joint_angles:
for i in leg:
angles.append(int(i/math.pi*180))
# print(angles)
print(angles, state.joint_angles)
pub_joint_states(state.joint_angles)
# print(hardware_interface.set_actuator_postions(state.joint_angles))
# time.sleep(2)
def pub_joint_states(joint_angles):
angles = []
ids = []
for leg_index in range(4):
for axis_index in range(3):
angle = (-joint_angles[axis_index][leg_index]) * \
hardware_interface.servo_params.servo_multipliers[axis_index, leg_index]
angles.append(angle)
joint_msg.position = angles
joint_states.publish(joint_msg)
if __name__ == '__main__':
import rospy
from sensor_msgs.msg import JointState
rospy.init_node('joystick_controller_node')
joint_states = rospy.Publisher('/joint_states', JointState)
joint_msg = JointState()
joint_msg.name = ["shoulder_joint_rf", "elbow_joint_rf", "wrist_joint_rf",
"shoulder_joint_lf", "elbow_joint_lf", "wrist_joint_lf",
"shoulder_joint_rb", "elbow_joint_rb", "wrist_joint_rb",
"shoulder_joint_lb", "elbow_joint_lb", "wrist_joint_lb",
]
main()