diff --git a/zonopyrobots/joint_reachable_set/jrs_trig/.gitignore b/zonopyrobots/joint_reachable_set/jrs_trig/.gitignore new file mode 100644 index 00000000..d408d4ab --- /dev/null +++ b/zonopyrobots/joint_reachable_set/jrs_trig/.gitignore @@ -0,0 +1,4 @@ +*.mat +jrs_trig_mat_saved*/ +jrs_trig_tensor_saved*/ +gen_jrs_trig/jrs_trig_dynamics/ \ No newline at end of file diff --git a/zonopyrobots/joint_reachable_set/jrs_trig/gen_jrs_trig/create_jrs_trig_tensor.m b/zonopyrobots/joint_reachable_set/jrs_trig/gen_jrs_trig/create_jrs_trig_tensor.m index 20ed083f..0481516e 100644 --- a/zonopyrobots/joint_reachable_set/jrs_trig/gen_jrs_trig/create_jrs_trig_tensor.m +++ b/zonopyrobots/joint_reachable_set/jrs_trig/gen_jrs_trig/create_jrs_trig_tensor.m @@ -15,6 +15,7 @@ t_plan = 0.5; t_total = 1; dt = 0.01; +d_kai = pi/24; % parameter range for acceleration % generates dynamics parameterized by K [dyn_zero_to_t_plan, dyn_t_plan_to_t_total] = ... @@ -43,6 +44,9 @@ % save vector of initial velocity subinterval centers save(fullfile(save_path,'c_kvi.mat'), 'c_kvi'); +% save the parameter range +save(fullfile(save_path,'d_kai.mat'), 'd_kai'); + % set options for reachability analysis: options.timeStep = dt; options.taylorTerms=5; % number of taylor terms for reachable sets @@ -63,9 +67,6 @@ for j = 1:n_JRS % break JRS computation into two steps... tic; - %delta_kai = max(pi/24, (abs(c_kvi(j))+delta_kvi)/3); - %delta_kai = pi/24; % EXCLUDE heuristic - delta_kai = pi/24; % first, use dyn_zero_to_t_plan dynamics params.tStart = 0; % start time params.tFinal = t_plan; % end time for these dynamics @@ -74,7 +75,7 @@ % this is described by eqs. (3) and (8) params.x0 = [1; 0; c_kvi(j); c_kai; c_kvi(j); 0]; % start at q_i = 0, so cos(q_i) = 1 and sin(q_i) = 0 % use two generators, one for K^a_i and one for K^v_i (eq. 8) - params.R0 = zonotope([params.x0, [0; 0; 0; delta_kai; 0; 0], [0; 0; delta_kvi; 0; delta_kvi+epsilon; 0]]); + params.R0 = zonotope([params.x0, [0; 0; 0; d_kai; 0; 0], [0; 0; delta_kvi; 0; delta_kvi+epsilon; 0]]); % create system for reachability analysis (1 dummy input) % CORA 2018: diff --git a/zonopyrobots/joint_reachable_set/jrs_trig/gen_jrs_trig/jrs_trig_dynamics/trig_dyn_t_plan_to_t_total.m b/zonopyrobots/joint_reachable_set/jrs_trig/gen_jrs_trig/jrs_trig_dynamics/trig_dyn_t_plan_to_t_total.m deleted file mode 100644 index 9163c40b..00000000 --- a/zonopyrobots/joint_reachable_set/jrs_trig/gen_jrs_trig/jrs_trig_dynamics/trig_dyn_t_plan_to_t_total.m +++ /dev/null @@ -1,13 +0,0 @@ -function dx = trig_dyn_t_plan_to_t_total(in1,udummy) -%TRIG_DYN_T_PLAN_TO_T_TOTAL -% DX = TRIG_DYN_T_PLAN_TO_T_TOTAL(IN1,UDUMMY) - -% This function was generated by the Symbolic Math Toolbox version 9.0. -% 05-Jun-2022 22:28:47 - -cqi = in1(1,:); -dqi = in1(3,:); -kai = in1(4,:); -kvi = in1(5,:); -sqi = in1(2,:); -dx = [-dqi.*sqi;cqi.*dqi;-kai-kvi.*2.0;0.0;0.0;1.0]; diff --git a/zonopyrobots/joint_reachable_set/jrs_trig/gen_jrs_trig/jrs_trig_dynamics/trig_dyn_zero_to_t_plan.m b/zonopyrobots/joint_reachable_set/jrs_trig/gen_jrs_trig/jrs_trig_dynamics/trig_dyn_zero_to_t_plan.m deleted file mode 100644 index f37a73a5..00000000 --- a/zonopyrobots/joint_reachable_set/jrs_trig/gen_jrs_trig/jrs_trig_dynamics/trig_dyn_zero_to_t_plan.m +++ /dev/null @@ -1,12 +0,0 @@ -function dx = trig_dyn_zero_to_t_plan(in1,udummy) -%TRIG_DYN_ZERO_TO_T_PLAN -% DX = TRIG_DYN_ZERO_TO_T_PLAN(IN1,UDUMMY) - -% This function was generated by the Symbolic Math Toolbox version 9.0. -% 05-Jun-2022 22:28:47 - -cqi = in1(1,:); -dqi = in1(3,:); -kai = in1(4,:); -sqi = in1(2,:); -dx = [-dqi.*sqi;cqi.*dqi;kai;0.0;0.0;1.0]; diff --git a/zonopyrobots/joint_reachable_set/jrs_trig/jrs_trig_mat_saved/d_kai.mat b/zonopyrobots/joint_reachable_set/jrs_trig/jrs_trig_mat_saved/d_kai.mat new file mode 100644 index 00000000..aaaa3231 Binary files /dev/null and b/zonopyrobots/joint_reachable_set/jrs_trig/jrs_trig_mat_saved/d_kai.mat differ diff --git a/zonopyrobots/joint_reachable_set/jrs_trig/jrs_trig_tensor_saved/d_kai.mat b/zonopyrobots/joint_reachable_set/jrs_trig/jrs_trig_tensor_saved/d_kai.mat new file mode 100644 index 00000000..aaaa3231 Binary files /dev/null and b/zonopyrobots/joint_reachable_set/jrs_trig/jrs_trig_tensor_saved/d_kai.mat differ diff --git a/zonopyrobots/joint_reachable_set/offline_jrs.py b/zonopyrobots/joint_reachable_set/offline_jrs.py index 05afae2a..ecc17117 100644 --- a/zonopyrobots/joint_reachable_set/offline_jrs.py +++ b/zonopyrobots/joint_reachable_set/offline_jrs.py @@ -11,12 +11,27 @@ Array = Union[Tensor, ndarray] class OfflineJRS: + """ Wrapper for preloading and processing ARMTD style JRS tensors generated offline + + These tensors are generated offline using the MATLAB scripts in the jrs_trig/gen_jrs_trig folder. + This provides a wrapper for some of the jrs_trig.load_jrs_trig and jrs_trig.process_jrs_trig functions + to make it easier to use the JRS tensors. The JRS tensors are preloaded and processed in the __init__ function + and then the __call__ function can be used to get the JRS and the corresponding rotatotopes for a given configuration + and velocity. + + This specifically loads the tensors from the jrs_trig/jrs_trig_tensor_saved folder + """ def __init__( self, device: torch.device = 'cpu', dtype: torch.dtype = torch.float, ): - """ Wrapper for preloading and processing JRS tensors """ + """ Wrapper for preloading and processing JRS tensors + + Args: + device (torch.device, optional): The device to use for the JRS tensors. Defaults to 'cpu'. + dtype (torch.dtype, optional): The dtype to use for the JRS tensors. Defaults to torch.float. + """ from .jrs_trig.load_jrs_trig import g_ka self.jrs_tensor = _preload_batch_JRS_trig(device=device, dtype=dtype) self.g_ka = g_ka