def _populate_strafe(self, num_times, strafe_distance):
self._update_starting_stance()
lf_stance = Footstep()
rf_stance = Footstep()
mf_stance = copy.deepcopy(self._mf_stance)
mf_stance_translated = copy.deepcopy(self._mf_stance)
for i in range(num_times):
mf_stance_translated.pos = mf_stance.pos + np.dot(
mf_stance.rot, np.array([0., strafe_distance, 0.]))
mf_stance_translated.rot = mf_stance.rot
lf_stance.pos = mf_stance_translated.pos + np.dot(
mf_stance_translated.rot,
np.array([0., self._nominal_footwidth / 2., 0.]))
lf_stance.rot = mf_stance_translated.rot
lf_stance.side = Footstep.LEFT_SIDE
rf_stance.pos = mf_stance_translated.pos + np.dot(
mf_stance_translated.rot,
np.array([0., -self._nominal_footwidth / 2., 0.]))
rf_stance.rot = mf_stance_translated.rot
rf_stance.side = Footstep.RIGHT_SIDE
if strafe_distance > 0:
self._footstep_list.append(copy.deepcopy(lf_stance))
self._footstep_list.append(copy.deepcopy(rf_stance))
else:
self._footstep_list.append(copy.deepcopy(rf_stance))
self._footstep_list.append(copy.deepcopy(lf_stance))
mf_stance = copy.deepcopy(mf_stance_translated)
def _populate_turn(self, num_times, turn_radians_per_step):
self._update_starting_stance()
foot_rotation = R.from_rotvec([0., 0.,
turn_radians_per_step]).as_matrix()
lf_stance = Footstep()
rf_stance = Footstep()
mf_stance = copy.deepcopy(self._mf_stance)
mf_stance_rotated = copy.deepcopy(self._mf_stance)
for i in range(num_times):
mf_stance_rotated.pos = np.copy(mf_stance.pos)
mf_stance_rotated.rot = np.dot(foot_rotation, mf_stance.rot)
lf_stance.pos = mf_stance_rotated.pos + np.dot(
mf_stance_rotated.rot,
np.array([0., self._nominal_footwidth / 2., 0.]))
lf_stance.rot = np.copy(mf_stance_rotated.rot)
lf_stance.side = Footstep.LEFT_SIDE
rf_stance.pos = mf_stance_rotated.pos + np.dot(
mf_stance_rotated.rot,
np.array([0., -self._nominal_footwidth / 2., 0.]))
rf_stance.rot = np.copy(mf_stance_rotated.rot)
rf_stance.side = Footstep.RIGHT_SIDE
if turn_radians_per_step > 0.:
self._footstep_list.append(copy.deepcopy(lf_stance))
self._footstep_list.append(copy.deepcopy(rf_stance))
else:
self._footstep_list.append(copy.deepcopy(rf_stance))
self._footstep_list.append(copy.deepcopy(lf_stance))
mf_stance = copy.deepcopy(mf_stance_rotated)
def __init__(self, pos_task, robot):
self._pos_task = pos_task
self._robot = robot
self._swing_start_time = 0.
self._swing_duration = 0.
self._swing_init_foot = Footstep()
self._swing_mid_foot = Footstep()
self._swing_land_foot = Footstep()
self._pos_traj_init_to_mid = None
self._pos_traj_mid_to_end = None
self._target_id = self._pos_task.target_id
# Attribute
self._swing_height = 0.05
def __init__(self, dcm_planner, com_task, base_ori_task, robot, lfoot_id,
rfoot_id):
self._dcm_planner = dcm_planner
self._com_task = com_task
self._base_ori_task = base_ori_task
self._robot = robot
self._lfoot_id = lfoot_id
self._rfoot_id = rfoot_id
self._des_com_pos = np.zeros(3)
self._des_com_vel = np.zeros(3)
self._des_com_acc = np.zeros(3)
self._des_quat = np.array([0., 0., 0., 1.])
self._des_ang_vel = np.zeros(3)
self._des_ang_acc = np.zeros(3)
self._reset_step_idx()
self._robot_side = Footstep.RIGHT_SIDE
self._footstep_list = []
self._footstep_preview_list = []
self._lf_stance = Footstep()
self._rf_stance = Footstep()
self._mf_stance = Footstep()
# Attributes
self._nominal_com_height = 1.015
self._t_additional_init_transfer = 0.
self._t_contact_transition = 0.45
self._t_swing = 1.
self._percentage_settle = 0.99
self._alpha_ds = 0.5
self._nominal_footwidth = 0.27
self._nominal_forward_step = 0.25
self._nominal_backward_step = -0.25
self._nominal_turn_radians = np.pi / 4.
self._nominal_strafe_distance = 0.125
self._set_temporal_params()
def __init__(self):
self._ini_lf_stance = Footstep()
self._ini_rf_stance = Footstep()
self._ini_dcm_pos = np.zeros(3)
self._ini_dcm_vel = np.zeros(3)
self._vrp_list = [] # vrp
self._vrp_type_list = []
# Attributes
self._t_transfer = 0.1
self._t_ds = 0.05
self._t_ss = 0.3
self._percentage_settle = 0.99
self._alpha_ds = 0.5
self._t_start = 0.
self._t_end = 0.
self._dt = 1e-3
self._z_vrp = 0.75 # com height
self._b = np.sqrt(self._z_vrp / 9.81)
self._robot_mass = 50
self._ini_quat = np.array([0., 0., 0., 1])
def _populate_walk_forward(self, num_steps, forward_distance):
self._update_starting_stance()
new_stance = Footstep()
mf_stance = copy.deepcopy(self._mf_stance)
robot_side = Footstep.LEFT_SIDE
for i in range(num_steps):
if robot_side == Footstep.LEFT_SIDE:
translate = np.array([(i + 1) * forward_distance,
self._nominal_footwidth / 2., 0.])
new_stance.pos = mf_stance.pos + np.dot(
mf_stance.rot, translate)
new_stance.rot = np.copy(mf_stance.rot)
new_stance.side = Footstep.LEFT_SIDE
robot_side = Footstep.RIGHT_SIDE
else:
translate = np.array([(i + 1) * forward_distance,
-self._nominal_footwidth / 2., 0.])
new_stance.pos = mf_stance.pos + np.dot(
mf_stance.rot, translate)
new_stance.rot = np.copy(mf_stance.rot)
new_stance.side = Footstep.RIGHT_SIDE
robot_side = Footstep.LEFT_SIDE
self._footstep_list.append(copy.deepcopy(new_stance))
# Add additional step forward to square the feet
if robot_side == Footstep.LEFT_SIDE:
translate = np.array([
num_steps * forward_distance, self._nominal_footwidth / 2., 0.
])
new_stance.pos = mf_stance.pos + np.dot(mf_stance.rot, translate)
new_stance.rot = mf_stance.rot
new_stance.side = Footstep.LEFT_SIDE
else:
translate = np.array([
num_steps * forward_distance, -self._nominal_footwidth / 2., 0.
])
new_stance.pos = mf_stance.pos + np.dot(mf_stance.rot, translate)
new_stance.rot = mf_stance.rot
new_stance.side = Footstep.RIGHT_SIDE
self._footstep_list.append(copy.deepcopy(new_stance))
import os import sys cwd = os.getcwd() sys.path.append(cwd) import numpy as np from pnc.planner.locomotion.dcm_planner.footstep import Footstep from pnc.planner.locomotion.dcm_planner.dcm_planner import DCMPlanner step_one = Footstep() step_one.pos = np.array([0.274672, 0.134096, -4.51381e-05]) step_one.quat = np.array([1.89965e-05, 9.56242e-05, -0.000503418, 1]) step_one.side = Footstep.LEFT_SIDE step_two = Footstep() step_two.pos = np.array([0.5244, -0.136156, -0.000103187]) step_two.quat = np.array([1.89965e-05, 9.56242e-05, -0.000503418, 1]) step_two.side = Footstep.RIGHT_SIDE step_third = Footstep() step_third.pos = np.array([0.524672, 0.133844, -9.2955e-05]) step_third.quat = np.array([1.89965e-05, 9.56242e-05, -0.000503418, 1]) step_third.side = Footstep.LEFT_SIDE lf_stance = Footstep() lf_stance.pos = np.array([0.0242475, 0.133069, 5.2579e-09]) lf_stance.quat = np.array([-5.63289e-08, 7.08106e-06, 0.00179564, 0.999998]) lf_stance.side = Footstep.LEFT_SIDE rf_stance = Footstep() rf_stance.pos = np.array([0.0248257, -0.134374, -4.11672e-06])
## ============================================================================= ## Main ## ============================================================================= initial_dcm = np.array([0., 0., 0.765]) dcm_planner = DCMPlanner() dcm_planner.t_transfer = 0. dcm_planner.t_ds = 0.45 dcm_planner.t_ss = 0.75 dcm_planner.percentage_settle = 0.9 dcm_planner.alpha_ds = 0.5 dcm_planner.robot_mass = 100. dcm_planner.z_vrp = 0.765 lf_stance = Footstep() lf_stance.pos = np.array([0., 0.11, 0.]) lf_stance.quat = np.array([0., 0., 0., 1.]) lf_stance.side = Footstep.LEFT_SIDE rf_stance = Footstep() rf_stance.pos = np.array([0., -0.11, 0.]) rf_stance.quat = np.array([0., 0., 0., 1.]) rf_stance.side = Footstep.RIGHT_SIDE first_step = Footstep() first_step.pos = np.array([0.15, 0.11, 0.]) first_step.quat = np.array([0., 0., 0., 1.]) first_step.side = Footstep.LEFT_SIDE second_step = Footstep()