Symbolically Applying Forces and Constraints in 3d

[shoul...@gmail.com]

Hello all,

I am attempting to model a complex multi-body dynamic system. The system has two bodies connected by links. Each body is undergoing loading. I need to constrain the distance between points on each body to always equal the length of the links. Then I need to model the force interaction between the two bodies due to the links. What would be the correct way of modeling the forces and constraints?

Also, I couldn't find any forum policies to see if it is appropriate or not (if not I apologize), but I would like to find someone to work with on my project. I am attempting to use Python/Sypmy to create a vehicle simulation tool. If you are familiar with vehicle dynamics and good with python and sympy I would be interested in discussing an arrangement. You would be compensated for your time. Please email me to start a discussion on this. shoul...@gmail.com

The following code is a portion of the my code to show how I am setting up the system. 

I need help constraining the Hsg rigid body to the Chassis rigid body via the LUB. The LUB has a length of LUB_L and goes betwen joints LUBB and LUBC. The Hsg and Chassis both have external forces applied to them, so there will be a force acting along the LUB to the two joints.

from __future__ import print_function, division

from sympy import symbols, simplify

from sympy.physics.mechanics import dynamicsymbols, ReferenceFrame, Point, inertia, RigidBody

# SymPy has a rich printing system. Here we initialize printing so that all of the mathematical equations are rendered in standard mathematical notation.

from sympy.physics.vector import init_vprinting

init_vprinting(use_latex='mathjax', pretty_print=False)

#___________________________________________________________________________________________________________________________________________________

# Reference Frames and Orientation

inertial_frame = ReferenceFrame('inertial_frame')

Chassis_frame = ReferenceFrame('Chassis_frame')

Hsg_frame = ReferenceFrame('Hsg_frame')

LBC_frame = ReferenceFrame('LBC_frame')

LUB_frame = ReferenceFrame('LUB_frame')

#___________________________________________________________________________________________________________________________________________________

# Define the rotation angle symbols

thRoll, thPitch, thYaw = dynamicsymbols('thRoll, thPitch, thYaw')

thHsgx, thHsgy, thHsgz = dynamicsymbols('thHsgx, thHsgy, thHsgz')

thLBCy, thRBCy = dynamicsymbols('thLBCy, thRBCy')

thLUBx, thLUBy, thLUBz = dynamicsymbols('thLUBx, thLUBy, thLUBz')

#___________________________________________________________________________________________________________________________________________________

# Orient the reference frames

Chassis_frame.orient(inertial_frame, 'Body', [thRoll,thPitch,thYaw],'XYZ')

Hsg_frame.orient(inertial_frame,'Body',[thHsgx,thHsgy,thHsgz],'XYZ')

LBC_frame.orient(Hsg_frame,'Axis',(thLBCy,Hsg_frame.y))

LUB_frame.orient(Chassis_frame,'Body',[thLUBx,thLUBy,thLUBz],'XYZ')

# ___________________________________________________________________________________________________________________________

# Point and Location

# Joints

CGpos = Point('CGpos') # CG position

GlobalCoord0 = Point('GlobalCoord0') # Global coordinate system origin location

ChassisC = Point('ChassisC') # Chassis Center, x=0, y=0 on global coordinate system

HSGC = Point('HSGC') # Housing center

LBCC = Point('LBCC') # Birdcage centers

LUBC = Point('LUBC') # Four bar positions at chassis

LUBB = Point('LUBB') # Four bar positions on birdcage

#___________________________________________________________________________________________________________________________________________________

# Define Component Lengths

LUB_L = symbols('LUB_L') # Four bar lengths

#___________________________________________________________________________________________________________________________________________________

# Define Position Components

ChassisDeltax = symbols('ChassisDeltax') # Chassis delta amounts from global origin

ChassisDeltay = symbols('ChassisDeltay')

ChassisDeltaz = symbols('ChassisDeltaz')

CGposx = symbols('CGposx') # CG positions

CGposy = symbols('CGposy')

CGposz = symbols('CGposz')

HSGCx = symbols('HSGCx') # Housing center position

HSGCy = symbols('HSGCy')

HSGCz = symbols('HSGCz')

LBCOffy = symbols('LBCOffy') # Birdcage center offsets

LBCOff_UBx = symbols('LBCOff_UBx') # Birdcage offsets

LBCOff_UBy = symbols('LBCOff_UBy')

LBCOff_UBz = symbols('LBCOff_UBz')

LUBCx = symbols('LUBCx') # Four bar positions at chassis

LUBCy = symbols('LUBCy')

LUBCz = symbols('LUBCz')

#___________________________________________________________________________________________________________________________________________________

# Set Chassis Positions

ChassisC.set_pos(GlobalCoord0,(ChassisDeltax * inertial_frame.x + ChassisDeltay * inertial_frame.y + ChassisDeltaz * inertial_frame.z)) # Chassis 

CGpos.set_pos(ChassisC,(CGposx * Chassis_frame.x + CGposy * Chassis_frame.y + CGposz * Chassis_frame.z)) # CG Position

LUBC.set_pos(ChassisC,(LUBCx * Chassis_frame.x + LUBCy * Chassis_frame.y + LUBCz * Chassis_frame.z)) # Four bar chassis positions

HSGC.set_pos(GlobalCoord0,(HSGCx * inertial_frame.x + HSGCy * inertial_frame.y + HSGCz * inertial_frame.z)) # Housing center position

LBCC.set_pos(HSGC,(0 * Hsg_frame.x + LBCOffy * Hsg_frame.y + 0 * Hsg_frame.z)) # Birdcage center positions

LUBB.set_pos(LBCC,(LBCOff_UBx * LBC_frame.x + LBCOff_UBy * LBC_frame.y + LBCOff_UBz * LBC_frame.z)) # Four bar birdcage positions

#____________________________________________________________________________________________________________________________

# Center of Mass Locations

CoM_Chassis = Point('CoM_Chassis')

CoM_Chassis.set_pos(CGpos,0)

CoM_Hsg = Point('CoM_Hsg')

CoM_Hsg.set_pos(HSGC, (0 * Hsg_frame.x + 0 * Hsg_frame.y + 0 * Hsg_frame.z))

CoM_LBC = Point('CoM_LBC')

CoM_LBC.set_pos(LBCC, (0 * LBC_frame.x + 0 * LBC_frame.y + 0 * LBC_frame.z))

CoM_LUB = Point('CoM_LUB')

CoM_LUB.set_pos(LUBC, ((LUB_L / 2) * LUB_frame.x + 0 * LUB_frame.y + 0 * LUB_frame.z))

#_________________________________________________________________________________________________________________________

# Kinematic Differential Equations

wRoll, wPitch, wYaw = dynamicsymbols('wRoll, wPitch, wYaw')

wHsgx, wHsgy, wHsgz = dynamicsymbols('wHsgx, wHsgy, wHsgz')

wLBCy = dynamicsymbols('wLBCy')

wLUBx, wLUBy, wLUBz = dynamicsymbols('wLUBx, wLUBy, wLUBz')

kinematical_differential_equations = [wRoll - thRoll.diff(),wPitch - thPitch.diff(),wYaw - thYaw.diff(),

                                      wHsgx - thHsgx.diff(),wHsgy - thHsgy.diff(),wHsgz - thHsgz.diff(),

                                      wLBCy - thLBCy.diff(),wLUBx - thLUBx.diff(),wLUBy - thLUBy.diff(),wLUBz - thLUBz.diff()]

#_____________________________________________________________________________________________________________________________

# Angular Velocities

Chassis_frame.set_ang_vel(inertial_frame,(wRoll * Chassis_frame.x + wPitch * Chassis_frame.y + wYaw * Chassis_frame.z))

Hsg_frame.set_ang_vel(inertial_frame,(wHsgx * Hsg_frame.x + wHsgy * Hsg_frame.y + wHsgz * Hsg_frame.z))

LBC_frame.set_ang_vel(Hsg_frame,(wLBCy * LBC_frame.y))

LUB_frame.set_ang_vel(Chassis_frame,(wLUBx * LUB_frame.x + wLUBy * LUB_frame.y + wLUBz * LUB_frame.z))

#_____________________________________________________________________________________________________________________________

# Linear Velocities

GlobalCoord0.set_vel(inertial_frame,0)

CoM_Chassis.v2pt_theory(GlobalCoord0,inertial_frame,Chassis_frame)

CoM_Hsg.v2pt_theory(GlobalCoord0,inertial_frame,Hsg_frame)

CoM_LBC.v2pt_theory(GlobalCoord0,inertial_frame,LBC_frame)

CoM_LUB.v2pt_theory(GlobalCoord0,inertial_frame,LUB_frame)

# ______________________________________________________________________________________________________________________________

# Define inertias

m_Chassis = symbols('m_Chassis')

m_Hsg = symbols('m_Hsg')

m_LBC = symbols('m_LBC')

m_LUB = symbols('m_LUB')

# Define inertia symbols

Chassis_Ixx, Chassis_Iyy, Chassis_Izz = symbols('Chassis_Ixx, Chassis_Iyy, Chassis_Izz')

Chassis_Ixy, Chassis_Iyz, Chassis_Izx = symbols('Chassis_Ixy, Chassis_Iyz, Chassis_Izx')

Hsg_Ixx, Hsg_Iyy, Hsg_Izz = symbols('Hsg_Ixx, Hsg_Iyy, Hsg_Izz')

LBC_Ixx, LBC_Iyy, LBC_Izz = symbols('LBC_Ixx, LBC_Iyy, LBC_Izz')

LUB_Ixx, LUB_Iyy, LUB_Izz = symbols('LUB_Ixx, LUB_Iyy, LUB_Izz')

# Define dyadics

Chassis_dyadic = inertia(Chassis_frame,Chassis_Ixx,Chassis_Iyy,Chassis_Izz,Chassis_Ixy,Chassis_Iyz,Chassis_Izx)

Hsg_dyadic = inertia(Hsg_frame,Hsg_Ixx,Hsg_Iyy,Hsg_Izz)

LBC_dyadic = inertia(LBC_frame,LBC_Ixx,LBC_Iyy,LBC_Izz)

LUB_dyadic = inertia(LUB_frame,LUB_Ixx,LUB_Iyy,LUB_Izz)

# Central Inertias

Chassis_central_inertia = (Chassis_dyadic,CoM_Chassis)

Hsg_central_inertia = (Hsg_dyadic,CoM_Hsg)

LBC_central_inertia = (LBC_dyadic,CoM_LBC)

LUB_central_inertia = (LUB_dyadic,CoM_LUB)

# Rigid Bodies

Chassis = RigidBody('Chassis',CoM_Chassis,Chassis_frame,m_Chassis,Chassis_central_inertia)

Hsg = RigidBody('Hsg',CoM_Hsg,Hsg_frame,m_Hsg,Hsg_central_inertia)

LBC = RigidBody('LBC',CoM_LBC,LBC_frame,m_LBC,LBC_central_inertia)

LUB = RigidBody('LUB',CoM_LUB,LUB_frame,m_LUB,LUB_central_inertia)

#_____________________________________________________________________________________________________________________________

# Define Gravity Forces

g = symbols('g')

Chassis_grav_force_vec = -m_Chassis * g * inertial_frame.z

Hsg_grav_force_vec = -m_Hsg * g * inertial_frame.z

LBC_grav_force_vec = -m_LBC * g * inertial_frame.z

LUB_grav_force_vec = -m_LUB * g * inertial_frame.z

# Now I need to define the constraints constraining the Hsg to the Chassis via the length of link LUB. LUB link goes from LUBB to LUBC and is a distance of LUB_L. The Hsg and Chassis are both undergoing forces and the LUB rigid body will have a force acting along its axis.