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))
示例#4
0
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])
示例#5
0
## 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.])