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 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