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