From 01ada3097f4f4f1472eac54e5cfbc4a1b3a6eb8d Mon Sep 17 00:00:00 2001 From: armantekinalp Date: Fri, 15 Apr 2022 18:04:35 -0500 Subject: [PATCH 1/5] enhancement:initial commit of muscular snake --- .../MuscularSnake/get_connection_vector.py | 54 ++ examples/MuscularSnake/muscle_forces.py | 133 +++++ examples/MuscularSnake/muscular_snake.py | 373 ++++++++++++ examples/MuscularSnake/post_processing.py | 529 ++++++++++++++++++ 4 files changed, 1089 insertions(+) create mode 100644 examples/MuscularSnake/get_connection_vector.py create mode 100644 examples/MuscularSnake/muscle_forces.py create mode 100644 examples/MuscularSnake/muscular_snake.py create mode 100644 examples/MuscularSnake/post_processing.py diff --git a/examples/MuscularSnake/get_connection_vector.py b/examples/MuscularSnake/get_connection_vector.py new file mode 100644 index 00000000..30b85246 --- /dev/null +++ b/examples/MuscularSnake/get_connection_vector.py @@ -0,0 +1,54 @@ +import numpy as np +# Join the two rods +from elastica._linalg import ( + _batch_norm, + _batch_matvec, +) + + + +def get_connection_vector_straight_straight_rod( + rod_one, + rod_two, + rod_one_start_idx, + rod_one_end_idx, +): + + # Compute rod element positions + rod_one_element_position = 0.5 * ( + rod_one.position_collection[..., 1:] + rod_one.position_collection[..., :-1] + ) + rod_one_element_position = rod_one_element_position[:, rod_one_start_idx:rod_one_end_idx] + rod_two_element_position = 0.5 * ( + rod_two.position_collection[..., 1:] + rod_two.position_collection[..., :-1] + ) + + # Lets get the distance between rod elements + distance_vector_rod_one_to_rod_two = ( + rod_two_element_position - rod_one_element_position + ) + distance_vector_rod_one_to_rod_two_norm = _batch_norm( + distance_vector_rod_one_to_rod_two + ) + distance_vector_rod_one_to_rod_two /= distance_vector_rod_one_to_rod_two_norm + + distance_vector_rod_two_to_rod_one = -distance_vector_rod_one_to_rod_two + + rod_one_direction_vec_in_material_frame = _batch_matvec( + rod_one.director_collection[:,:,rod_one_start_idx:rod_one_end_idx], distance_vector_rod_one_to_rod_two + ) + rod_two_direction_vec_in_material_frame = _batch_matvec( + rod_two.director_collection, distance_vector_rod_two_to_rod_one + ) + + offset_btw_rods = distance_vector_rod_one_to_rod_two_norm - ( + rod_one.radius[rod_one_start_idx:rod_one_end_idx] + rod_two.radius + ) + + return ( + rod_one_direction_vec_in_material_frame, + rod_two_direction_vec_in_material_frame, + offset_btw_rods, + ) + + diff --git a/examples/MuscularSnake/muscle_forces.py b/examples/MuscularSnake/muscle_forces.py new file mode 100644 index 00000000..b30c4b6c --- /dev/null +++ b/examples/MuscularSnake/muscle_forces.py @@ -0,0 +1,133 @@ +__doc__ = """ Muscular snake muscle forces NumPy implementation """ +__all__ = ["MuscleForces"] +import numpy as np +import numba +from numba import njit +from elastica import NoForces +from elastica._calculus import difference_kernel + + +class MuscleForces(NoForces): + """ + This class is for computing muscle forces. Detailed formulation is given in Eq. 2 + Zhang et. al. Nature Comm 2019 paper + + Attributes + ---------- + amplitude : float + Amplitude of muscle forces. + wave_number : float + Wave number for muscle actuation. + side_of_body : int + Depending on which side of body, left or right this variable becomes +1 or -1. + This variable determines the sin wave direction. + time_delay : float + Delay time for muscles. + muscle_start_end_index : numpy.ndarray + 1D (2) array containing data with 'int' type. + Element start and end index of muscle forces. + """ + + def __init__( + self, + amplitude, + wave_number, + arm_length, + time_delay, + side_of_body, + muscle_start_end_index, + step, + post_processing, + ): + """ + + Parameters + ---------- + amplitude : float + Amplitude of muscle forces. + wave_number : float + Wave number for muscle actuation. + arm_length : float + Used to map the torques optimized by CMA into the contraction forces of our muscles. + time_delay : float + Delay time for muscles. + side_of_body : int + Depending on which side of body, left or right this variable becomes +1 or -1. + muscle_start_end_index : numpy.ndarray + 1D (2) array containing data with 'int' type. + Element start and end index of muscle forces. + """ + + self.amplitude = amplitude + self.wave_number = wave_number + self.side_of_body = side_of_body + self.time_delay = time_delay + self.muscle_start_end_index = muscle_start_end_index + + """ + For legacy purposes, the input from the optimizer is given in terms of + torque amplitudes (see Gazzola et al. Royal Society Open Source, 2018). + We then use the same set up and map those values into muscle + contractile forces by dividing them by the arm length. This is captured + through the parameter "factor". This also enables a meaningful + comparison with the continuum snake case of the above reference. + """ + self.amplitude /= arm_length + + self.post_processing = post_processing + self.step = step + self.counter=0 + + def apply_forces(self, system, time: np.float = 0.0): + forces = self._apply_forces( + self.amplitude, + self.wave_number, + self.side_of_body, + time, + self.time_delay, + self.muscle_start_end_index, + system.tangents, + system.external_forces, + ) + + if self.counter % self.step ==0: + self.post_processing["time"].append(time) + self.post_processing["step"].append(self.counter) + self.post_processing["external_forces"].append(forces.copy()) + self.post_processing["element_position"].append(np.cumsum(system.lengths).copy()) + + self.counter += 1 + + + @staticmethod + @njit(cache=True) + def _apply_forces( + amplitude, + wave_number, + side_of_body, + time, + time_delay, + muscle_start_end_index, + tangents, + external_forces, + ): + + real_time = time - time_delay + ramp = real_time if real_time <= 1.0 else 1.0 + factor = 0.0 if real_time <= 0.0 else ramp # max(0.0, ramp) + + forces = np.zeros(external_forces.shape) + + muscle_forces = ( + factor + * amplitude + * (side_of_body * 0.5 * np.sin(wave_number * real_time) + 0.5) + * tangents[..., muscle_start_end_index[0] : muscle_start_end_index[1]] + ) + + forces[ + ..., muscle_start_end_index[0] : muscle_start_end_index[1] + 1 + ] += difference_kernel(muscle_forces) + + external_forces += forces + return forces \ No newline at end of file diff --git a/examples/MuscularSnake/muscular_snake.py b/examples/MuscularSnake/muscular_snake.py new file mode 100644 index 00000000..619535b8 --- /dev/null +++ b/examples/MuscularSnake/muscular_snake.py @@ -0,0 +1,373 @@ +__doc__ = """Muscular snake example from Zhang et. al. Nature Comm 2019 paper.""" +from elastica import * +from examples.MuscularSnake.post_processing import plot_video_with_surface, plot_snake_velocity +from examples.MuscularSnake.muscle_forces import MuscleForces +from elastica.experimental.connection_contact_joint.parallel_connection import SurfaceJointSideBySide +from examples.MuscularSnake.get_connection_vector import get_connection_vector_straight_straight_rod + +# Set base simulator class +class MuscularSnakeSimulator(BaseSystemCollection, Constraints, Connections, Forcing, CallBacks): + pass + +muscular_snake_simulator = MuscularSnakeSimulator() + +# Simulation parameters +final_time = 0.01#16.0 +time_step = 5E-6 +total_steps = int(final_time/time_step) +rendering_fps = 30 +step_skip = int(1.0/(rendering_fps*time_step)) + + +rod_list = [] +# Snake body +n_elem_body = 100 +density_body = 1000 +base_length_body = 1.0 +base_radius_body = 0.025 +E = 1e7 +nu = 4E-3 +shear_modulus = E/ 2*(0.5 + 1.0) +poisson_ratio = 0.5 + +direction = np.array([1.0, 0.0, 0.0]) +normal = np.array([0.0, 0.0, 1.0]) +start = np.array([0.0, 0.0, base_radius_body]) + +snake_body = CosseratRod.straight_rod( + n_elem_body, + start, + direction, + normal, + base_length_body, + base_radius_body, + density_body, + nu, + youngs_modulus=E, + shear_modulus=shear_modulus, +) + +body_elem_length = snake_body.rest_lengths[0] + +# Define muscle fibers +n_muscle_fibers = 8 + +# Muscle force amplitudes +muscle_force_amplitudes = np.array( + [22.96, 22.96, 20.95, 20.95, 9.51, 9.51, 13.7, 13.7] +)[::-1]/2 + +# Set connection index of first node of each muscle with body +muscle_start_connection_index = [4, 4, 33, 33, 23, 23, 61, 61] +muscle_end_connection_index = [] +muscle_glue_connection_index = ( + [] +) # These are the middle node idx of muscles that are glued to body +muscle_rod_list = [] +""" +The muscle density is higher than the physiological one, since +we lump many muscles (SSP-SP, LD and IC) into one actuator. These rods +also represent the two tendons on the sides of the muscle which biologically +have a higher density than the muscle itself. For these reasons,we set the +muscle density to approximately twice the biological value. +""" + +density_muscle = 2000 +E_muscle = 1e4 +nu_muscle = nu +shear_modulus_muscle = E_muscle/ 2*(0.5 + 1.0) + +# Muscle group 1 and 3, define two antagonistic muscle pairs +n_elem_muscle_group_one_to_three = 13*3 +base_length_muscle = 0.39 +""" +In our simulation, we lump many biological tendons into one computational +tendon. As a result, our computational tendon is bigger in size, set as elements other than 4-8 +below. +""" +muscle_radius = np.zeros((n_elem_muscle_group_one_to_three)) +muscle_radius[:] = 0.003 # First set tendon radius for whole rod. +muscle_radius[4*3:9*3] = 0.006 # Change the radius of muscle elements + +for i in range(int(n_muscle_fibers/2)): + + index = muscle_start_connection_index[i] + # Chose which side of body we are attaching the muscles. Note that these muscles are antagonistic pairs. + # So they are at the opposite sides of the body and side_sign determines that. + side_sign = -1 if i % 2 == 0 else 1 + start_muscle = np.array( + [ + index * body_elem_length, + side_sign * (base_radius_body+0.003), + base_radius_body, + ] + ) + + muscle_rod = CosseratRod.straight_rod( + n_elem_muscle_group_one_to_three, + start_muscle, + direction, + normal, + base_length_muscle, + muscle_radius, + density_muscle, + nu_muscle, + youngs_modulus=E_muscle, + shear_modulus=shear_modulus_muscle, + ) + + + """ + The biological tendons have a high Young's modulus E.,but are very slender. + As a result, they resist extension (stretch) but can bend easily. + Due to our decision to lump tendons and in order to mimic the above behavior + of the biological tendons, we use a lower Young's + Modulus and harden the stiffness of the shear and stretch modes only. + Numerically, this is done by putting a pre-factor of 50000 before the + shear/stretch matrix below. The actual value of the prefactor does not matter, + what is important is that it is a high value to high stretch/shear stiffness. + """ + + muscle_rod.shear_matrix[..., :4*3] *= 50000 + muscle_rod.shear_matrix[..., 9*3:] *= 50000 + + muscle_rod_list.append(muscle_rod) + muscle_end_connection_index.append( + index + n_elem_muscle_group_one_to_three + ) + muscle_glue_connection_index.append( + np.hstack((np.arange(0, 4*3, 1, dtype=np.int64), np.arange(9*3, n_elem_muscle_group_one_to_three, 1, dtype=np.int64))) + ) + + +# Muscle group 2 and 4, define two antagonistic muscle pairs +n_elem_muscle_group_two_to_four = 33 +base_length_muscle = 0.33 +""" +In our simulation, we lump many biological tendons into one computational +tendon. As a result, our computational tendon is bigger in size, set as rm_t +below. +""" +muscle_radius = np.zeros((n_elem_muscle_group_two_to_four)) +muscle_radius[:] = 0.003 # First set tendon radius for whole rod. +muscle_radius[4*3:9*3] = 0.006 # Change the radius of muscle elements + +for i in range(int(n_muscle_fibers / 2), n_muscle_fibers): + + index = muscle_start_connection_index[i] + # Chose which side of body we are attaching the muscles. Note that these muscles are antagonistic pairs. + # So they are at the opposite sides of the body and side_sign determines that. + side_sign = -1 if i % 2 == 0 else 1 + start_muscle = np.array( + [ + index * body_elem_length, + side_sign * (base_radius_body+0.003), + base_radius_body, + ] + ) + + muscle_rod = CosseratRod.straight_rod( + n_elem_muscle_group_two_to_four, + start_muscle, + direction, + normal, + base_length_muscle, + muscle_radius, + density_muscle, + nu_muscle, + youngs_modulus=E_muscle, + shear_modulus=shear_modulus_muscle, + ) + + """ + The biological tendons have a high Young's modulus E.,but are very slender. + As a result, they resist extension (stretch) but can bend easily. + Due to our decision to lump tendons and in order to mimic the above behavior + of the biological tendons, we use a lower Young's + Modulus and harden the stiffness of the shear and stretch modes only. + Numerically, this is done by putting a pre-factor of 50000 before the + shear/stretch matrix below. The actual value of the prefactor does not matter, + what is important is that it is a high value to high stretch/shear stiffness. + """ + + muscle_rod.shear_matrix[..., :4*3] *= 50000 + muscle_rod.shear_matrix[..., 9*3:] *= 50000 + + muscle_rod_list.append(muscle_rod) + muscle_end_connection_index.append( + index + n_elem_muscle_group_two_to_four + ) + muscle_glue_connection_index.append( + # np.array([0,1, 2, 3, 9, 10 ], dtype=np.int) + np.hstack((np.arange(0, 4*3, 1, dtype=np.int64), np.arange(9*3, n_elem_muscle_group_two_to_four, 1, dtype=np.int64))) + ) + + +# After initializing the rods append them on to the simulation +rod_list.append(snake_body) +rod_list = rod_list + muscle_rod_list +for _, my_rod in enumerate(rod_list): + muscular_snake_simulator.append(my_rod) + +# Muscle actuation +post_processing_forces_dict_list = [] + +for i in range(n_muscle_fibers): + post_processing_forces_dict_list.append(defaultdict(list)) + muscle_rod = muscle_rod_list[i] + side_of_body = 1 if i % 2 == 0 else -1 + + time_delay = muscle_start_connection_index[::-1][i] * 1.0 / 101.76 + + muscular_snake_simulator.add_forcing_to(muscle_rod).using( + MuscleForces, + amplitude=muscle_force_amplitudes[i], + wave_number=2.0 * np.pi / 1.0, + arm_length=(base_radius_body+0.003), + time_delay=time_delay, + side_of_body=side_of_body, + muscle_start_end_index=np.array([4*3, 9*3], np.int64), + step=step_skip, + post_processing=post_processing_forces_dict_list[i], + ) + + +straight_straight_rod_connection_list = [] +straight_straight_rod_connection_post_processing_dict = defaultdict(list) +for idx, rod_two in enumerate(muscle_rod_list): + rod_one = snake_body + ( + rod_one_direction_vec_in_material_frame, + rod_two_direction_vec_in_material_frame, + offset_btw_rods, + ) = get_connection_vector_straight_straight_rod(rod_one, rod_two,muscle_start_connection_index[idx], muscle_end_connection_index[idx]) + straight_straight_rod_connection_list.append( + [ + rod_one, + rod_two, + rod_one_direction_vec_in_material_frame.copy(), + rod_two_direction_vec_in_material_frame.copy(), + offset_btw_rods.copy(), + ] + ) + for k in range(rod_two.n_elems): + rod_one_index = k + muscle_start_connection_index[idx] + rod_two_index = k + k_conn = rod_one.radius[rod_one_index] * rod_two.radius[rod_two_index] / (rod_one.radius[rod_one_index] + rod_two.radius[rod_two_index]) * body_elem_length * E / (rod_one.radius[rod_one_index] + rod_two.radius[rod_two_index]) + + if k < 12 or k>= 27: + scale = 1*2 + scale_contact = 20 + else: + scale = 0.01*5 + scale_contact = 20 + + muscular_snake_simulator.connect( + first_rod=rod_one, + second_rod=rod_two, + first_connect_idx=rod_one_index, + second_connect_idx=rod_two_index, + ).using( + SurfaceJointSideBySide, + k=k_conn * scale , + nu=1E-4, + k_repulsive=k_conn * scale_contact, + rod_one_direction_vec_in_material_frame=rod_one_direction_vec_in_material_frame[ + ..., k + ], + rod_two_direction_vec_in_material_frame=rod_two_direction_vec_in_material_frame[ + ..., k + ], + offset_btw_rods=offset_btw_rods[k], + post_processing_dict=straight_straight_rod_connection_post_processing_dict, + step_skip=step_skip, + ) + + # Friction forces + # Only apply to the snake body. + gravitational_acc = -9.81 + muscular_snake_simulator.add_forcing_to(snake_body).using( + GravityForces, acc_gravity=np.array([0.0, 0.0, gravitational_acc]) + ) + + origin_plane = np.array([0.0, 0.0, 0.0]) + normal_plane = normal + slip_velocity_tol = 1e-8 + froude = 0.1 + period = 1.0 + mu = base_length_body / (period * period * np.abs(gravitational_acc) * froude) + kinetic_mu_array = np.array( + [1.0*mu, 1.5 * mu, 2.0 * mu] + ) # [forward, backward, sideways] + static_mu_array = 2 * kinetic_mu_array + muscular_snake_simulator.add_forcing_to(snake_body).using( + AnisotropicFrictionalPlane, + k=1E1, + nu=20, + plane_origin=origin_plane, + plane_normal=normal_plane, + slip_velocity_tol=slip_velocity_tol, + static_mu_array=static_mu_array, + kinetic_mu_array=kinetic_mu_array, + ) + +class MuscularSnakeCallBack(CallBackBaseClass): + def __init__(self, step_skip: int, callback_params: dict): + CallBackBaseClass.__init__(self) + self.every = step_skip + self.callback_params = callback_params + + def make_callback(self, system, time, current_step: int): + + + if current_step % self.every == 0: + self.callback_params["time"].append(time) + self.callback_params["step"].append(current_step) + self.callback_params["position"].append( + system.position_collection.copy() + ) + self.callback_params["com"].append( + system.compute_position_center_of_mass() + ) + self.callback_params["radius"].append(system.radius.copy()) + self.callback_params["velocity"].append( + system.velocity_collection.copy() + ) + self.callback_params["avg_velocity"].append( + system.compute_velocity_center_of_mass() + ) + + self.callback_params["center_of_mass"].append( + system.compute_position_center_of_mass() + ) + +post_processing_dict_list = [] + +for idx, rod in enumerate(rod_list): + post_processing_dict_list.append(defaultdict(list)) + muscular_snake_simulator.collect_diagnostics(rod).using( + MuscularSnakeCallBack, + step_skip=step_skip, + callback_params=post_processing_dict_list[idx], + ) + +muscular_snake_simulator.finalize() +timestepper = PositionVerlet() +integrate(timestepper, muscular_snake_simulator, final_time, total_steps) + + +plot_video_with_surface( + post_processing_dict_list, + video_name="muscular_snake.mp4", + fps=rendering_fps, + step=1, + # The following parameters are optional + x_limits=(-0.1, 1.0), # Set bounds on x-axis + y_limits=(-0.3, 0.3), # Set bounds on y-axis + z_limits=(-0.3, 0.3), # Set bounds on z-axis + dpi=100, # Set the quality of the image + vis3D=True, # Turn on 3D visualization + vis2D=True, # Turn on projected (2D) visualization +) + +plot_snake_velocity(post_processing_dict_list[0], period=period, filename="muscular_snake_velocity.png") diff --git a/examples/MuscularSnake/post_processing.py b/examples/MuscularSnake/post_processing.py new file mode 100644 index 00000000..8de47eb1 --- /dev/null +++ b/examples/MuscularSnake/post_processing.py @@ -0,0 +1,529 @@ +import numpy as np +import matplotlib + +matplotlib.use("Agg") # Must be before importing matplotlib.pyplot or pylab! +from matplotlib import pyplot as plt +from matplotlib.colors import to_rgb +from matplotlib import cm +from mpl_toolkits.mplot3d import proj3d, Axes3D +from tqdm import tqdm + +from typing import Dict, Sequence + + +def plot_video_with_surface( + rods_history: Sequence[Dict], + video_name="video.mp4", + fps=60, + step=1, + vis2D=True, + **kwargs, +): + plt.rcParams.update({"font.size": 22}) + + folder_name = kwargs.get("folder_name","") + + # 2d case + import matplotlib.animation as animation + + # simulation time + sim_time = np.array(rods_history[0]["time"]) + + # Rod + n_visualized_rods = len(rods_history) # should be one for now + # Rod info + rod_history_unpacker = lambda rod_idx, t_idx: ( + rods_history[rod_idx]["position"][t_idx], + rods_history[rod_idx]["radius"][t_idx], + ) + # Rod center of mass + com_history_unpacker = lambda rod_idx, t_idx: rods_history[rod_idx]["com"][time_idx] + + # Generate target sphere data + sphere_flag = False + if kwargs.__contains__("sphere_history"): + sphere_flag = True + sphere_history = kwargs.get("sphere_history") + n_visualized_spheres = len(sphere_history) # should be one for now + sphere_history_unpacker = lambda sph_idx, t_idx: ( + sphere_history[sph_idx]["position"][t_idx], + sphere_history[sph_idx]["radius"][t_idx], + ) + # color mapping + sphere_cmap = cm.get_cmap("Spectral", n_visualized_spheres) + + # video pre-processing + print("plot scene visualization video") + FFMpegWriter = animation.writers["ffmpeg"] + metadata = dict(title="Movie Test", artist="Matplotlib", comment="Movie support!") + writer = FFMpegWriter(fps=fps, metadata=metadata) + dpi = kwargs.get("dpi", 100) + + xlim = kwargs.get("x_limits", (-1.0, 1.0)) + ylim = kwargs.get("y_limits", (-1.0, 1.0)) + zlim = kwargs.get("z_limits", (-0.05, 1.0)) + + difference = lambda x: x[1] - x[0] + max_axis_length = max(difference(xlim), difference(ylim)) + # The scaling factor from physical space to matplotlib space + scaling_factor = (2 * 0.1) / max_axis_length # Octopus head dimension + scaling_factor *= 2.6e3 # Along one-axis + + if kwargs.get("vis3D", True): + fig = plt.figure(1, figsize=(10, 8), frameon=True, dpi=dpi) + ax = plt.axes(projection="3d") + + ax.set_xlabel("x") + ax.set_ylabel("y") + ax.set_zlabel("z") + + ax.set_xlim(*xlim) + ax.set_ylim(*ylim) + ax.set_zlim(*zlim) + + time_idx = 0 + rod_lines = [None for _ in range(n_visualized_rods)] + rod_com_lines = [None for _ in range(n_visualized_rods)] + rod_scatters = [None for _ in range(n_visualized_rods)] + + for rod_idx in range(n_visualized_rods): + inst_position, inst_radius = rod_history_unpacker(rod_idx, time_idx) + if not inst_position.shape[1] == inst_radius.shape[0]: + inst_position = 0.5 * (inst_position[..., 1:] + inst_position[..., :-1]) + + rod_scatters[rod_idx] = ax.scatter( + inst_position[0], + inst_position[1], + inst_position[2], + s=np.pi * (scaling_factor * inst_radius) ** 2, + ) + + if sphere_flag: + sphere_artists = [None for _ in range(n_visualized_spheres)] + for sphere_idx in range(n_visualized_spheres): + sphere_position, sphere_radius = sphere_history_unpacker( + sphere_idx, time_idx + ) + sphere_artists[sphere_idx] = ax.scatter( + sphere_position[0], + sphere_position[1], + sphere_position[2], + s=np.pi * (scaling_factor * sphere_radius) ** 2, + ) + # sphere_radius, + # color=sphere_cmap(sphere_idx),) + ax.add_artist(sphere_artists[sphere_idx]) + + # ax.set_aspect("equal") + video_name_3D = folder_name+"3D_" + video_name + + with writer.saving(fig, video_name_3D, dpi): + with plt.style.context("seaborn-whitegrid"): + for time_idx in tqdm(range(0, sim_time.shape[0], int(step))): + + for rod_idx in range(n_visualized_rods): + inst_position, inst_radius = rod_history_unpacker( + rod_idx, time_idx + ) + if not inst_position.shape[1] == inst_radius.shape[0]: + inst_position = 0.5 * ( + inst_position[..., 1:] + inst_position[..., :-1] + ) + + rod_scatters[rod_idx]._offsets3d = ( + inst_position[0], + inst_position[1], + inst_position[2], + ) + + # rod_scatters[rod_idx].set_offsets(inst_position[:2].T) + rod_scatters[rod_idx].set_sizes( + np.pi * (scaling_factor * inst_radius) ** 2 + ) + + if sphere_flag: + for sphere_idx in range(n_visualized_spheres): + sphere_position, _ = sphere_history_unpacker( + sphere_idx, time_idx + ) + sphere_artists[sphere_idx]._offsets3d = ( + sphere_position[0], + sphere_position[1], + sphere_position[2], + ) + + writer.grab_frame() + + # Be a good boy and close figures + # https://stackoverflow.com/a/37451036 + # plt.close(fig) alone does not suffice + # See https://github.com/matplotlib/matplotlib/issues/8560/ + plt.close(plt.gcf()) + + if kwargs.get("vis2D", True): + max_axis_length = max(difference(xlim), difference(ylim)) + # The scaling factor from physical space to matplotlib space + scaling_factor = (2 * 0.1) / max_axis_length # Octopus head dimension + scaling_factor *= 2.6e3 # Along one-axis + + fig = plt.figure(2, figsize=(10, 8), frameon=True, dpi=dpi) + ax = fig.add_subplot(111) + ax.set_xlim(*xlim) + ax.set_ylim(*ylim) + + time_idx = 0 + rod_lines = [None for _ in range(n_visualized_rods)] + rod_com_lines = [None for _ in range(n_visualized_rods)] + rod_scatters = [None for _ in range(n_visualized_rods)] + + for rod_idx in range(n_visualized_rods): + inst_position, inst_radius = rod_history_unpacker(rod_idx, time_idx) + if not inst_position.shape[1] == inst_radius.shape[0]: + inst_position = 0.5 * (inst_position[..., 1:] + inst_position[..., :-1]) + rod_lines[rod_idx] = ax.plot( + inst_position[0], inst_position[1], "r", lw=0.5 + )[0] + inst_com = com_history_unpacker(rod_idx, time_idx) + rod_com_lines[rod_idx] = ax.plot(inst_com[0], inst_com[1], "k--", lw=2.0)[0] + + rod_scatters[rod_idx] = ax.scatter( + inst_position[0], + inst_position[1], + s=np.pi * (scaling_factor * inst_radius) ** 2, + ) + + if sphere_flag: + sphere_artists = [None for _ in range(n_visualized_spheres)] + for sphere_idx in range(n_visualized_spheres): + sphere_position, sphere_radius = sphere_history_unpacker( + sphere_idx, time_idx + ) + sphere_artists[sphere_idx] = Circle( + (sphere_position[0], sphere_position[1]), + sphere_radius, + color=sphere_cmap(sphere_idx), + ) + ax.add_artist(sphere_artists[sphere_idx]) + + ax.set_aspect("equal") + video_name_2D = folder_name+"2D_xy_" + video_name + + with writer.saving(fig, video_name_2D, dpi): + with plt.style.context("seaborn-whitegrid"): + for time_idx in tqdm(range(0, sim_time.shape[0], int(step))): + + for rod_idx in range(n_visualized_rods): + inst_position, inst_radius = rod_history_unpacker( + rod_idx, time_idx + ) + if not inst_position.shape[1] == inst_radius.shape[0]: + inst_position = 0.5 * ( + inst_position[..., 1:] + inst_position[..., :-1] + ) + + rod_lines[rod_idx].set_xdata(inst_position[0]) + rod_lines[rod_idx].set_ydata(inst_position[1]) + + com = com_history_unpacker(rod_idx, time_idx) + rod_com_lines[rod_idx].set_xdata(com[0]) + rod_com_lines[rod_idx].set_ydata(com[1]) + + rod_scatters[rod_idx].set_offsets(inst_position[:2].T) + rod_scatters[rod_idx].set_sizes( + np.pi * (scaling_factor * inst_radius) ** 2 + ) + + if sphere_flag: + for sphere_idx in range(n_visualized_spheres): + sphere_position, _ = sphere_history_unpacker( + sphere_idx, time_idx + ) + sphere_artists[sphere_idx].center = ( + sphere_position[0], + sphere_position[1], + ) + + writer.grab_frame() + + # Be a good boy and close figures + # https://stackoverflow.com/a/37451036 + # plt.close(fig) alone does not suffice + # See https://github.com/matplotlib/matplotlib/issues/8560/ + plt.close(plt.gcf()) + + # Plot zy + max_axis_length = max(difference(zlim), difference(ylim)) + # The scaling factor from physical space to matplotlib space + scaling_factor = (2 * 0.1) / max_axis_length # Octopus head dimension + scaling_factor *= 2.6e3 # Along one-axis + + fig = plt.figure(2, figsize=(10, 8), frameon=True, dpi=dpi) + ax = fig.add_subplot(111) + ax.set_xlim(*zlim) + ax.set_ylim(*ylim) + + time_idx = 0 + rod_lines = [None for _ in range(n_visualized_rods)] + rod_com_lines = [None for _ in range(n_visualized_rods)] + rod_scatters = [None for _ in range(n_visualized_rods)] + + for rod_idx in range(n_visualized_rods): + inst_position, inst_radius = rod_history_unpacker(rod_idx, time_idx) + if not inst_position.shape[1] == inst_radius.shape[0]: + inst_position = 0.5 * (inst_position[..., 1:] + inst_position[..., :-1]) + rod_lines[rod_idx] = ax.plot( + inst_position[2], inst_position[1], "r", lw=0.5 + )[0] + inst_com = com_history_unpacker(rod_idx, time_idx) + rod_com_lines[rod_idx] = ax.plot(inst_com[2], inst_com[1], "k--", lw=2.0)[0] + + rod_scatters[rod_idx] = ax.scatter( + inst_position[2], + inst_position[1], + s=np.pi * (scaling_factor * inst_radius) ** 2, + ) + + if sphere_flag: + sphere_artists = [None for _ in range(n_visualized_spheres)] + for sphere_idx in range(n_visualized_spheres): + sphere_position, sphere_radius = sphere_history_unpacker( + sphere_idx, time_idx + ) + sphere_artists[sphere_idx] = Circle( + (sphere_position[2], sphere_position[1]), + sphere_radius, + color=sphere_cmap(sphere_idx), + ) + ax.add_artist(sphere_artists[sphere_idx]) + + ax.set_aspect("equal") + video_name_2D = folder_name+"2D_zy_" + video_name + + with writer.saving(fig, video_name_2D, dpi): + with plt.style.context("seaborn-whitegrid"): + for time_idx in tqdm(range(0, sim_time.shape[0], int(step))): + + for rod_idx in range(n_visualized_rods): + inst_position, inst_radius = rod_history_unpacker( + rod_idx, time_idx + ) + if not inst_position.shape[1] == inst_radius.shape[0]: + inst_position = 0.5 * ( + inst_position[..., 1:] + inst_position[..., :-1] + ) + + rod_lines[rod_idx].set_xdata(inst_position[2]) + rod_lines[rod_idx].set_ydata(inst_position[1]) + + com = com_history_unpacker(rod_idx, time_idx) + rod_com_lines[rod_idx].set_xdata(com[2]) + rod_com_lines[rod_idx].set_ydata(com[1]) + + rod_scatters[rod_idx].set_offsets( + np.vstack((inst_position[2], inst_position[1])).T + ) + rod_scatters[rod_idx].set_sizes( + np.pi * (scaling_factor * inst_radius) ** 2 + ) + + if sphere_flag: + for sphere_idx in range(n_visualized_spheres): + sphere_position, _ = sphere_history_unpacker( + sphere_idx, time_idx + ) + sphere_artists[sphere_idx].center = ( + sphere_position[2], + sphere_position[1], + ) + + writer.grab_frame() + + # Be a good boy and close figures + # https://stackoverflow.com/a/37451036 + # plt.close(fig) alone does not suffice + # See https://github.com/matplotlib/matplotlib/issues/8560/ + plt.close(plt.gcf()) + + # Plot xz + fig = plt.figure(2, figsize=(10, 8), frameon=True, dpi=dpi) + ax = fig.add_subplot(111) + ax.set_xlim(*xlim) + ax.set_ylim(*zlim) + + # The scaling factor from physical space to matplotlib space + max_axis_length = max(difference(zlim), difference(xlim)) + scaling_factor = (2 * 0.1) / (max_axis_length) # Octopus head dimension + scaling_factor *= 2.6e3 # Along one-axis + + time_idx = 0 + rod_lines = [None for _ in range(n_visualized_rods)] + rod_com_lines = [None for _ in range(n_visualized_rods)] + rod_scatters = [None for _ in range(n_visualized_rods)] + + for rod_idx in range(n_visualized_rods): + inst_position, inst_radius = rod_history_unpacker(rod_idx, time_idx) + if not inst_position.shape[1] == inst_radius.shape[0]: + inst_position = 0.5 * (inst_position[..., 1:] + inst_position[..., :-1]) + rod_lines[rod_idx] = ax.plot( + inst_position[0], inst_position[2], "r", lw=0.5 + )[0] + inst_com = com_history_unpacker(rod_idx, time_idx) + rod_com_lines[rod_idx] = ax.plot(inst_com[0], inst_com[2], "k--", lw=2.0)[0] + + rod_scatters[rod_idx] = ax.scatter( + inst_position[0], + inst_position[2], + s=np.pi * (scaling_factor * inst_radius) ** 2, + ) + + if sphere_flag: + sphere_artists = [None for _ in range(n_visualized_spheres)] + for sphere_idx in range(n_visualized_spheres): + sphere_position, sphere_radius = sphere_history_unpacker( + sphere_idx, time_idx + ) + sphere_artists[sphere_idx] = Circle( + (sphere_position[0], sphere_position[2]), + sphere_radius, + color=sphere_cmap(sphere_idx), + ) + ax.add_artist(sphere_artists[sphere_idx]) + + ax.set_aspect("equal") + video_name_2D = folder_name+"2D_xz_" + video_name + + with writer.saving(fig, video_name_2D, dpi): + with plt.style.context("seaborn-whitegrid"): + for time_idx in tqdm(range(0, sim_time.shape[0], int(step))): + + for rod_idx in range(n_visualized_rods): + inst_position, inst_radius = rod_history_unpacker( + rod_idx, time_idx + ) + if not inst_position.shape[1] == inst_radius.shape[0]: + inst_position = 0.5 * ( + inst_position[..., 1:] + inst_position[..., :-1] + ) + + rod_lines[rod_idx].set_xdata(inst_position[0]) + rod_lines[rod_idx].set_ydata(inst_position[2]) + + com = com_history_unpacker(rod_idx, time_idx) + rod_com_lines[rod_idx].set_xdata(com[0]) + rod_com_lines[rod_idx].set_ydata(com[2]) + + rod_scatters[rod_idx].set_offsets( + np.vstack((inst_position[0], inst_position[2])).T + ) + rod_scatters[rod_idx].set_sizes( + np.pi * (scaling_factor * inst_radius) ** 2 + ) + + if sphere_flag: + for sphere_idx in range(n_visualized_spheres): + sphere_position, _ = sphere_history_unpacker( + sphere_idx, time_idx + ) + sphere_artists[sphere_idx].center = ( + sphere_position[0], + sphere_position[2], + ) + + writer.grab_frame() + + # Be a good boy and close figures + # https://stackoverflow.com/a/37451036 + # plt.close(fig) alone does not suffice + # See https://github.com/matplotlib/matplotlib/issues/8560/ + plt.close(plt.gcf()) + +def plot_snake_velocity( + plot_params: dict, + period, + filename="slithering_snake_velocity.png", +): + time_per_period = np.array(plot_params["time"]) / period + avg_velocity = np.array(plot_params["avg_velocity"]) + + [ + velocity_in_direction_of_rod, + velocity_in_rod_roll_dir, + _, + _, + ] = compute_projected_velocity(plot_params, period) + + fig = plt.figure(figsize=(10, 8), frameon=True, dpi=150) + ax = fig.add_subplot(111) + ax.grid(b=True, which="minor", color="k", linestyle="--") + ax.grid(b=True, which="major", color="k", linestyle="-") + ax.plot( + time_per_period[:], velocity_in_direction_of_rod[:, 0], "r-", label="forward" + ) + ax.plot( + time_per_period[:], + velocity_in_rod_roll_dir[:, 1], + c=to_rgb("xkcd:bluish"), + label="lateral", + ) + ax.plot(time_per_period[:], avg_velocity[:, 2], "k-", label="normal") + fig.legend(prop={"size": 20}) + fig.savefig(filename) + +def compute_projected_velocity(plot_params: dict, period): + + time_per_period = np.array(plot_params["time"]) / period + avg_velocity = np.array(plot_params["avg_velocity"]) + center_of_mass = np.array(plot_params["center_of_mass"]) + + # Compute rod velocity in rod direction. We need to compute that because, + # after snake starts to move it chooses an arbitrary direction, which does not + # have to be initial tangent direction of the rod. Thus we need to project the + # snake velocity with respect to its new tangent and roll direction, after that + # we will get the correct forward and lateral speed. After this projection + # lateral velocity of the snake has to be oscillating between + and - values with + # zero mean. + + # Number of steps in one period. + period_step = int(period / (time_per_period[-1] - time_per_period[-2])) + 1 + number_of_period = int(time_per_period.shape[0] / period_step) + # Center of mass position averaged in one period + center_of_mass_averaged_over_one_period = np.zeros((number_of_period - 2, 3)) + for i in range(1, number_of_period - 1): + # position of center of mass averaged over one period + center_of_mass_averaged_over_one_period[i - 1] = np.mean( + center_of_mass[(i + 1) * period_step : (i + 2) * period_step] + - center_of_mass[(i + 0) * period_step : (i + 1) * period_step], + axis=0, + ) + + # Average the rod directions over multiple periods and get the direction of the rod. + direction_of_rod = np.mean(center_of_mass_averaged_over_one_period, axis=0) + direction_of_rod /= np.linalg.norm(direction_of_rod, ord=2) + print("direction of rod " + str(direction_of_rod)) + + # Compute the projected rod velocity in the direction of the rod + velocity_mag_in_direction_of_rod = np.einsum( + "ji,i->j", avg_velocity, direction_of_rod + ) + velocity_in_direction_of_rod = np.einsum( + "j,i->ji", velocity_mag_in_direction_of_rod, direction_of_rod + ) + + # Get the lateral or roll velocity of the rod after subtracting its projected + # velocity in the direction of rod + velocity_in_rod_roll_dir = avg_velocity - velocity_in_direction_of_rod + + # Compute the average velocity over the simulation, this can be used for optimizing snake + # for fastest forward velocity. We start after first period, because of the ramping up happens + # in first period. + average_velocity_over_simulation = np.mean( + velocity_in_direction_of_rod[period_step * 2 :], axis=0 + ) + + return ( + velocity_in_direction_of_rod, + velocity_in_rod_roll_dir, + average_velocity_over_simulation[0], + average_velocity_over_simulation[1], + ) + From a079f22a2fd20bbce6acfc9adba3b773e56766e3 Mon Sep 17 00:00:00 2001 From: armantekinalp Date: Sat, 16 Apr 2022 09:50:48 -0500 Subject: [PATCH 2/5] fix:path issues and black fmt muscular snake --- examples/MuscularSnake/muscular_snake.py | 141 ++++++++++++++--------- 1 file changed, 84 insertions(+), 57 deletions(-) diff --git a/examples/MuscularSnake/muscular_snake.py b/examples/MuscularSnake/muscular_snake.py index 619535b8..c49dd083 100644 --- a/examples/MuscularSnake/muscular_snake.py +++ b/examples/MuscularSnake/muscular_snake.py @@ -1,22 +1,35 @@ __doc__ = """Muscular snake example from Zhang et. al. Nature Comm 2019 paper.""" +import sys + +sys.path.append("../../") from elastica import * -from examples.MuscularSnake.post_processing import plot_video_with_surface, plot_snake_velocity +from examples.MuscularSnake.post_processing import ( + plot_video_with_surface, + plot_snake_velocity, +) from examples.MuscularSnake.muscle_forces import MuscleForces -from elastica.experimental.connection_contact_joint.parallel_connection import SurfaceJointSideBySide -from examples.MuscularSnake.get_connection_vector import get_connection_vector_straight_straight_rod +from elastica.experimental.connection_contact_joint.parallel_connection import ( + SurfaceJointSideBySide, +) +from examples.MuscularSnake.get_connection_vector import ( + get_connection_vector_straight_straight_rod, +) # Set base simulator class -class MuscularSnakeSimulator(BaseSystemCollection, Constraints, Connections, Forcing, CallBacks): +class MuscularSnakeSimulator( + BaseSystemCollection, Constraints, Connections, Forcing, CallBacks +): pass + muscular_snake_simulator = MuscularSnakeSimulator() # Simulation parameters -final_time = 0.01#16.0 -time_step = 5E-6 -total_steps = int(final_time/time_step) +final_time = 16.0 +time_step = 5e-6 +total_steps = int(final_time / time_step) rendering_fps = 30 -step_skip = int(1.0/(rendering_fps*time_step)) +step_skip = int(1.0 / (rendering_fps * time_step)) rod_list = [] @@ -26,8 +39,8 @@ class MuscularSnakeSimulator(BaseSystemCollection, Constraints, Connections, For base_length_body = 1.0 base_radius_body = 0.025 E = 1e7 -nu = 4E-3 -shear_modulus = E/ 2*(0.5 + 1.0) +nu = 4e-3 +shear_modulus = E / 2 * (0.5 + 1.0) poisson_ratio = 0.5 direction = np.array([1.0, 0.0, 0.0]) @@ -53,9 +66,9 @@ class MuscularSnakeSimulator(BaseSystemCollection, Constraints, Connections, For n_muscle_fibers = 8 # Muscle force amplitudes -muscle_force_amplitudes = np.array( - [22.96, 22.96, 20.95, 20.95, 9.51, 9.51, 13.7, 13.7] -)[::-1]/2 +muscle_force_amplitudes = ( + np.array([22.96, 22.96, 20.95, 20.95, 9.51, 9.51, 13.7, 13.7])[::-1] / 2 +) # Set connection index of first node of each muscle with body muscle_start_connection_index = [4, 4, 33, 33, 23, 23, 61, 61] @@ -75,10 +88,10 @@ class MuscularSnakeSimulator(BaseSystemCollection, Constraints, Connections, For density_muscle = 2000 E_muscle = 1e4 nu_muscle = nu -shear_modulus_muscle = E_muscle/ 2*(0.5 + 1.0) +shear_modulus_muscle = E_muscle / 2 * (0.5 + 1.0) # Muscle group 1 and 3, define two antagonistic muscle pairs -n_elem_muscle_group_one_to_three = 13*3 +n_elem_muscle_group_one_to_three = 13 * 3 base_length_muscle = 0.39 """ In our simulation, we lump many biological tendons into one computational @@ -87,9 +100,9 @@ class MuscularSnakeSimulator(BaseSystemCollection, Constraints, Connections, For """ muscle_radius = np.zeros((n_elem_muscle_group_one_to_three)) muscle_radius[:] = 0.003 # First set tendon radius for whole rod. -muscle_radius[4*3:9*3] = 0.006 # Change the radius of muscle elements +muscle_radius[4 * 3 : 9 * 3] = 0.006 # Change the radius of muscle elements -for i in range(int(n_muscle_fibers/2)): +for i in range(int(n_muscle_fibers / 2)): index = muscle_start_connection_index[i] # Chose which side of body we are attaching the muscles. Note that these muscles are antagonistic pairs. @@ -98,9 +111,9 @@ class MuscularSnakeSimulator(BaseSystemCollection, Constraints, Connections, For start_muscle = np.array( [ index * body_elem_length, - side_sign * (base_radius_body+0.003), + side_sign * (base_radius_body + 0.003), base_radius_body, - ] + ] ) muscle_rod = CosseratRod.straight_rod( @@ -116,7 +129,6 @@ class MuscularSnakeSimulator(BaseSystemCollection, Constraints, Connections, For shear_modulus=shear_modulus_muscle, ) - """ The biological tendons have a high Young's modulus E.,but are very slender. As a result, they resist extension (stretch) but can bend easily. @@ -128,15 +140,18 @@ class MuscularSnakeSimulator(BaseSystemCollection, Constraints, Connections, For what is important is that it is a high value to high stretch/shear stiffness. """ - muscle_rod.shear_matrix[..., :4*3] *= 50000 - muscle_rod.shear_matrix[..., 9*3:] *= 50000 + muscle_rod.shear_matrix[..., : 4 * 3] *= 50000 + muscle_rod.shear_matrix[..., 9 * 3 :] *= 50000 muscle_rod_list.append(muscle_rod) - muscle_end_connection_index.append( - index + n_elem_muscle_group_one_to_three - ) + muscle_end_connection_index.append(index + n_elem_muscle_group_one_to_three) muscle_glue_connection_index.append( - np.hstack((np.arange(0, 4*3, 1, dtype=np.int64), np.arange(9*3, n_elem_muscle_group_one_to_three, 1, dtype=np.int64))) + np.hstack( + ( + np.arange(0, 4 * 3, 1, dtype=np.int64), + np.arange(9 * 3, n_elem_muscle_group_one_to_three, 1, dtype=np.int64), + ) + ) ) @@ -150,7 +165,7 @@ class MuscularSnakeSimulator(BaseSystemCollection, Constraints, Connections, For """ muscle_radius = np.zeros((n_elem_muscle_group_two_to_four)) muscle_radius[:] = 0.003 # First set tendon radius for whole rod. -muscle_radius[4*3:9*3] = 0.006 # Change the radius of muscle elements +muscle_radius[4 * 3 : 9 * 3] = 0.006 # Change the radius of muscle elements for i in range(int(n_muscle_fibers / 2), n_muscle_fibers): @@ -161,9 +176,9 @@ class MuscularSnakeSimulator(BaseSystemCollection, Constraints, Connections, For start_muscle = np.array( [ index * body_elem_length, - side_sign * (base_radius_body+0.003), + side_sign * (base_radius_body + 0.003), base_radius_body, - ] + ] ) muscle_rod = CosseratRod.straight_rod( @@ -190,16 +205,19 @@ class MuscularSnakeSimulator(BaseSystemCollection, Constraints, Connections, For what is important is that it is a high value to high stretch/shear stiffness. """ - muscle_rod.shear_matrix[..., :4*3] *= 50000 - muscle_rod.shear_matrix[..., 9*3:] *= 50000 + muscle_rod.shear_matrix[..., : 4 * 3] *= 50000 + muscle_rod.shear_matrix[..., 9 * 3 :] *= 50000 muscle_rod_list.append(muscle_rod) - muscle_end_connection_index.append( - index + n_elem_muscle_group_two_to_four - ) + muscle_end_connection_index.append(index + n_elem_muscle_group_two_to_four) muscle_glue_connection_index.append( # np.array([0,1, 2, 3, 9, 10 ], dtype=np.int) - np.hstack((np.arange(0, 4*3, 1, dtype=np.int64), np.arange(9*3, n_elem_muscle_group_two_to_four, 1, dtype=np.int64))) + np.hstack( + ( + np.arange(0, 4 * 3, 1, dtype=np.int64), + np.arange(9 * 3, n_elem_muscle_group_two_to_four, 1, dtype=np.int64), + ) + ) ) @@ -223,10 +241,10 @@ class MuscularSnakeSimulator(BaseSystemCollection, Constraints, Connections, For MuscleForces, amplitude=muscle_force_amplitudes[i], wave_number=2.0 * np.pi / 1.0, - arm_length=(base_radius_body+0.003), + arm_length=(base_radius_body + 0.003), time_delay=time_delay, side_of_body=side_of_body, - muscle_start_end_index=np.array([4*3, 9*3], np.int64), + muscle_start_end_index=np.array([4 * 3, 9 * 3], np.int64), step=step_skip, post_processing=post_processing_forces_dict_list[i], ) @@ -240,7 +258,12 @@ class MuscularSnakeSimulator(BaseSystemCollection, Constraints, Connections, For rod_one_direction_vec_in_material_frame, rod_two_direction_vec_in_material_frame, offset_btw_rods, - ) = get_connection_vector_straight_straight_rod(rod_one, rod_two,muscle_start_connection_index[idx], muscle_end_connection_index[idx]) + ) = get_connection_vector_straight_straight_rod( + rod_one, + rod_two, + muscle_start_connection_index[idx], + muscle_end_connection_index[idx], + ) straight_straight_rod_connection_list.append( [ rod_one, @@ -253,13 +276,20 @@ class MuscularSnakeSimulator(BaseSystemCollection, Constraints, Connections, For for k in range(rod_two.n_elems): rod_one_index = k + muscle_start_connection_index[idx] rod_two_index = k - k_conn = rod_one.radius[rod_one_index] * rod_two.radius[rod_two_index] / (rod_one.radius[rod_one_index] + rod_two.radius[rod_two_index]) * body_elem_length * E / (rod_one.radius[rod_one_index] + rod_two.radius[rod_two_index]) + k_conn = ( + rod_one.radius[rod_one_index] + * rod_two.radius[rod_two_index] + / (rod_one.radius[rod_one_index] + rod_two.radius[rod_two_index]) + * body_elem_length + * E + / (rod_one.radius[rod_one_index] + rod_two.radius[rod_two_index]) + ) - if k < 12 or k>= 27: - scale = 1*2 + if k < 12 or k >= 27: + scale = 1 * 2 scale_contact = 20 else: - scale = 0.01*5 + scale = 0.01 * 5 scale_contact = 20 muscular_snake_simulator.connect( @@ -269,8 +299,8 @@ class MuscularSnakeSimulator(BaseSystemCollection, Constraints, Connections, For second_connect_idx=rod_two_index, ).using( SurfaceJointSideBySide, - k=k_conn * scale , - nu=1E-4, + k=k_conn * scale, + nu=1e-4, k_repulsive=k_conn * scale_contact, rod_one_direction_vec_in_material_frame=rod_one_direction_vec_in_material_frame[ ..., k @@ -297,12 +327,12 @@ class MuscularSnakeSimulator(BaseSystemCollection, Constraints, Connections, For period = 1.0 mu = base_length_body / (period * period * np.abs(gravitational_acc) * froude) kinetic_mu_array = np.array( - [1.0*mu, 1.5 * mu, 2.0 * mu] + [1.0 * mu, 1.5 * mu, 2.0 * mu] ) # [forward, backward, sideways] static_mu_array = 2 * kinetic_mu_array muscular_snake_simulator.add_forcing_to(snake_body).using( AnisotropicFrictionalPlane, - k=1E1, + k=1e1, nu=20, plane_origin=origin_plane, plane_normal=normal_plane, @@ -311,6 +341,7 @@ class MuscularSnakeSimulator(BaseSystemCollection, Constraints, Connections, For kinetic_mu_array=kinetic_mu_array, ) + class MuscularSnakeCallBack(CallBackBaseClass): def __init__(self, step_skip: int, callback_params: dict): CallBackBaseClass.__init__(self) @@ -319,20 +350,13 @@ def __init__(self, step_skip: int, callback_params: dict): def make_callback(self, system, time, current_step: int): - if current_step % self.every == 0: self.callback_params["time"].append(time) self.callback_params["step"].append(current_step) - self.callback_params["position"].append( - system.position_collection.copy() - ) - self.callback_params["com"].append( - system.compute_position_center_of_mass() - ) + self.callback_params["position"].append(system.position_collection.copy()) + self.callback_params["com"].append(system.compute_position_center_of_mass()) self.callback_params["radius"].append(system.radius.copy()) - self.callback_params["velocity"].append( - system.velocity_collection.copy() - ) + self.callback_params["velocity"].append(system.velocity_collection.copy()) self.callback_params["avg_velocity"].append( system.compute_velocity_center_of_mass() ) @@ -341,6 +365,7 @@ def make_callback(self, system, time, current_step: int): system.compute_position_center_of_mass() ) + post_processing_dict_list = [] for idx, rod in enumerate(rod_list): @@ -370,4 +395,6 @@ def make_callback(self, system, time, current_step: int): vis2D=True, # Turn on projected (2D) visualization ) -plot_snake_velocity(post_processing_dict_list[0], period=period, filename="muscular_snake_velocity.png") +plot_snake_velocity( + post_processing_dict_list[0], period=period, filename="muscular_snake_velocity.png" +) From 0d8f5c29543e9bfc3a0e0a80862e3e14fd920322 Mon Sep 17 00:00:00 2001 From: armantekinalp Date: Sat, 16 Apr 2022 22:28:24 -0500 Subject: [PATCH 3/5] bugfix:friction forces defined on wrong rods --- examples/MuscularSnake/muscular_snake.py | 52 ++++++++++++------------ 1 file changed, 26 insertions(+), 26 deletions(-) diff --git a/examples/MuscularSnake/muscular_snake.py b/examples/MuscularSnake/muscular_snake.py index c49dd083..da5dc437 100644 --- a/examples/MuscularSnake/muscular_snake.py +++ b/examples/MuscularSnake/muscular_snake.py @@ -313,33 +313,33 @@ class MuscularSnakeSimulator( step_skip=step_skip, ) - # Friction forces - # Only apply to the snake body. - gravitational_acc = -9.81 - muscular_snake_simulator.add_forcing_to(snake_body).using( - GravityForces, acc_gravity=np.array([0.0, 0.0, gravitational_acc]) - ) +# Friction forces +# Only apply to the snake body. +gravitational_acc = -9.81 +muscular_snake_simulator.add_forcing_to(snake_body).using( + GravityForces, acc_gravity=np.array([0.0, 0.0, gravitational_acc]) +) - origin_plane = np.array([0.0, 0.0, 0.0]) - normal_plane = normal - slip_velocity_tol = 1e-8 - froude = 0.1 - period = 1.0 - mu = base_length_body / (period * period * np.abs(gravitational_acc) * froude) - kinetic_mu_array = np.array( - [1.0 * mu, 1.5 * mu, 2.0 * mu] - ) # [forward, backward, sideways] - static_mu_array = 2 * kinetic_mu_array - muscular_snake_simulator.add_forcing_to(snake_body).using( - AnisotropicFrictionalPlane, - k=1e1, - nu=20, - plane_origin=origin_plane, - plane_normal=normal_plane, - slip_velocity_tol=slip_velocity_tol, - static_mu_array=static_mu_array, - kinetic_mu_array=kinetic_mu_array, - ) +origin_plane = np.array([0.0, 0.0, 0.0]) +normal_plane = normal +slip_velocity_tol = 1e-8 +froude = 0.1 +period = 1.0 +mu = base_length_body / (period * period * np.abs(gravitational_acc) * froude) +kinetic_mu_array = np.array( + [1.0 * mu, 1.5 * mu, 2.0 * mu] +) # [forward, backward, sideways] +static_mu_array = 2 * kinetic_mu_array +muscular_snake_simulator.add_forcing_to(snake_body).using( + AnisotropicFrictionalPlane, + k=1e1, + nu=20, + plane_origin=origin_plane, + plane_normal=normal_plane, + slip_velocity_tol=slip_velocity_tol, + static_mu_array=static_mu_array, + kinetic_mu_array=kinetic_mu_array, +) class MuscularSnakeCallBack(CallBackBaseClass): From 9bc2310259da7c3c68d1e7a01e0ad7426820255b Mon Sep 17 00:00:00 2001 From: armantekinalp Date: Sun, 17 Apr 2022 10:29:40 -0500 Subject: [PATCH 4/5] Update: Example readme.md updated and muscular snake description added --- examples/README.md | 3 +++ 1 file changed, 3 insertions(+) diff --git a/examples/README.md b/examples/README.md index c4af06dc..52800518 100644 --- a/examples/README.md +++ b/examples/README.md @@ -21,6 +21,9 @@ Examples can serve as a starting template for customized usages. * [ContinuumSnakeCase](./ContinuumSnakeCase) * __Purpose__: Demonstrate simple case of modeling biological creature using PyElastica. The example use friction to create slithering snake, and optimize the speed using [CMA](https://github.com/CMA-ES/pycma). * __Features__: CosseratRod, MuscleTorques, AnisotropicFrictionalPlane, Gravity, CMA Optimization + * [MuscularSnake](./MuscularSnake) + * __Purpose__: Example of [Parallel connection module](../elastica/experimental/connection_contact_joint/parallel_connection.py) and customized [Force module](./MuscularSnake/muscle_forces.py) to implement muscular snake. + * __Features__: MuscleForces(custom implemented) * [ButterflyCase](./ButterflyCase) * __Purpose__: Demonstrate simple restoration with initial strain. * __Features__: CosseratRod From e5884245aacbaf458fde7f8aac244f160146ee9c Mon Sep 17 00:00:00 2001 From: armantekinalp Date: Sun, 17 Apr 2022 18:07:11 -0500 Subject: [PATCH 5/5] update:get_connection_vector function is updated Now it takes the indexes that user want to connect. --- .../parallel_connection.py | 19 +++++-- .../parallel_connection_example.py | 2 +- .../MuscularSnake/get_connection_vector.py | 54 ------------------- examples/MuscularSnake/muscular_snake.py | 9 ++-- 4 files changed, 21 insertions(+), 63 deletions(-) delete mode 100644 examples/MuscularSnake/get_connection_vector.py diff --git a/elastica/experimental/connection_contact_joint/parallel_connection.py b/elastica/experimental/connection_contact_joint/parallel_connection.py index d9a74ab6..5113e07b 100644 --- a/elastica/experimental/connection_contact_joint/parallel_connection.py +++ b/elastica/experimental/connection_contact_joint/parallel_connection.py @@ -18,15 +18,25 @@ def get_connection_vector_straight_straight_rod( rod_one, rod_two, + rod_one_idx, + rod_two_idx, ): + rod_one_start_idx, rod_one_end_idx = rod_one_idx + rod_two_start_idx, rod_two_end_idx = rod_two_idx # Compute rod element positions rod_one_element_position = 0.5 * ( rod_one.position_collection[..., 1:] + rod_one.position_collection[..., :-1] ) + rod_one_element_position = rod_one_element_position[ + :, rod_one_start_idx:rod_one_end_idx + ] rod_two_element_position = 0.5 * ( rod_two.position_collection[..., 1:] + rod_two.position_collection[..., :-1] ) + rod_two_element_position = rod_two_element_position[ + :, rod_two_start_idx:rod_two_end_idx + ] # Lets get the distance between rod elements distance_vector_rod_one_to_rod_two = ( @@ -40,14 +50,17 @@ def get_connection_vector_straight_straight_rod( distance_vector_rod_two_to_rod_one = -distance_vector_rod_one_to_rod_two rod_one_direction_vec_in_material_frame = _batch_matvec( - rod_one.director_collection, distance_vector_rod_one_to_rod_two + rod_one.director_collection[:, :, rod_one_start_idx:rod_one_end_idx], + distance_vector_rod_one_to_rod_two, ) rod_two_direction_vec_in_material_frame = _batch_matvec( - rod_two.director_collection, distance_vector_rod_two_to_rod_one + rod_two.director_collection[:, :, rod_two_start_idx:rod_two_end_idx], + distance_vector_rod_two_to_rod_one, ) offset_btw_rods = distance_vector_rod_one_to_rod_two_norm - ( - rod_one.radius + rod_two.radius + rod_one.radius[rod_one_start_idx:rod_one_end_idx] + + rod_two.radius[rod_two_start_idx:rod_two_end_idx] ) return ( diff --git a/examples/ExperimentalCases/ParallelConnectionExample/parallel_connection_example.py b/examples/ExperimentalCases/ParallelConnectionExample/parallel_connection_example.py index a0de0c46..08db51cd 100644 --- a/examples/ExperimentalCases/ParallelConnectionExample/parallel_connection_example.py +++ b/examples/ExperimentalCases/ParallelConnectionExample/parallel_connection_example.py @@ -99,7 +99,7 @@ def apply_forces(self, system, time: np.float64 = 0.0): rod_one_direction_vec_in_material_frame, rod_two_direction_vec_in_material_frame, offset_btw_rods, -) = get_connection_vector_straight_straight_rod(rod_one, rod_two) +) = get_connection_vector_straight_straight_rod(rod_one, rod_two, (0, n_elem), (0, n_elem)) for i in range(n_elem): parallel_connection_sim.connect( diff --git a/examples/MuscularSnake/get_connection_vector.py b/examples/MuscularSnake/get_connection_vector.py deleted file mode 100644 index 30b85246..00000000 --- a/examples/MuscularSnake/get_connection_vector.py +++ /dev/null @@ -1,54 +0,0 @@ -import numpy as np -# Join the two rods -from elastica._linalg import ( - _batch_norm, - _batch_matvec, -) - - - -def get_connection_vector_straight_straight_rod( - rod_one, - rod_two, - rod_one_start_idx, - rod_one_end_idx, -): - - # Compute rod element positions - rod_one_element_position = 0.5 * ( - rod_one.position_collection[..., 1:] + rod_one.position_collection[..., :-1] - ) - rod_one_element_position = rod_one_element_position[:, rod_one_start_idx:rod_one_end_idx] - rod_two_element_position = 0.5 * ( - rod_two.position_collection[..., 1:] + rod_two.position_collection[..., :-1] - ) - - # Lets get the distance between rod elements - distance_vector_rod_one_to_rod_two = ( - rod_two_element_position - rod_one_element_position - ) - distance_vector_rod_one_to_rod_two_norm = _batch_norm( - distance_vector_rod_one_to_rod_two - ) - distance_vector_rod_one_to_rod_two /= distance_vector_rod_one_to_rod_two_norm - - distance_vector_rod_two_to_rod_one = -distance_vector_rod_one_to_rod_two - - rod_one_direction_vec_in_material_frame = _batch_matvec( - rod_one.director_collection[:,:,rod_one_start_idx:rod_one_end_idx], distance_vector_rod_one_to_rod_two - ) - rod_two_direction_vec_in_material_frame = _batch_matvec( - rod_two.director_collection, distance_vector_rod_two_to_rod_one - ) - - offset_btw_rods = distance_vector_rod_one_to_rod_two_norm - ( - rod_one.radius[rod_one_start_idx:rod_one_end_idx] + rod_two.radius - ) - - return ( - rod_one_direction_vec_in_material_frame, - rod_two_direction_vec_in_material_frame, - offset_btw_rods, - ) - - diff --git a/examples/MuscularSnake/muscular_snake.py b/examples/MuscularSnake/muscular_snake.py index da5dc437..9a0bfcbd 100644 --- a/examples/MuscularSnake/muscular_snake.py +++ b/examples/MuscularSnake/muscular_snake.py @@ -10,11 +10,10 @@ from examples.MuscularSnake.muscle_forces import MuscleForces from elastica.experimental.connection_contact_joint.parallel_connection import ( SurfaceJointSideBySide, -) -from examples.MuscularSnake.get_connection_vector import ( get_connection_vector_straight_straight_rod, ) + # Set base simulator class class MuscularSnakeSimulator( BaseSystemCollection, Constraints, Connections, Forcing, CallBacks @@ -261,8 +260,8 @@ class MuscularSnakeSimulator( ) = get_connection_vector_straight_straight_rod( rod_one, rod_two, - muscle_start_connection_index[idx], - muscle_end_connection_index[idx], + (muscle_start_connection_index[idx],muscle_end_connection_index[idx]), + (0, rod_two.n_elems) ) straight_straight_rod_connection_list.append( [ @@ -333,7 +332,7 @@ class MuscularSnakeSimulator( muscular_snake_simulator.add_forcing_to(snake_body).using( AnisotropicFrictionalPlane, k=1e1, - nu=20, + nu=40, plane_origin=origin_plane, plane_normal=normal_plane, slip_velocity_tol=slip_velocity_tol,