def position_delta(self, leg_index, state, command, first_cycle, move_sideways, move_left): z = state.foot_locations[2, leg_index] step_dist_x = command.velocity[0] *\ (float(self.phase_length)/self.swing_ticks) if first_cycle: shift_factor = 1 else: shift_factor = 2 side_vel = 0.0 if move_sideways: if move_left: side_vel = -(self.body_shift_y * shift_factor) / ( float(self.time_step) * self.stance_ticks) else: side_vel = (self.body_shift_y * shift_factor) / ( float(self.time_step) * self.stance_ticks) velocity = np.array([ -(step_dist_x / 3) / (float(self.time_step) * self.stance_ticks), side_vel, 1.0 / self.z_error_constant * (state.robot_height - z) ]) delta_pos = velocity * self.time_step delta_ori = rotz(-command.yaw_rate * self.time_step) return (delta_pos, delta_ori)
def raibert_touchdown_location(self, leg_index, command): delta_pos_2d = command.velocity * self.phase_length * self.time_step delta_pos = np.array([delta_pos_2d[0], delta_pos_2d[1], 0]) theta = self.stance_ticks * self.time_step * command.yaw_rate rotation = rotz(theta) return np.matmul(rotation, self.default_stance[:, leg_index]) + delta_pos
def raibert_touchdown_location(self, leg_index, command, shifted_left): delta_pos_2d = command.velocity * self.phase_length * self.time_step delta_pos = np.array([delta_pos_2d[0], delta_pos_2d[1], 0]) theta = self.stance_ticks * self.time_step * command.yaw_rate rotation = rotz(theta) shift_correction = np.array([0., 0., 0.]) if shifted_left: shift_correction[1] = -self.body_shift_y else: shift_correction[1] = self.body_shift_y return np.matmul( rotation, self.default_stance[:, leg_index]) + delta_pos + shift_correction
def position_delta(self, leg_index, state, command): z = state.foot_locations[2, leg_index] step_dist_x = command.velocity[0] *\ (float(self.phase_length)/self.swing_ticks) step_dist_y = command.velocity[1] *\ (float(self.phase_length)/self.swing_ticks) velocity = np.array([ -(step_dist_x / 4) / (float(self.time_step) * self.stance_ticks), -(step_dist_y / 4) / (float(self.time_step) * self.stance_ticks), 1.0 / self.z_error_constant * (state.robot_height - z) ]) delta_pos = velocity * self.time_step delta_ori = rotz(-command.yaw_rate * self.time_step) return (delta_pos, delta_ori)