Ed_ddlmZmZddlmZddlmZddlmZm Z m Z ddl m Z ddl Z gdZGdd eZGd d eZGd d eZdS))ABCabstractmethod)pi)Body)Vectordynamicsymbolscross)ReferenceFrameN)JointPinJointPrismaticJointceZdZdZ ddZdZdZedZedZ edZ ed Z ed Z ed Z ed Zed ZedZedZedZedZedZedZedZdZdZdZdZdZdZdS)r u Abstract base class for all specific joints. Explanation =========== A joint subtracts degrees of freedom from a body. This is the base class for all specific joints and holds all common methods acting as an interface for all joints. Custom joint can be created by inheriting Joint class and defining all abstract functions. The abstract methods are: - ``_generate_coordinates`` - ``_generate_speeds`` - ``_orient_frames`` - ``_set_angular_velocity`` - ``_set_linar_velocity`` Parameters ========== name : string A unique name for the joint. parent : Body The parent body of joint. child : Body The child body of joint. coordinates: List of dynamicsymbols, optional Generalized coordinates of the joint. speeds : List of dynamicsymbols, optional Generalized speeds of joint. parent_joint_pos : Vector, optional Vector from the parent body's mass center to the point where the parent and child are connected. The default value is the zero vector. child_joint_pos : Vector, optional Vector from the child body's mass center to the point where the parent and child are connected. The default value is the zero vector. parent_axis : Vector, optional Axis fixed in the parent body which aligns with an axis fixed in the child body. The default is x axis in parent's reference frame. child_axis : Vector, optional Axis fixed in the child body which aligns with an axis fixed in the parent body. The default is x axis in child's reference frame. Attributes ========== name : string The joint's name. parent : Body The joint's parent body. child : Body The joint's child body. coordinates : list List of the joint's generalized coordinates. speeds : list List of the joint's generalized speeds. parent_point : Point The point fixed in the parent body that represents the joint. child_point : Point The point fixed in the child body that represents the joint. parent_axis : Vector The axis fixed in the parent frame that represents the joint. child_axis : Vector The axis fixed in the child frame that represents the joint. kdes : list Kinematical differential equations of the joint. Notes ===== The direction cosine matrix between the child and parent is formed using a simple rotation about an axis that is normal to both ``child_axis`` and ``parent_axis``. In general, the normal axis is formed by crossing the ``child_axis`` into the ``parent_axis`` except if the child and parent axes are in exactly opposite directions. In that case the rotation vector is chosen using the rules in the following table where ``C`` is the child reference frame and ``P`` is the parent reference frame: .. list-table:: :header-rows: 1 * - ``child_axis`` - ``parent_axis`` - ``rotation_axis`` * - ``-C.x`` - ``P.x`` - ``P.z`` * - ``-C.y`` - ``P.y`` - ``P.x`` * - ``-C.z`` - ``P.z`` - ``P.y`` * - ``-C.x-C.y`` - ``P.x+P.y`` - ``P.z`` * - ``-C.y-C.z`` - ``P.y+P.z`` - ``P.x`` * - ``-C.x-C.z`` - ``P.x+P.z`` - ``P.y`` * - ``-C.x-C.y-C.z`` - ``P.x+P.y+P.z`` - ``(P.x+P.y+P.z) × P.x`` Nc t|tstd||_t|tstd||_t|tstd||_|||_| ||_ | |_ | |||_| || |_||||_||||_|||dS)NzSupply a valid name.z#Parent must be an instance of Body.) isinstancestr TypeError_namer_parent_child_generate_coordinates _coordinates_generate_speeds_speeds_generate_kdes_kdes_axis _parent_axis _child_axis_locate_joint_pos _parent_point _child_point_orient_frames_set_angular_velocity_set_linear_velocity) selfnameparentchild coordinatesspeedsparent_joint_poschild_joint_pos parent_axis child_axiss =lib/python3.11/site-packages/sympy/physics/mechanics/joint.py__init__zJoint.__init__}sU$$$ 4233 3 &$'' CABB B %&& CABB B  66{CC,,V44 ((**  JJv{;;::eZ88!33FThe joint's point where parent body is connected to the joint.)r r4s r/ parent_pointzJoint.parent_points !!r1c|jS)z=The joint's point where child body is connected to the joint.)r!r4s r/ child_pointzJoint.child_pointr=r1cdS)z3Generate list generalized coordinates of the joint.N)r%r)s r/rzJoint._generate_coordinates  r1cdS)z.Generate list generalized speeds of the joint.NrH)r%r*s r/rzJoint._generate_speedsrIr1cdS)zOrient frames as per the joint.NrHr4s r/r"zJoint._orient_framesrIr1cdSr3rHr4s r/r#zJoint._set_angular_velocity r1cdSr3rHr4s r/r$zJoint._set_linear_velocityrMr1cg}tj}tt|jD]D}||j|| |j|zE|Sr3)r_trangelenr)appenddiffr*)r%r@tis r/rzJoint._generate_kdessq  s4+,,-- G GA KK)!,11!444t{1~E F F F F r1c||jj}|St|tst d||jdksd}t ||S)NzAxis must be of type Vector.rzAAxis cannot be time-varying when viewed from the associated body.)framexrrrdt ValueError)r%bodyaxmsgs r/rz Joint._axissl  BI"f%% <:;; ;uuTZ  A% "&CS// ! r1c*|td}t|tstd||jdksd}t||jdz|jzdz}|j||S)Nrz*Joint Position must be supplied as Vector.zLPosition Vector cannot be time-varying when viewed from the associated body.__joint) rrr[rZrXrr& masscenter locatenew)r%r\ joint_posr^ point_names r/rzJoint._locate_joint_poss  "q I)V,, KIJJ J||DJ''1, "*CS// !Z#% 1H< ((Y???r1cx||}t||}||fSr3) angle_betweenr normalize)r%r'r(angleaxiss r/_alignment_rotationzJoint._alignment_rotations:$$U++UF##--//d{r1cP|jj}|j|}|d|d|d}}}|dkr:|dkr |dkrt |j|jS|dkr|jS|jS|dkr!|dkr|dkr|jS|jS|jSdS)Nr)r'rXr- to_matrixr rYyz)r% parent_frame componentsrYrprqs r/_generate_vectorzJoint._generate_vectors{( %// == Q-A 1 a1 6 "!t 0a40 !1 ,000!t &#~%> ! 6 "!t &a4*'>)#~%> !  " "r1c|jj|jj|jd||j|j\}}tj5tj dt|tdks |tkr|tkr| }td}||jj|jd||jj|||cdddS dddn #1swxYwY|jjS)Nrignore)category int_frame)r(rX orient_axisr'r-rkr.warningscatch_warningsfilterwarnings UserWarningrrrtr )r%rirjrxs r/_set_orientationzJoint._set_orientation#s $$T[%68H!LLL..t/?04AA t $ & & ! !  #H{ C C C Cvayy  !ERK !B;30022D*;77 %%dj&6KKK%%dk&7uEEE  ! ! ! ! ! ! ! ! ! ! ! ! ! ! ! ! ! ! ! ! ! ! ! !{  s'B0D11D58D5NNNNNN)__name__ __module__ __qualname____doc__r0r5r7propertyr&r'r(r)r*r@r-r.rDrFrrrr"r#r$rrrrkrtr~rHr1r/r r sRkkZFJJN $$$$<XXX!!X!XX!!X!  X ""X"!!X!  ^   ^   ^   ^   ^     @ @ @ """*!!!!!r1r cNeZdZdZ d fd ZdZdZdZdZdZ d Z xZ S) r uPin (Revolute) Joint. .. image:: PinJoint.png Explanation =========== A pin joint is defined such that the joint rotation axis is fixed in both the child and parent and the location of the joint is relative to the mass center of each body. The child rotates an angle, θ, from the parent about the rotation axis and has a simple angular speed, ω, relative to the parent. The direction cosine matrix between the child and parent is formed using a simple rotation about an axis that is normal to both ``child_axis`` and ``parent_axis``, see the Notes section for a detailed explanation of this. Parameters ========== name : string A unique name for the joint. parent : Body The parent body of joint. child : Body The child body of joint. coordinates: dynamicsymbol, optional Generalized coordinates of the joint. speeds : dynamicsymbol, optional Generalized speeds of joint. parent_joint_pos : Vector, optional Vector from the parent body's mass center to the point where the parent and child are connected. The default value is the zero vector. child_joint_pos : Vector, optional Vector from the child body's mass center to the point where the parent and child are connected. The default value is the zero vector. parent_axis : Vector, optional Axis fixed in the parent body which aligns with an axis fixed in the child body. The default is x axis in parent's reference frame. child_axis : Vector, optional Axis fixed in the child body which aligns with an axis fixed in the parent body. The default is x axis in child's reference frame. Attributes ========== name : string The joint's name. parent : Body The joint's parent body. child : Body The joint's child body. coordinates : list List of the joint's generalized coordinates. speeds : list List of the joint's generalized speeds. parent_point : Point The point fixed in the parent body that represents the joint. child_point : Point The point fixed in the child body that represents the joint. parent_axis : Vector The axis fixed in the parent frame that represents the joint. child_axis : Vector The axis fixed in the child frame that represents the joint. kdes : list Kinematical differential equations of the joint. Examples ========= A single pin joint is created from two bodies and has the following basic attributes: >>> from sympy.physics.mechanics import Body, PinJoint >>> parent = Body('P') >>> parent P >>> child = Body('C') >>> child C >>> joint = PinJoint('PC', parent, child) >>> joint PinJoint: PC parent: P child: C >>> joint.name 'PC' >>> joint.parent P >>> joint.child C >>> joint.parent_point PC_P_joint >>> joint.child_point PC_C_joint >>> joint.parent_axis P_frame.x >>> joint.child_axis C_frame.x >>> joint.coordinates [theta_PC(t)] >>> joint.speeds [omega_PC(t)] >>> joint.child.frame.ang_vel_in(joint.parent.frame) omega_PC(t)*P_frame.x >>> joint.child.frame.dcm(joint.parent.frame) Matrix([ [1, 0, 0], [0, cos(theta_PC(t)), sin(theta_PC(t))], [0, -sin(theta_PC(t)), cos(theta_PC(t))]]) >>> joint.child_point.pos_from(joint.parent_point) 0 To further demonstrate the use of the pin joint, the kinematics of simple double pendulum that rotates about the Z axis of each connected body can be created as follows. >>> from sympy import symbols, trigsimp >>> from sympy.physics.mechanics import Body, PinJoint >>> l1, l2 = symbols('l1 l2') First create bodies to represent the fixed ceiling and one to represent each pendulum bob. >>> ceiling = Body('C') >>> upper_bob = Body('U') >>> lower_bob = Body('L') The first joint will connect the upper bob to the ceiling by a distance of ``l1`` and the joint axis will be about the Z axis for each body. >>> ceiling_joint = PinJoint('P1', ceiling, upper_bob, ... child_joint_pos=-l1*upper_bob.frame.x, ... parent_axis=ceiling.frame.z, ... child_axis=upper_bob.frame.z) The second joint will connect the lower bob to the upper bob by a distance of ``l2`` and the joint axis will also be about the Z axis for each body. >>> pendulum_joint = PinJoint('P2', upper_bob, lower_bob, ... child_joint_pos=-l2*lower_bob.frame.x, ... parent_axis=upper_bob.frame.z, ... child_axis=lower_bob.frame.z) Once the joints are established the kinematics of the connected bodies can be accessed. First the direction cosine matrices of pendulum link relative to the ceiling are found: >>> upper_bob.frame.dcm(ceiling.frame) Matrix([ [ cos(theta_P1(t)), sin(theta_P1(t)), 0], [-sin(theta_P1(t)), cos(theta_P1(t)), 0], [ 0, 0, 1]]) >>> trigsimp(lower_bob.frame.dcm(ceiling.frame)) Matrix([ [ cos(theta_P1(t) + theta_P2(t)), sin(theta_P1(t) + theta_P2(t)), 0], [-sin(theta_P1(t) + theta_P2(t)), cos(theta_P1(t) + theta_P2(t)), 0], [ 0, 0, 1]]) The position of the lower bob's masscenter is found with: >>> lower_bob.masscenter.pos_from(ceiling.masscenter) l1*U_frame.x + l2*L_frame.x The angular velocities of the two pendulum links can be computed with respect to the ceiling. >>> upper_bob.frame.ang_vel_in(ceiling.frame) omega_P1(t)*C_frame.z >>> lower_bob.frame.ang_vel_in(ceiling.frame) omega_P1(t)*C_frame.z + omega_P2(t)*U_frame.z And finally, the linear velocities of the two pendulum bobs can be computed with respect to the ceiling. >>> upper_bob.masscenter.vel(ceiling.frame) l1*omega_P1(t)*U_frame.y >>> lower_bob.masscenter.vel(ceiling.frame) l1*omega_P1(t)*U_frame.y + l2*(omega_P1(t) + omega_P2(t))*L_frame.y Nc Zt||||||||| dSr3superr0 r%r&r'r(r)r*r+r,r-r. __class__s r/r0zPinJoint.__init__s? vuk6)?K# % % % % %r1c6d|jd|jd|jS)Nz PinJoint: parent: child: r&r'r(r4s r/r5zPinJoint.__str__s7'TY''$+''*'' (r1cjg}|td|jz}|}|||S)Ntheta_rrrS)r% coordinater)thetas r/rzPinJoint._generate_coordinatessB  "=4:#=>>EJ:&&&r1cjg}|td|jz}|}|||S)Nomega_r)r%speedr*omegas r/rzPinJoint._generate_speedss?  "=4:#=>>EE e r1c|}|jj||j|jddSNr)r~r(rXryr-r)r%rXs r/r"zPinJoint._orient_framessL%%'' $$UD,<$($4Q$7 9 9 9 9 9r1c|jj|jj|jd|jzdSr)r(rX set_ang_velr'r*r-rhr4s r/r#zPinJoint._set_angular_velocity sU $$T[%6 A%)%5%?%?%A%A9B C C C C Cr1cN|j|jjd|j|jjd|j|jd|jj|j|jj|jjdSr) rDset_velr'rXrFr(set_posrb v2pt_theoryr4s r/r$zPinJoint._set_linear_velocitys !!$+"3Q777   !11555   !2A666 ))$*;*.+*;TZ=M O O O O Or1r rrrrr0r5rrr"r#r$ __classcell__rs@r/r r 8sqqfFJJN %%%%%%(((999 CCCOOOOOOOr1r cLeZdZdZ d fd ZdZdZdZdZdZ d Z xZ S) r aPrismatic (Sliding) Joint. .. image:: PrismaticJoint.png Explanation =========== It is defined such that the child body translates with respect to the parent body along the body fixed parent axis. The location of the joint is defined by two points in each body which coincides when the generalized coordinate is zero. The direction cosine matrix between the child and parent is formed using a simple rotation about an axis that is normal to both ``child_axis`` and ``parent_axis``, see the Notes section for a detailed explanation of this. Parameters ========== name : string A unique name for the joint. parent : Body The parent body of joint. child : Body The child body of joint. coordinates: dynamicsymbol, optional Generalized coordinates of the joint. speeds : dynamicsymbol, optional Generalized speeds of joint. parent_joint_pos : Vector, optional Vector from the parent body's mass center to the point where the parent and child are connected. The default value is the zero vector. child_joint_pos : Vector, optional Vector from the child body's mass center to the point where the parent and child are connected. The default value is the zero vector. parent_axis : Vector, optional Axis fixed in the parent body which aligns with an axis fixed in the child body. The default is x axis in parent's reference frame. child_axis : Vector, optional Axis fixed in the child body which aligns with an axis fixed in the parent body. The default is x axis in child's reference frame. Attributes ========== name : string The joint's name. parent : Body The joint's parent body. child : Body The joint's child body. coordinates : list List of the joint's generalized coordinates. speeds : list List of the joint's generalized speeds. parent_point : Point The point fixed in the parent body that represents the joint. child_point : Point The point fixed in the child body that represents the joint. parent_axis : Vector The axis fixed in the parent frame that represents the joint. child_axis : Vector The axis fixed in the child frame that represents the joint. kdes : list Kinematical differential equations of the joint. Examples ========= A single prismatic joint is created from two bodies and has the following basic attributes: >>> from sympy.physics.mechanics import Body, PrismaticJoint >>> parent = Body('P') >>> parent P >>> child = Body('C') >>> child C >>> joint = PrismaticJoint('PC', parent, child) >>> joint PrismaticJoint: PC parent: P child: C >>> joint.name 'PC' >>> joint.parent P >>> joint.child C >>> joint.parent_point PC_P_joint >>> joint.child_point PC_C_joint >>> joint.parent_axis P_frame.x >>> joint.child_axis C_frame.x >>> joint.coordinates [x_PC(t)] >>> joint.speeds [v_PC(t)] >>> joint.child.frame.ang_vel_in(joint.parent.frame) 0 >>> joint.child.frame.dcm(joint.parent.frame) Matrix([ [1, 0, 0], [0, 1, 0], [0, 0, 1]]) >>> joint.child_point.pos_from(joint.parent_point) x_PC(t)*P_frame.x To further demonstrate the use of the prismatic joint, the kinematics of two masses sliding, one moving relative to a fixed body and the other relative to the moving body. about the X axis of each connected body can be created as follows. >>> from sympy.physics.mechanics import PrismaticJoint, Body First create bodies to represent the fixed ceiling and one to represent a particle. >>> wall = Body('W') >>> Part1 = Body('P1') >>> Part2 = Body('P2') The first joint will connect the particle to the ceiling and the joint axis will be about the X axis for each body. >>> J1 = PrismaticJoint('J1', wall, Part1) The second joint will connect the second particle to the first particle and the joint axis will also be about the X axis for each body. >>> J2 = PrismaticJoint('J2', Part1, Part2) Once the joint is established the kinematics of the connected bodies can be accessed. First the direction cosine matrices of Part relative to the ceiling are found: >>> Part1.dcm(wall) Matrix([ [1, 0, 0], [0, 1, 0], [0, 0, 1]]) >>> Part2.dcm(wall) Matrix([ [1, 0, 0], [0, 1, 0], [0, 0, 1]]) The position of the particles' masscenter is found with: >>> Part1.masscenter.pos_from(wall.masscenter) x_J1(t)*W_frame.x >>> Part2.masscenter.pos_from(wall.masscenter) x_J1(t)*W_frame.x + x_J2(t)*P1_frame.x The angular velocities of the two particle links can be computed with respect to the ceiling. >>> Part1.ang_vel_in(wall) 0 >>> Part2.ang_vel_in(wall) 0 And finally, the linear velocities of the two particles can be computed with respect to the ceiling. >>> Part1.masscenter_vel(wall) v_J1(t)*W_frame.x >>> Part2.masscenter.vel(wall.frame) v_J1(t)*W_frame.x + Derivative(x_J2(t), t)*P1_frame.x Nc Zt||||||||| dSr3rrs r/r0zPrismaticJoint.__init__sD vuk6CS'j B B B B Br1c6d|jd|jd|jS)NzPrismaticJoint: rrrr4s r/r5zPrismaticJoint.__str__s7'49'' ''*'' (r1cjg}|td|jz}|}|||S)Nx_r)r%rr)rYs r/rz$PrismaticJoint._generate_coordinatessB  y4:566AJ:&&&r1cjg}|td|jz}|}|||S)Nv_r)r%rr*rps r/rzPrismaticJoint._generate_speedss?  y4:566AE e r1cz|}|jj||jddSr)r~r(rXryr-rs r/r"zPrismaticJoint._orient_framess9%%'' $$UD,rs=$#######!!!!!!------>>>>>>>>>>555555 1 1 1f!f!f!f!f!Cf!f!f!R ^O^O^O^O^Ou^O^O^OBVhVhVhVhVhUVhVhVhVhVhr1