1+ # -*- coding: utf-8 -*-
2+ """
3+ Created on Mon May 5 16:11:43 2025
4+
5+ @author: adiazfl
6+ """
7+ import sys
8+ import os
9+
10+ # Add the source and examples directory to the path to run examples
11+ source_dir = os .path .abspath (os .path .join (os .path .dirname (__file__ ), '..' ))
12+ sys .path .insert (0 , source_dir )
13+
14+ # External libraries
15+ import numpy as np
16+ import sympy as sym
17+
18+ # This module libraries
19+ import multibody as mb
20+ from multibody import JointSystem , normalize_prismatic
21+ from multibody import symvars_definition
22+
23+ # Do not modify
24+ Initial_Points = {}
25+ Force = {}
26+ # End of Do not Modify
27+
28+ # Example 4
29+ Reference_frame_Origin = np .array ([0 ,0.91 ])
30+
31+ # Bodies definition
32+ dataNames = [] # Define symbolic variables for force points (for example, b1, b2, b3).
33+ BodyDataSym = symvars_definition (dataNames ,globals ()) # DO NOT MODIFY
34+
35+
36+ joints = [[0 , 1 ],[1 , 2 ],[1 ,3 ]] # Joint connectivity: [parent, child]
37+ types = ['F' ,'R' , 'R' ] # Joint types: 'R' for revolute, 'P' for prismatic, 'F' for floating
38+ parent_cg_to_joint = [[0 , 0 ],[0.64 , 0 ],[- 0.64 , 0 ]] # Vectors from parent's center-of-gravity (CG) to the joint location.
39+ joint_to_child_cg = [[np .nan , np .nan ],[0 , 0.17 ],[0 , 0.17 ]] # Vectors from the joint to the child's CG.
40+ prismatic_direction = [[np .nan , np .nan ],[np .nan , np .nan ],[np .nan , np .nan ]] # For prismatic joints, the direction vector; for others, [nan, nan] is used.
41+ prismatic_direction = normalize_prismatic (prismatic_direction ) # DO NOT MODIFY
42+
43+ # Points definition
44+ PointNames = [] # Define symbolic variables for force points (for example, b1, b2, b3).
45+ PointsSym = symvars_definition (PointNames ,globals ()) # DO NOT MODIFY
46+
47+ # Define the structure for initial points.
48+ # Ground points: these are fixed and given as [x, z] coordinates.
49+ Initial_Points ["GR" ] = [[0.75 , - 0.91 ],[- 0.75 , - 0.91 ]]
50+
51+ # Body-defined points ("BD"): we use a dictionary where each key (an integer)
52+ # corresponds to a body and the value is a list of points.
53+ Initial_Points ["BD" ] = {}
54+ Initial_Points ["BD" ][1 ] = [[0.75 , 0 ],[- 0.75 , 0 ]]
55+
56+ # Forces definition
57+ ForceNames = ['t' ,'k' ] # Define symbolic variables for force points (for example, b1, b2, b3).
58+ ForceSym = symvars_definition (ForceNames ,globals ()) # DO NOT MODIFY
59+
60+ # Define the structure for Force.
61+ # Forces applied on body-defined points (PointsBD):
62+ Force ["PointsBD" ] = []
63+
64+ # Forces applied at the center of gravity (CG):
65+ Force ["CG" ] = [[1 ,0 ,0.1 * sym .exp (- t ),0 ]]
66+
67+ # Tension springs: stored as a list of tuples: (connection, [l0, stiffness]).
68+ Force ["TensionSpring" ] = [(('GR00' ,'BD10' ),[5 ,k ]),
69+ (('GR01' ,'BD11' ),[5 ,k ])
70+ ]
71+
72+ # Tension dampers: here we have two pairs.
73+ # The first pair uses a constant damping coefficient.
74+ # The second uses a lambda function to represent a nonlinear damping coefficient.
75+ Force ["TensionDamper" ] = [(('GR00' ,'BD10' ),1 ),
76+ (('GR01' ,'BD11' ),1 )]
77+
78+ # Torsion springs: here the first element is a list of parameters and the second element is a lambda.
79+ Force ["TorsionSpring" ] = [((1 ,2 ),[0 ,10 ]),
80+ ((1 ,3 ),[0 ,10 ])]
81+
82+ # Torsion dampers:
83+ Force ["TorsionDamper" ] = [((1 ,2 ),0 ),
84+ ((1 ,3 ),0 )]
85+
86+ ForcesPointsSym = PointsSym + ForceSym
87+
88+ # Initial conditions
89+ # Create the JointSystem using the from_data class method.
90+ joint_system = JointSystem .from_data (joints , types , parent_cg_to_joint , joint_to_child_cg , prismatic_direction ) # DO NOT MODIFY
91+ Q , QD , _ , NDOF , _ = joint_system .coordinate_finder () # DO NOT MODIFY
92+
93+ # If you are unsure what are the systems DOF run the example and you will see on screen
94+ ic = np .zeros (2 * sum (NDOF )) # Multiplied by 2 because is position and velocity
95+
96+ # Parameters to loop over
97+ ForcesPointsNum = np .ones (len (ForcesPointsSym ))
98+ BodyDataNum = np .ones (len (BodyDataSym ))
99+
100+ # Simulation
101+ TimeStep = 0.05
102+ tspan = 10.
103+ g = 0. # gravity
104+ gVec = np .ones ((len (types ),1 )) # Percentage of gravity acting on each body
105+ m0 = np .ones (len (types ))
106+ J0 = 2. * np .ones (len (types ))
107+
108+ # Animation
109+ animation_on = 1 # Display animation:1, 0-otherwise
110+ SaveMovieOn = None # Extension:gif Saves animation if given a string as name, otherwise set as None
111+ plotTstep = 5 # how many times faster are we plotting w.r.t to simulation time
112+
113+ ###########################################################################
114+ ######################### END OF USER DEFINITION ##########################
115+ ###########################################################################
116+
117+ #%% Checks section: ensures everything is defined correctly
118+ mb .checks .bodies (joints , types , parent_cg_to_joint , joint_to_child_cg , prismatic_direction )
119+ mb .checks .points_forces (joints , types , Initial_Points , Force )
120+
121+ NBodies = len (joints )
122+
123+ if len (m0 ) != NBodies or len (J0 ) != NBodies or not np .all (m0 ) or not np .all (J0 ):
124+ raise ValueError ("The mass matrix has zero-valued entries check m0 and J0" )
125+
126+ '''
127+ The following sections are for user visualization but unnecessary for the
128+ main to run. Uncomment if you want to check how your bodies tables is defines
129+ or if unsure on what the initial conditions are and what is the order
130+ '''
131+ #%% Display tables
132+ # Print the joint system for inspection.
133+ print ('\n Bodies and joints table:' )
134+ joint_system .display_table ()
135+
136+ # Display points table
137+ print ('\n Points table:' )
138+ mb .tables .points_table (Initial_Points )
139+ print ('\n Forces table:' )
140+ mb .tables .force_table (Force )
141+
142+ #%% Problem variables and required initial conditions
143+ ic_vars = Q + QD
144+
145+ print ('\n The variables that require initial conditions, in this order, are:\n ' + str (ic_vars ) + '\n ' )
146+
147+ #%% Documentation
148+ '''
149+ Bodies data: defining the joint and body configuration
150+ data.Joints: List where each element represents a joint [Parent, Child]
151+ data.Type: List specifying joint types ('F' for floating, 'R' for revolute, 'P' for prismatic)
152+ data.ParentCGtoJoint: List containing vectors from parent CG to joint location
153+ data.JointtoChildCG: List containing vectors from joint to child CG
154+ data.prismatic_direction: Unit vectors for prismatic joints, use [nan,nan] if NO prismatic joint
155+
156+ Points data: dictionary
157+ You can use the variables in PointNames to define points location, Ex: PointNames = ["b1"]
158+ Initial points of the system, categorized by grounded points (GR) and body points (BD)
159+ DEFINE THESE EMPTY IF NO POINTS ARE REQUIRED, Ex: Initial_Points['GR'] = []
160+ Initial_Points["GR"] = [[20, 0], [b1, 0]]
161+ Initial_Points["BD"] is a DICTIONARY where each entry is the body number they belong to
162+ Initial_Points["BD"][i], 'i', is the body and the rest is defined as 'GR' points
163+
164+ Forces definition: dictionary
165+ You can use the variables in ForceNames to define points location, Ex: ForceNames = ["f1"]
166+ DEFINE ALL FIELDS, if empty use: Force['PointsBD ']= []
167+ Force.PointsBD = list of lists: [[Initial_points.BD(i),Initial_points.BD(j),Fx,Fy,Mz]]
168+ Force.CG = list of lists: [[Initial_points.BD(i),Initial_points.BD(j),Fx,Fy,Mz]]
169+ Force.TensionSpring = list of tuples: [(('Point1','Point2'),[L_0,k])]
170+ Force.TensionDamper = list of tuples: [(('Point1','Point2'),C_0)]
171+ Force.TorsionSpring = list of tuples: [(('Body1','Body2'),[theta_0,k])]
172+ Force.TorsionDamper = list of tuples: [(('Body1','Body2'),C_0)]
173+ LEGEND:
174+ Fx,Fy,Mz : force components
175+ L_0: spring undeformed length
176+ k: spring constant
177+ C_0: damping coefficient
178+
179+ Numerical inputs for simulation
180+ m0: column vector with the mass of each body
181+ J0: column vector with the inertia of each body
182+ gVec: column vector of 0s and 1s, with 0 indicating if a body is not
183+ subject to gravity (let's say it's floating)
184+ '''
0 commit comments