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 _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() second_step.pos = np.array([0.30, -0.11, 0.])