def create_curves(lfoot_ini_iso, lfoot_mid_iso, lfoot_fin_iso, lfoot_mid_vel,
rfoot_ini_iso, rfoot_mid_iso, rfoot_fin_iso, rfoot_mid_vel,
base_ini_iso, base_fin_iso):
lfoot_pos_curve_ini_to_mid = interpolation.HermiteCurveVec(
lfoot_ini_iso[0:3, 3], np.zeros(3), lfoot_mid_iso[0:3, 3],
lfoot_mid_vel)
lfoot_pos_curve_mid_to_fin = interpolation.HermiteCurveVec(
lfoot_mid_iso[0:3, 3], lfoot_mid_vel, lfoot_fin_iso[0:3, 3],
np.zeros(3))
lfoot_quat_curve = interpolation.HermiteCurveQuat(
util.rot_to_quat(lfoot_ini_iso[0:3, 0:3]), np.zeros(3),
util.rot_to_quat(lfoot_fin_iso[0:3, 0:3]), np.zeros(3))
rfoot_pos_curve_ini_to_mid = interpolation.HermiteCurveVec(
rfoot_ini_iso[0:3, 3], np.zeros(3), rfoot_mid_iso[0:3, 3],
rfoot_mid_vel)
rfoot_pos_curve_mid_to_fin = interpolation.HermiteCurveVec(
rfoot_mid_iso[0:3, 3], rfoot_mid_vel, rfoot_fin_iso[0:3, 3],
np.zeros(3))
rfoot_quat_curve = interpolation.HermiteCurveQuat(
util.rot_to_quat(rfoot_ini_iso[0:3, 0:3]), np.zeros(3),
util.rot_to_quat(rfoot_fin_iso[0:3, 0:3]), np.zeros(3))
base_pos_curve = interpolation.HermiteCurveVec(base_ini_iso[0:3, 3],
np.zeros(3),
base_fin_iso[0:3, 3],
np.zeros(3))
base_quat_curve = interpolation.HermiteCurveQuat(
util.rot_to_quat(base_ini_iso[0:3, 0:3]), np.zeros(3),
util.rot_to_quat(base_fin_iso[0:3, 0:3]), np.zeros(3))
return lfoot_pos_curve_ini_to_mid, lfoot_pos_curve_mid_to_fin, lfoot_quat_curve, rfoot_pos_curve_ini_to_mid, rfoot_pos_curve_mid_to_fin, rfoot_quat_curve, base_pos_curve, base_quat_curve
def initialize_swing_foot_trajectory(self, start_time, swing_duration,
landing_foot):
self._swing_start_time = start_time
self._swing_duration = swing_duration
self._swing_land_foot = copy.deepcopy(landing_foot)
self._swing_init_foot.iso = np.copy(
self._robot.get_link_iso(self._target_id))
self._swing_init_foot.side = landing_foot.side
self._swing_mid_foot = interpolate(self._swing_init_foot, landing_foot,
0.5)
# compute midfoot boundary conditions
mid_swing_local_foot_pos = np.array([0., 0., self._swing_height])
mid_swing_pos = self._swing_mid_foot.pos + np.dot(
self._swing_mid_foot.rot, mid_swing_local_foot_pos)
mid_swing_vel = (self._swing_land_foot.pos -
self._swing_init_foot.pos) / self._swing_duration
# construct trajectories
self._pos_traj_init_to_mid = interpolation.HermiteCurveVec(
self._swing_init_foot.pos, np.zeros(3), mid_swing_pos,
mid_swing_vel)
self._pos_traj_mid_to_end = interpolation.HermiteCurveVec(
mid_swing_pos, mid_swing_vel, self._swing_land_foot.pos,
np.zeros(3))
self._quat_hermite_curve = interpolation.HermiteCurveQuat(
self._swing_init_foot.quat, np.zeros(3),
self._swing_land_foot.quat, np.zeros(3))
def _compute_reference_base_ori_trajectory(self):
self._base_quat_curves = []
prev_lf_stance = self._ini_lf_stance
prev_rf_stance = self._ini_rf_stance
curr_base_quat = self._ini_quat
step_counter = 0
for i in range(len(self._vrp_type_list)):
# swing state
if self._vrp_type_list[
i] == VRPType.RF_SWING or self._vrp_type_list[
i] == VRPType.LF_SWING:
target_step = self._footstep_list[step_counter]
if target_step.side == Footstep.LEFT_SIDE:
stance_step = prev_rf_stance
prev_lf_stance = target_step
else:
stance_step = prev_lf_stance
prev_rf_stance = target_step
# Find the midefeet
mid_foot_stance = interpolate(stance_step, target_step, 0.5)
# Create the hermite quaternion curve object
self._base_quat_curves.append(
interpolation.HermiteCurveQuat(curr_base_quat, np.zeros(3),
mid_foot_stance.quat,
np.zeros(3)))
# Update the base orientation
curr_base_quat = np.copy(mid_foot_stance.quat)
step_counter += 1
else:
# TODO : Initiate angular velocity with curr angular velocity
mid_foot_stance = interpolate(prev_lf_stance, prev_rf_stance,
0.5)
curr_base_quat = mid_foot_stance.quat
self._base_quat_curves.append(
interpolation.HermiteCurveQuat(curr_base_quat,
np.zeros(3), curr_base_quat,
np.zeros(3)))
def initialize_hand_trajectory(self, start_time, duration,
target_hand_iso):
self._start_moving_time = start_time
self._moving_duration = duration
init_hand_iso = self._robot.get_link_iso(self._target_id)
init_hand_vel = self._robot.get_link_vel(self._target_id)
init_hand_quat = util.rot_to_quat(init_hand_iso[0:3, 0:3])
target_hand_quat = util.rot_to_quat(target_hand_iso[0:3, 0:3])
# self._pos_hermite_curve = interpolation.HermiteCurveVec(
# init_hand_iso[0:3, 3], np.zeros(3), target_hand_iso[0:3, 3],
# np.zeros(3))
self._init_hand_pos = init_hand_iso[0:3, 3]
self._target_hand_pos = target_hand_iso[0:3, 3]
self._quat_hermite_curve = interpolation.HermiteCurveQuat(
init_hand_quat, init_hand_vel[0:3], target_hand_quat, np.zeros(3))