def add_stairStep(system, center, stair_r, h, theta_f, theta_b, dh, i, stepNum,
steps):
# Add stair step
width = 1.53 # Step width
height = 0.1 # Step height
# Create upper half of stair step
dir_f = np.array([np.cos(theta_f), 0, np.sin(theta_f)])
pos_f = center + dir_f * stair_r + np.array([0, h + 3 / 4 * height, 0])
dir_b = np.array([np.cos(theta_b), 0, np.sin(theta_b)])
pos_b = center + dir_b * stair_r + np.array([0, h + 3 / 4 * height, 0])
if steps == True:
step_t = MiroAPI.stepShape(pos_f, dir_f, pos_b, dir_b, width,
1 / 4 * height, [0.05, 0.03, 0.03])
system.Add(step_t)
# Create lower half of stair step
dir_f = np.array([np.cos(theta_f), 0, np.sin(theta_f)]) # Direction front
pos_f = center + dir_f * stair_r + np.array(
[0, h, 0]) # Start postiton front of step
dir_b = np.array([np.cos(theta_b), 0, np.sin(theta_b)]) # Direction back
pos_b = center + dir_b * stair_r + np.array(
[0, h, 0]) # Start postiton back of step
if steps == True:
step = MiroAPI.stepShape(pos_f, dir_f, pos_b, dir_b, width,
3 / 4 * height, [1, 1, 1])
system.Add(step)
# Add stair handle
add_stairHandle(system, width, dir_f, pos_f, dir_b, pos_b, dh, i, stepNum)
def stage(system):
theta_f = 0
theta_b = np.pi / 2
pos_f = np.array([-4.3, 0, -8.8])
dir_f = np.array([np.cos(theta_f), 0, np.sin(theta_f)])
pos_b = np.array([-5.3, 0, -7.8])
dir_b = np.array([np.cos(theta_b), 0, np.sin(theta_b)])
step = MiroAPI.stepShape(pos_f, dir_f, pos_b, dir_b, 3, 0.3,
[0.02, 0.02, 0.02])
system.Add(step)