diff --git a/elastica/joint.py b/elastica/joint.py index 7b589a122..6cd15a9e8 100644 --- a/elastica/joint.py +++ b/elastica/joint.py @@ -377,6 +377,8 @@ def _calculate_contact_forces_rod_rigid_body( velocity_cylinder, contact_k, contact_nu, + velocity_damping_coefficient, + friction_coefficient, ): # We already pass in only the first n_elem x n_points = x_collection_rod.shape[1] @@ -425,19 +427,44 @@ def _calculate_contact_forces_rod_rigid_body( # Compute contact spring force contact_force = contact_k * gamma * distance_vector - interpenetration_velocity = ( - 0.5 * (velocity_rod[..., i] + velocity_rod[..., i + 1]) - - velocity_cylinder[..., 0] + interpenetration_velocity = velocity_cylinder[..., 0] - 0.5 * ( + velocity_rod[..., i] + velocity_rod[..., i + 1] ) # Compute contact damping normal_interpenetration_velocity = ( _dot_product(interpenetration_velocity, distance_vector) * distance_vector ) - contact_damping_force = contact_nu * normal_interpenetration_velocity + contact_damping_force = -contact_nu * normal_interpenetration_velocity # magnitude* direction net_contact_force = 0.5 * mask * (contact_damping_force + contact_force) + # Compute friction + slip_interpenetration_velocity = ( + interpenetration_velocity - normal_interpenetration_velocity + ) + slip_interpenetration_velocity_mag = np.linalg.norm( + slip_interpenetration_velocity + ) + slip_interpenetration_velocity_unitized = slip_interpenetration_velocity / ( + slip_interpenetration_velocity_mag + 1e-14 + ) + # Compute friction force in the slip direction. + damping_force_in_slip_direction = ( + velocity_damping_coefficient * slip_interpenetration_velocity_mag + ) + # Compute Coulombic friction + coulombic_friction_force = friction_coefficient * np.linalg.norm( + net_contact_force + ) + # Compare damping force in slip direction and kinetic friction and minimum is the friction force. + friction_force = ( + -min(damping_force_in_slip_direction, coulombic_friction_force) + * slip_interpenetration_velocity_unitized + ) + # Update contact force + net_contact_force += friction_force + # Torques acting on the cylinder moment_arm = x_cylinder_contact_point - x_cylinder_center @@ -771,20 +798,68 @@ def _prune_using_aabbs_rod_rod( class ExternalContact(FreeJoint): """ - Assumes that the second entity is a rigid body for now, can be - changed at a later time + This class is for applying contact forces between rod-cylinder and rod-rod. + If you are want to apply contact forces between rod and cylinder, first system is always rod and second system + is always cylinder. + In addition to the contact forces, user can define apply friction forces between rod and cylinder that + are in contact. For details on friction model refer to this `paper `_. + TODO: Currently friction force is between rod-cylinder, in future implement friction forces between rod-rod. - Most of the cylinder-cylinder contact SHOULD be implemented - as given in this `paper `_. + Notes + ----- + The `velocity_damping_coefficient` is set to a high value (e.g. 1e4) to minimize slip and simulate stiction + (static friction), while friction_coefficient corresponds to the Coulombic friction coefficient. + + Examples + -------- + How to define contact between rod and cylinder. + + >>> simulator.connect(rod, cylidner).using( + ... ExternalContact, + ... k=1e4, + ... nu=10, + ... velocity_damping_coefficient=10, + ... kinetic_friction_coefficient=10, + ... ) - but, it isn't (the elastica-cpp kernels are implented)! + How to define contact between rod and rod. + + >>> simulator.connect(rod, rod).using( + ... ExternalContact, + ... k=1e4, + ... nu=10, + ... ) + + + Developer Note + -------------- + Most of the cylinder-cylinder contact SHOULD be implemented + as given in this `paper `, + but the elastica-cpp kernels are implemented. This is maybe to speed-up the kernel, but it's potentially dangerous as it does not deal with "end" conditions correctly. + """ - def __init__(self, k, nu): + def __init__(self, k, nu, velocity_damping_coefficient=0, friction_coefficient=0): + """ + + Parameters + ---------- + k : float + Contact spring constant. + nu : float + Contact damping constant. + velocity_damping_coefficient : float + Velocity damping coefficient between rigid-body and rod contact is used to apply friction force in the + slip direction. + friction_coefficient : float + For Coulombic friction coefficient for rigid-body and rod contact. + """ super().__init__(k, nu) + self.velocity_damping_coefficient = velocity_damping_coefficient + self.friction_coefficient = friction_coefficient def apply_forces(self, rod_one, index_one, rod_two, index_two): # del index_one, index_two @@ -833,6 +908,8 @@ def apply_forces(self, rod_one, index_one, rod_two, index_two): cylinder_two.velocity_collection, self.k, self.nu, + self.velocity_damping_coefficient, + self.friction_coefficient, ) else: diff --git a/examples/RigidBodyCases/RodRigidBodyContact/post_processing.py b/examples/RigidBodyCases/RodRigidBodyContact/post_processing.py index 14b81311c..cc0899264 100644 --- a/examples/RigidBodyCases/RodRigidBodyContact/post_processing.py +++ b/examples/RigidBodyCases/RodRigidBodyContact/post_processing.py @@ -725,3 +725,35 @@ def plot_video_with_surface( # plt.close(fig) alone does not suffice # See /~https://github.com/matplotlib/matplotlib/issues/8560/ plt.close(plt.gcf()) + + +def plot_force_vs_energy( + normalized_force, + total_final_energy, + friction_coefficient, + filename="energy_vs_force.png", + SAVE_FIGURE=False, +): + + fig = plt.figure(figsize=(12, 10), frameon=True, dpi=150) + axs = [] + axs.append(plt.subplot2grid((1, 1), (0, 0))) + + axs[0].plot( + normalized_force, + total_final_energy, + linewidth=3, + ) + plt.axvline(x=friction_coefficient, linewidth=3, color="r", label="threshold") + axs[0].set_ylabel("total energy", fontsize=20) + axs[0].set_xlabel("normalized force", fontsize=20) + + plt.tight_layout() + # fig.align_ylabels() + fig.legend(prop={"size": 20}) + # fig.savefig(filename) + # plt.show() + plt.close(plt.gcf()) + + if SAVE_FIGURE: + fig.savefig(filename) diff --git a/examples/RigidBodyCases/RodRigidBodyContact/rod_cylinder_contact_friction.py b/examples/RigidBodyCases/RodRigidBodyContact/rod_cylinder_contact_friction.py new file mode 100644 index 000000000..7ad3e5691 --- /dev/null +++ b/examples/RigidBodyCases/RodRigidBodyContact/rod_cylinder_contact_friction.py @@ -0,0 +1,263 @@ +import numpy as np +import sys + +sys.path.append("../../../") +from elastica import * +from post_processing import plot_velocity, plot_video_with_surface + + +def rod_cylinder_contact_friction_case( + force_coefficient=0.1, normal_force_mag=10, POST_PROCESSING=False +): + class RodCylinderParallelContact( + BaseSystemCollection, Constraints, Connections, CallBacks, Forcing + ): + pass + + rod_cylinder_parallel_contact_simulator = RodCylinderParallelContact() + + # time step etc + final_time = 20.0 + time_step = 1e-4 + total_steps = int(final_time / time_step) + 1 + rendering_fps = 30 # 20 * 1e1 + step_skip = int(1.0 / (rendering_fps * time_step)) + + base_length = 0.5 + base_radius = 0.1 + density = 1750 + E = 3e5 + poisson_ratio = 0.5 + shear_modulus = E / (2 * (1 + poisson_ratio)) + n_elem = 50 + nu = 0.5 + start = np.zeros((3,)) + direction = np.array([0, 0.0, 1.0]) + normal = np.array([0.0, 1.0, 0.0]) + + rod = CosseratRod.straight_rod( + n_elem, + start, + direction, + normal, + base_length, + base_radius, + density, + nu, + E, + shear_modulus=shear_modulus, + ) + + rod_cylinder_parallel_contact_simulator.append(rod) + + # Push the rod towards the cylinder to make sure contact is there + normal_force_direction = np.array([-1.0, 0.0, 0.0]) + rod_cylinder_parallel_contact_simulator.add_forcing_to(rod).using( + UniformForces, force=normal_force_mag, direction=normal_force_direction + ) + # Apply uniform forces on the rod + rod_cylinder_parallel_contact_simulator.add_forcing_to(rod).using( + UniformForces, force=normal_force_mag * force_coefficient, direction=direction + ) + + cylinder_height = 8 * base_length + cylinder_radius = base_radius + + cylinder_start = start + np.array([-1.0, 0.0, 0.0]) * 2 * base_radius + cylinder_direction = np.array([0.0, 0.0, 1.0]) + cylinder_normal = np.array([0.0, 1.0, 0.0]) + + rigid_body = Cylinder( + start=cylinder_start, + direction=cylinder_direction, + normal=cylinder_normal, + base_length=cylinder_height, + base_radius=cylinder_radius, + density=density, + ) + rod_cylinder_parallel_contact_simulator.append(rigid_body) + + # Constrain the rigid body position and directors + rod_cylinder_parallel_contact_simulator.constrain(rigid_body).using( + OneEndFixedBC, constrained_position_idx=(0,), constrained_director_idx=(0,) + ) + + # Add contact between rigid body and rod + rod_cylinder_parallel_contact_simulator.connect(rod, rigid_body).using( + ExternalContact, + k=1e5, + nu=100, + velocity_damping_coefficient=1e5, + friction_coefficient=0.5, + ) + + # Add callbacks + post_processing_dict_list = [] + # For rod + class StraightRodCallBack(CallBackBaseClass): + """ + Call back function for two arm octopus + """ + + 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["radius"].append(system.radius.copy()) + self.callback_params["com"].append( + system.compute_position_center_of_mass() + ) + if current_step == 0: + self.callback_params["lengths"].append(system.rest_lengths.copy()) + else: + self.callback_params["lengths"].append(system.lengths.copy()) + + self.callback_params["com_velocity"].append( + system.compute_velocity_center_of_mass() + ) + + total_energy = ( + system.compute_translational_energy() + + system.compute_rotational_energy() + + system.compute_bending_energy() + + system.compute_shear_energy() + ) + self.callback_params["total_energy"].append(total_energy) + + return + + class RigidCylinderCallBack(CallBackBaseClass): + """ + Call back function for two arm octopus + """ + + def __init__( + self, step_skip: int, callback_params: dict, resize_cylinder_elems: int + ): + CallBackBaseClass.__init__(self) + self.every = step_skip + self.callback_params = callback_params + self.n_elem_cylinder = resize_cylinder_elems + self.n_node_cylinder = self.n_elem_cylinder + 1 + + 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) + + cylinder_center_position = system.position_collection + cylinder_length = system.length + cylinder_direction = system.director_collection[2, :, :].reshape(3, 1) + cylinder_radius = system.radius + + # Expand cylinder data. Create multiple points on cylinder later to use for rendering. + + start_position = ( + cylinder_center_position - cylinder_length / 2 * cylinder_direction + ) + + cylinder_position_collection = ( + start_position + + np.linspace(0, cylinder_length[0], self.n_node_cylinder) + * cylinder_direction + ) + cylinder_radius_collection = ( + np.ones((self.n_elem_cylinder)) * cylinder_radius + ) + cylinder_length_collection = ( + np.ones((self.n_elem_cylinder)) * cylinder_length + ) + cylinder_velocity_collection = ( + np.ones((self.n_node_cylinder)) * system.velocity_collection + ) + + self.callback_params["position"].append( + cylinder_position_collection.copy() + ) + self.callback_params["velocity"].append( + cylinder_velocity_collection.copy() + ) + self.callback_params["radius"].append(cylinder_radius_collection.copy()) + self.callback_params["com"].append( + system.compute_position_center_of_mass() + ) + + self.callback_params["lengths"].append( + cylinder_length_collection.copy() + ) + self.callback_params["com_velocity"].append( + system.velocity_collection[..., 0].copy() + ) + + total_energy = ( + system.compute_translational_energy() + + system.compute_rotational_energy() + ) + self.callback_params["total_energy"].append(total_energy[..., 0].copy()) + + return + + if POST_PROCESSING: + post_processing_dict_list.append(defaultdict(list)) + rod_cylinder_parallel_contact_simulator.collect_diagnostics(rod).using( + StraightRodCallBack, + step_skip=step_skip, + callback_params=post_processing_dict_list[0], + ) + # For rigid body + post_processing_dict_list.append(defaultdict(list)) + rod_cylinder_parallel_contact_simulator.collect_diagnostics(rigid_body).using( + RigidCylinderCallBack, + step_skip=step_skip, + callback_params=post_processing_dict_list[1], + resize_cylinder_elems=n_elem, + ) + + rod_cylinder_parallel_contact_simulator.finalize() + timestepper = PositionVerlet() + + integrate( + timestepper, rod_cylinder_parallel_contact_simulator, final_time, total_steps + ) + + if POST_PROCESSING: + # Plot the rods + plot_video_with_surface( + post_processing_dict_list, + video_name="rod_cylinder_contact.mp4", + fps=rendering_fps, + step=1, + # The following parameters are optional + x_limits=(-base_length * 5, base_length * 5), # Set bounds on x-axis + y_limits=(-base_length * 5, base_length * 5), # Set bounds on y-axis + z_limits=(-base_length * 5, base_length * 5), # 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 + ) + + filaname = "rod_rigid_velocity.png" + plot_velocity( + post_processing_dict_list[0], + post_processing_dict_list[1], + filename=filaname, + SAVE_FIGURE=True, + ) + + # Compute final total energy + total_final_energy = ( + rod.compute_translational_energy() + + rod.compute_rotational_energy() + + rod.compute_bending_energy() + + rod.compute_shear_energy() + ) + + return total_final_energy diff --git a/examples/RigidBodyCases/RodRigidBodyContact/rod_cylinder_contact_friction_case.py b/examples/RigidBodyCases/RodRigidBodyContact/rod_cylinder_contact_friction_case.py new file mode 100644 index 000000000..c8dfe2516 --- /dev/null +++ b/examples/RigidBodyCases/RodRigidBodyContact/rod_cylinder_contact_friction_case.py @@ -0,0 +1,8 @@ +if __name__ == "__main__": + from examples.RigidBodyCases.RodRigidBodyContact.rod_cylinder_contact_friction import ( + rod_cylinder_contact_friction_case, + ) + + total_energy = rod_cylinder_contact_friction_case( + force_coefficient=0.1, normal_force_mag=10, POST_PROCESSING=True + ) diff --git a/examples/RigidBodyCases/RodRigidBodyContact/rod_cylinder_contact_friction_phase_space.py b/examples/RigidBodyCases/RodRigidBodyContact/rod_cylinder_contact_friction_phase_space.py new file mode 100644 index 000000000..7b7ae1d13 --- /dev/null +++ b/examples/RigidBodyCases/RodRigidBodyContact/rod_cylinder_contact_friction_phase_space.py @@ -0,0 +1,25 @@ +if __name__ == "__main__": + import multiprocessing as mp + from examples.RigidBodyCases.RodRigidBodyContact.rod_cylinder_contact_friction import ( + rod_cylinder_contact_friction_case, + ) + from examples.RigidBodyCases.RodRigidBodyContact.post_processing import ( + plot_force_vs_energy, + ) + + # total_energy = rod_cylinder_contact_friction_case(friction_coefficient=1.0, normal_force_mag=10, velocity_damping_coefficient=1E4, POST_PROCESSING=True) + + friction_coefficient = list([(float(x)) / 100.0 for x in range(0, 100, 5)]) + + with mp.Pool(mp.cpu_count()) as pool: + result_total_energy = pool.map( + rod_cylinder_contact_friction_case, friction_coefficient + ) + + plot_force_vs_energy( + friction_coefficient, + result_total_energy, + friction_coefficient=0.5, + filename="rod_energy_vs_force.png", + SAVE_FIGURE=True, + )