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)