Rune Harlyk
55 lines
1.5 KiB
Python
55 lines
1.5 KiB
Python
|
|
import numpy as np
|
|
import copy
|
|
import sys
|
|
|
|
from .GaitGenerator.Bezier import BezierGait
|
|
from .State import BodyState, GaitState
|
|
|
|
sys.path.append("../../..")
|
|
|
|
class Spot:
|
|
def __init__(self, kinematic, camera, imu, hardware_interface, controller_interface):
|
|
self.kinematic = kinematic
|
|
self.camera = camera
|
|
self.imu = imu
|
|
self.hardware_interface = hardware_interface
|
|
self.controller_interface = controller_interface
|
|
|
|
self.body_state = BodyState()
|
|
self.gait_state = GaitState()
|
|
self.gait_controller = BezierGait()
|
|
|
|
self.body_state.worldFeetPositions = copy.deepcopy(self.kinematic.WorldToFoot)
|
|
|
|
def start(self):
|
|
self.controller_interface.start()
|
|
|
|
def stop(self):
|
|
self.controller_interface.stop()
|
|
|
|
def joint_angles(self):
|
|
return self.kinematic.IK(
|
|
self.body_state.rotation,
|
|
self.body_state.position,
|
|
self.body_state.worldFeetPositions,
|
|
)
|
|
|
|
def run(self, dt=.01):
|
|
self.gait_state.update_gait_state(dt)
|
|
# self.gui.UserInput(self.body_state, self.gait_state)
|
|
|
|
self.gait_controller.generate_trajectory(self.body_state, self.gait_state, dt)
|
|
|
|
self.update_environment()
|
|
|
|
def update_environment(self):
|
|
joint_angles = self.kinematic.IK(
|
|
self.body_state.rotation,
|
|
self.body_state.position,
|
|
self.body_state.worldFeetPositions,
|
|
)
|
|
self.hardware_interface.set_actuator_positions(joint_angles.reshape(-1))
|
|
|
|
|