def step_controller(controller, robot_config):
"""Steps the controller forward one timestep
Parameters
----------
controller : Controller
Robot controller object.
"""
controller.foot_locations = step(
controller.ticks,
controller.foot_locations,
controller.swing_params,
controller.stance_params,
controller.gait_params,
controller.movement_reference,
)
# Apply the desired body rotation
# TODO: See https://github.com/Nate711/PupperPythonSim/issues/2
rotated_foot_locations = (euler2mat(controller.movement_reference.roll,
controller.movement_reference.pitch,
0.0) @ controller.foot_locations)
controller.joint_angles = four_legs_inverse_kinematics(
rotated_foot_locations, robot_config)
controller.ticks += 1
def __init__(self, robot_config):
self.swing_params = SwingParams()
self.stance_params = StanceParams()
self.gait_params = GaitParams()
self.movement_reference = MovementReference()
self.ticks = 0
# Set default for foot locations and joint angles
self.foot_locations = (
self.stance_params.default_stance +
np.array([0, 0, self.movement_reference.z_ref])[:, np.newaxis])
self.joint_angles = four_legs_inverse_kinematics(
self.foot_locations, robot_config)
def __init__(self, robot_config):
self.swing_params = SwingParams()
self.stance_params = StanceParams()
self.gait_params = GaitParams()
self.movement_reference = MovementReference()
self.smoothed_yaw = 0.0 # for REST mode only
self.previous_state = BehaviorState.REST
self.state = BehaviorState.REST
self.ticks = 0
# Set default for foot locations and joint angles
self.foot_locations = (
self.stance_params.default_stance +
np.array([0, 0, self.movement_reference.z_ref])[:, np.newaxis])
self.contact_modes = np.zeros(4)
self.joint_angles = four_legs_inverse_kinematics(
self.foot_locations, robot_config)
def set_pose_to_default(controller, robot_config):
controller.foot_locations = (
controller.stance_params.default_stance +
np.array([0, 0, controller.movement_reference.z_ref])[:, np.newaxis])
controller.joint_angles = four_legs_inverse_kinematics(
controller.foot_locations, robot_config)
def step_controller(controller, robot_config, quat_orientation):
"""Steps the controller forward one timestep
Parameters
----------
controller : Controller
Robot controller object.
"""
if controller.state == BehaviorState.TROT:
controller.foot_locations, controller.contact_modes = step(
controller.ticks,
controller.foot_locations,
controller.swing_params,
controller.stance_params,
controller.gait_params,
controller.movement_reference,
)
# Apply the desired body rotation
# foot_locations = (
# euler2mat(
# controller.movement_reference.roll, controller.movement_reference.pitch, 0.0
# )
# @ controller.foot_locations
# )
# Disable joystick-based pitch and roll for trotting with IMU feedback
foot_locations = controller.foot_locations
# Construct foot rotation matrix to compensate for body tilt
(roll, pitch, yaw) = quat2euler(quat_orientation)
correction_factor = 0.8
max_tilt = 0.4
roll_compensation = correction_factor * np.clip(
roll, -max_tilt, max_tilt)
pitch_compensation = correction_factor * np.clip(
pitch, -max_tilt, max_tilt)
rmat = euler2mat(roll_compensation, pitch_compensation, 0)
foot_locations = rmat.T @ foot_locations
controller.joint_angles = four_legs_inverse_kinematics(
foot_locations, robot_config)
elif controller.state == BehaviorState.HOP:
hop_foot_locations = (controller.stance_params.default_stance +
np.array([0, 0, -0.09])[:, np.newaxis])
controller.joint_angles = four_legs_inverse_kinematics(
hop_foot_locations, robot_config)
elif controller.state == BehaviorState.FINISHHOP:
hop_foot_locations = (controller.stance_params.default_stance +
np.array([0, 0, -.22])[:, np.newaxis])
controller.joint_angles = four_legs_inverse_kinematics(
hop_foot_locations, robot_config)
elif controller.state == BehaviorState.REST:
if controller.previous_state != BehaviorState.REST:
controller.smoothed_yaw = 0
yaw_factor = -0.25
controller.smoothed_yaw += controller.gait_params.dt * clipped_first_order_filter(
controller.smoothed_yaw,
controller.movement_reference.wz_ref * yaw_factor, 1.5, 0.25)
# Set the foot locations to the default stance plus the standard height
controller.foot_locations = (
controller.stance_params.default_stance + np.array(
[0, 0, controller.movement_reference.z_ref])[:, np.newaxis])
# Apply the desired body rotation
rotated_foot_locations = (
euler2mat(controller.movement_reference.roll,
controller.movement_reference.pitch,
controller.smoothed_yaw) @ controller.foot_locations)
controller.joint_angles = four_legs_inverse_kinematics(
rotated_foot_locations, robot_config)
controller.ticks += 1
controller.previous_state = controller.state