def _create_rigid_bodies(self):
self.rigid_bodies = OrderedDict()
self.rigid_bodies['lthigh'] = \
me.RigidBody('lthigh',
self.points['lthigh_mass_center'],
self.frames['left_thigh'],
self.parameters['thigh_mass'],
self.central_inertias['lthigh'])
self.rigid_bodies['lshank'] = \
me.RigidBody('lshank',
self.points['lshank_mass_center'],
self.frames['left_shank'],
self.parameters['shank_mass'],
self.central_inertias['lshank'])
self.rigid_bodies['rthigh'] = \
me.RigidBody('rthigh',
self.points['rthigh_mass_center'],
self.frames['right_thigh'],
self.parameters['thigh_mass'],
self.central_inertias['rthigh'])
self.rigid_bodies['rshank'] = \
me.RigidBody('rshank',
self.points['rshank_mass_center'],
self.frames['right_shank'],
self.parameters['shank_mass'],
self.central_inertias['rshank'])
def test_specifying_coordinate_issue_339():
"""This test ensures that you can use derivatives as specified values."""
# beta will be a specified angle
beta = me.dynamicsymbols('beta')
q1, q2, q3, q4 = me.dynamicsymbols('q1, q2, q3, q4')
u1, u2, u3, u4 = me.dynamicsymbols('u1, u2, u3, u4')
N = me.ReferenceFrame('N')
A = N.orientnew('A', 'Axis', (q1, N.x))
B = A.orientnew('B', 'Axis', (beta, A.y))
No = me.Point('No')
Ao = No.locatenew('Ao', q2 * N.x + q3 * N.y + q4 * N.z)
Bo = Ao.locatenew('Bo', 10 * A.x + 10 * A.y + 10 * A.z)
A.set_ang_vel(N, u1 * N.x)
B.ang_vel_in(N) # compute it automatically
No.set_vel(N, 0)
Ao.set_vel(N, u2 * N.x + u3 * N.y + u4 * N.z)
Bo.v2pt_theory(Ao, N, B)
body_A = me.RigidBody('A', Ao, A, 1.0, (me.inertia(A, 1, 2, 3), Ao))
body_B = me.RigidBody('B', Bo, B, 1.0, (me.inertia(A, 3, 2, 1), Bo))
bodies = [body_A, body_B]
# TODO : This should be able to be simple an empty iterable.
loads = [(No, 0 * N.x)]
kdes = [u1 - q1.diff(), u2 - q2.diff(), u3 - q3.diff(), u4 - q4.diff()]
kane = me.KanesMethod(N,
q_ind=[q1, q2, q3, q4],
u_ind=[u1, u2, u3, u4],
kd_eqs=kdes)
if sympy_newer_than('1.0'):
fr, frstar = kane.kanes_equations(bodies, loads)
else:
fr, frstar = kane.kanes_equations(loads, bodies)
sys = System(kane)
sys.specifieds = {
(beta, beta.diff(), beta.diff().diff()):
lambda x, t: np.array([1.0, 1.0, 1.0])
}
sys.times = np.linspace(0, 10, 20)
sys.integrate()
def _create_rigid_bodies(self):
self.rigid_bodies = OrderedDict()
self.rigid_bodies['leg'] = \
me.RigidBody('Leg',
self.points['leg_mass_center'],
self.frames['leg'],
self.parameters['leg_mass'],
self.central_inertias['leg'])
self.rigid_bodies['torso'] = \
me.RigidBody('Torso',
self.points['torso_mass_center'],
self.frames['torso'],
self.parameters['torso_mass'],
self.central_inertias['torso'])
# linear velocities and accelerations #
No.set_vel(N, 0) # the newtonian origin is fixed
Ao.set_vel(N, omega * lA * A.x)
Ao.a2pt_theory(No, N, A)
Bo.set_vel(N, omega * lB * A.x)
Bo.a2pt_theory(No, N, A)
# kinematical differential equations #
kinDiffs = [omega - thetad, alpha - phid]
## kinetics ##
# rigid bodies #
# create the empty rod object
rod = me.RigidBody('rod', Ao, A, mA, (me.inertia(A, IAxx, IAxx, 0.0), Ao))
# create the empty plate object
plate = me.RigidBody('plate', Bo, B, mB, (me.inertia(B, IBxx, IByy, IBzz), Bo))
# forces #
# add the gravitional force to each body
rodGravity = (Ao, N.z * gravity * mA)
plateGravity = (Bo, N.z * gravity * mB)
## equations of motion with Kane's method ##
# make a list of the bodies and forces
bodyList = [rod, plate]
forceList = [rodGravity, plateGravity]
# create a Kane object with respect to the Newtonian reference frame
# central inertia
IAxx = sym.S(1) / 12 * mA * (2 * lA)**2
IAyy = IAxx
IAzz = 0
IA = (me.inertia(A, IAxx, IAyy, IAzz), Ao)
IBxx = sym.S(1) / 12 * mB * h**2
IByy = sym.S(1) / 12 * mB * (w**2 + h**2)
IBzz = sym.S(1) / 12 * mB * w**2
IB = (me.inertia(B, IBxx, IByy, IBzz), Bo)
# rigid bodies
rod = me.RigidBody('rod', Ao, A, mA, IA)
plate = me.RigidBody('plate', Bo, B, mB, IB)
# forces #
# add the gravitional force to each body
rod_gravity = (Ao, mA * g * N.z)
plate_gravity = (Bo, mB * g * N.z)
# equations of motion with Kane's method
# make a tuple of the bodies and forces
bodies = (rod, plate)
loads = (rod_gravity, plate_gravity)
# create a Kane object with respect to the Newtonian reference frame
kane = me.KanesMethod(N,
# Create frame for rod BC
rodBCFrame = mech.ReferenceFrame('rodBCFrame')
rodBCFrame.set_ang_vel(N, -phiDot * N.z)
# Create point for the center of mass of BC
comBC_x = (L / 2) * sympy.cos(phi)
comBC_y = (L / 2) * sympy.sin(phi)
comBC_vx = phiDot * comBC_x
comBC_vy = phiDot * comBC_y
comBC = mech.Point('comBC')
comBC.set_vel(N, comBC_vx * N.x + comBC_vy * N.y)
comBC.set_pos(B, comBC_x * N.x + comBC_y * N.y)
# Create rigid body for blade BC
I_BC = mech.inertia(N, 0, 0, Izz)
rodBC = mech.RigidBody('rodBC', comBC, rodBCFrame, m, (I_BC, comBC))
# Get the kinetic energy of each component to check results
mech.kinetic_energy(N, rodBC)
# Set potential energies
rodBC.potential_energy = (1 / 2) * kT * phi**2
# Get (unforced) Lagrangian of the system
L = mech.Lagrangian(N, rodBC)
# Get equation of motion
LM = mech.LagrangesMethod(L, [rodBC], frame=N)
LM.form_lagranges_equations()
# Now the origin and the center of mass of the ladder are defined.
O = me.Point('O')
A = me.Point('A')
# And we use the length and angle to locate the center of mass relative to
# the origin.
A.set_pos(O, -l / 2 * sm.cos(q) * N.x + l / 2 * sm.sin(q) * N.y)
# Take the derivative of the position of the center of mass to get the
# corresponding velocity.
O.set_vel(N, 0)
A.set_vel(N, l / 2 * u * sm.sin(q) * N.x + l / 2 * u * sm.cos(q) * N.y)
# The ladder can now be defined as a rigid body.
ladder = me.RigidBody('ladder', A, L, m, (me.inertia(L, 0, 0, Izz), A))
# Set up all the inputs to Kane's method.
kd = [u - qd]
body_list = [ladder]
force_list = [(A, -m * g * N.y)]
# Finally we solve the dynamics
kane = me.KanesMethod(N, q_ind=[q], u_ind=[u], kd_eqs=kd)
kane.kanes_equations(force_list, body_list)
# The mass matrix and the forcing function can be taken out of the
# KanesMethod object.
MM = kane.mass_matrix
forcing = kane.forcing
def formulate_holonomic_snake(n):
"""Formulate the n-chain system, returning a bunch of sympy objects
to make the equations of motion.
Parameters
----------
n : int
Number of links
Returns
-------
TODO: fixme These are acutally 4 dicts filled with lists
q : list
Generalized coordinates
u : list
Generalized speeds
T : list
Torques
m : list
Masses of each segment
l : list
Lengths of each segment
Izz : list
Moments of inertia about center of mass
"""
# dynamic variables
q = me.dynamicsymbols('q:' +
str(n + 2)) # (x, y, theta, phi_0, phi_1, ...)
u = me.dynamicsymbols('u:' + str(n + 2))
c = me.dynamicsymbols('c:' + str(n - 1), 1) # derivative of constraint
# variables for bodies
g, t = sym.symbols('g t')
m = sym.symbols('m:' + str(n))
l = sym.symbols('l:' + str(n))
Izz = sym.symbols('I_zz_:' + str(n))
# frames
frames = [me.ReferenceFrame('fr_N')]
for i in range(2, n + 2):
name = 'fr_' + str(i)
frame = frames[i].orientnew(name, 'Axis', [q[i], frames[i].z])
frame.set_ang_vel(frames[i], u[i] * frames[i].z)
frames.append(frame)
# origin of inertial frame
joints = [me.Point('N_or')]
joints[0].set_vel(frames[0], 0)
# joints
for i in range(n + 1):
name = 'jt_' + str(i)
if i == 0:
joint = joints[i].locatenew(
name, q[-2] * frames[0].x + q[-1] * frames[0].y)
joint.set_vel(frames[0], u[-2] * frames[0].x + u[-1] * frames[0].y)
else:
joint = joints[i].locatenew(name, l[i - 1] * frames[i].x)
joint.v2pt_theory(joints[i], frames[0], frames[i])
joints.append(joint)
# mass centers
mc = []
for i in range(n):
name = 'mc_' + str(i)
mcent = joints[i + 1].locatenew(name, l[i] / 2 * frames[i + 1].x)
mcent.v2pt_theory(joints[i + 1], frames[0], frames[i + 1])
mc.append(mcent)
# inertia dyads
dyads = []
for i in range(n):
dyad = (me.inertia(frames[i + 1], 0, 0, Izz[i]), mc[i])
dyads.append(dyad)
# bodies
bodies = []
for i in range(n):
name = 'bd_' + str(i)
body = me.RigidBody(name, mc[i], frames[i + 1], m[i], dyads[i])
bodies.append(body)
# kinematic differential equations
kd = []
for i in range(n + 2):
kd.append(q[i].diff(t) - u[i])
forces = []
for i in range(n):
forces.append((mc[i], -m[i] * g * frames[0].z))
torques = []
for i in range(n):
torques.append((frames[i + 1], (T[i] - T[i + 1]) * frames[0].z))
fl = forces #+ torques
# setup dictionaries of values to return
dynamic = dict(q=q, u=u, T=T)
symbol = dict(m=m, l=l, Izz=Izz, g=g, t=t)
setup = dict(frames=frames, joints=joints, mc=mc, dyads=dyads)
fbd = dict(kd=kd, fl=fl, bodies=bodies)
return dynamic, symbol, setup, fbd
# (y-direction)
# Create reference frame for analysis
N = mech.ReferenceFrame('N')
# Set origin
O = mech.Point('O')
O.set_vel(N, 0 * N.x + 0 * N.y + 0 * N.z)
# Set point B
B = mech.Point('B')
B.set_vel(N, vB * N.y)
# Create vertical rod (this one isn't moving)
I1 = mech.inertia(N, 0, 0, 0)
rod1 = mech.RigidBody('rod1', B, N, m, (I1, B))
# Create rod AB
Izz = m * L**2 / 12
I2 = mech.inertia(N, 0, 0, Izz)
rod2Frame = mech.ReferenceFrame('rod2Frame')
rod2Frame.set_ang_vel(N, -thetaDot * N.z)
rod2 = mech.RigidBody('rod2', B, rod2Frame, m, (I2, B))
# Get the kinetic energy of each component tp check results
mech.kinetic_energy(N, rod1)
mech.kinetic_energy(N, rod2)
# Set potential energies
deltaz = (L / 2) * (sympy.sin(theta) - 1 / 2)
V1 = m * g * deltaz
import sympy.physics.mechanics as me
import sympy as sm
import math as m
import numpy as np
g, lb, w, h = sm.symbols('g lb w h', real=True)
theta, phi, omega, alpha = me.dynamicsymbols('theta phi omega alpha')
thetad, phid, omegad, alphad = me.dynamicsymbols('theta phi omega alpha', 1)
thetad2, phid2 = me.dynamicsymbols('theta phi', 2)
frame_n = me.ReferenceFrame('n')
body_a_cm = me.Point('a_cm')
body_a_cm.set_vel(frame_n, 0)
body_a_f = me.ReferenceFrame('a_f')
body_a = me.RigidBody('a', body_a_cm, body_a_f, sm.symbols('m'), (me.outer(body_a_f.x,body_a_f.x),body_a_cm))
body_b_cm = me.Point('b_cm')
body_b_cm.set_vel(frame_n, 0)
body_b_f = me.ReferenceFrame('b_f')
body_b = me.RigidBody('b', body_b_cm, body_b_f, sm.symbols('m'), (me.outer(body_b_f.x,body_b_f.x),body_b_cm))
body_a_f.orient(frame_n, 'Axis', [theta, frame_n.y])
body_b_f.orient(body_a_f, 'Axis', [phi, body_a_f.z])
point_o = me.Point('o')
la = (lb-h/2)/2
body_a_cm.set_pos(point_o, la*body_a_f.z)
body_b_cm.set_pos(point_o, lb*body_a_f.z)
body_a_f.set_ang_vel(frame_n, omega*frame_n.y)
body_b_f.set_ang_vel(body_a_f, alpha*body_a_f.z)
point_o.set_vel(frame_n, 0)
body_a_cm.v2pt_theory(point_o,frame_n,body_a_f)
body_b_cm.v2pt_theory(point_o,frame_n,body_a_f)
ma = sm.symbols('ma')
body_a.mass = ma
Z_G.set_vel(B, e_dot * B.y)
Z_G.v1pt_theory(G, N, B)
Z_G.a1pt_theory(G, N, B)
kde = [
x_dot - x.diff(t), y_dot - y.diff(t), beta_dot - beta.diff(t),
e_dot - e.diff(t)
]
I_plate = me.inertia(N, 0, 0, Izz)
inertia_plate = (I_plate, G)
I_rod = me.inertia(N, 0, 0, Izz_rod)
inertia_rod = (I_rod, Z_G)
Plate = me.RigidBody('Plate', G, B, M, inertia_plate)
rod = me.RigidBody('rod', Z_G, B, m, inertia_rod)
def Lengths_and_Moments(x, y):
'''
This function determines the initial length of the springs and the
moment needed in order to keep the plate in position.
'''
xxx = x
yyy = y
a, b, x, y, H, k1, k2, Length1, Length2 = sympy.symbols(
'a b x y H k1 k2 Length1 Length2')
m, Fsp1, Fsp2, Ma, t1, t2 = sympy.symbols('m Fsp1 Fsp2 Ma t1 t2')
L1 = sympy.sqrt((x - (a / 2))**2 + (y - (b / 2))**2)
L2 = sympy.sqrt((-(H - x) + (a / 2))**2 + (y - (b / 2))**2)
frame_a = me.ReferenceFrame("a")
a = 0
d = me.inertia(frame_a, 1, 1, 1)
point_po1 = me.Point("po1")
point_po2 = me.Point("po2")
particle_p1 = me.Particle("p1", me.Point("p1_pt"), sm.Symbol("m"))
particle_p2 = me.Particle("p2", me.Point("p2_pt"), sm.Symbol("m"))
c1, c2, c3 = me.dynamicsymbols("c1 c2 c3")
c1d, c2d, c3d = me.dynamicsymbols("c1 c2 c3", 1)
body_r_cm = me.Point("r_cm")
body_r_cm.set_vel(frame_n, 0)
body_r_f = me.ReferenceFrame("r_f")
body_r = me.RigidBody(
"r",
body_r_cm,
body_r_f,
sm.symbols("m"),
(me.outer(body_r_f.x, body_r_f.x), body_r_cm),
)
point_po2.set_pos(particle_p1.point, c1 * frame_a.x)
v = 2 * point_po2.pos_from(particle_p1.point) + c2 * frame_a.y
frame_a.set_ang_vel(frame_n, c3 * frame_a.z)
v = 2 * frame_a.ang_vel_in(frame_n) + c2 * frame_a.y
body_r_f.set_ang_vel(frame_n, c3 * frame_a.z)
v = 2 * body_r_f.ang_vel_in(frame_n) + c2 * frame_a.y
frame_a.set_ang_acc(frame_n, (frame_a.ang_vel_in(frame_n)).dt(frame_a))
v = 2 * frame_a.ang_acc_in(frame_n) + c2 * frame_a.y
particle_p1.point.set_vel(frame_a, c1 * frame_a.x + c3 * frame_a.y)
body_r_cm.set_acc(frame_n, c2 * frame_a.y)
v_a = me.cross(body_r_cm.acc(frame_n), particle_p1.point.vel(frame_a))
x_b_c = v_a
point_c = me.Point("c")
point_d = me.Point("d")
point_po1 = me.Point("po1")
point_po2 = me.Point("po2")
point_po3 = me.Point("po3")
particle_l = me.Particle("l", me.Point("l_pt"), sm.Symbol("m"))
particle_p1 = me.Particle("p1", me.Point("p1_pt"), sm.Symbol("m"))
particle_p2 = me.Particle("p2", me.Point("p2_pt"), sm.Symbol("m"))
particle_p3 = me.Particle("p3", me.Point("p3_pt"), sm.Symbol("m"))
body_s_cm = me.Point("s_cm")
body_s_cm.set_vel(frame_n, 0)
body_s_f = me.ReferenceFrame("s_f")
body_s = me.RigidBody(
"s",
body_s_cm,
body_s_f,
sm.symbols("m"),
(me.outer(body_s_f.x, body_s_f.x), body_s_cm),
)
body_r1_cm = me.Point("r1_cm")
body_r1_cm.set_vel(frame_n, 0)
body_r1_f = me.ReferenceFrame("r1_f")
body_r1 = me.RigidBody(
"r1",
body_r1_cm,
body_r1_f,
sm.symbols("m"),
(me.outer(body_r1_f.x, body_r1_f.x), body_r1_cm),
)
body_r2_cm = me.Point("r2_cm")
body_r2_cm.set_vel(frame_n, 0)
v1 = x1 * frame_a.x + x2 * frame_a.y + x3 * frame_a.z
v2 = x1 * frame_b.x + x2 * frame_b.y + x3 * frame_b.z
v3 = x1 * frame_n.x + x2 * frame_n.y + x3 * frame_n.z
v = v1 + v2 + v3
point_c = me.Point('c')
point_d = me.Point('d')
point_po1 = me.Point('po1')
point_po2 = me.Point('po2')
point_po3 = me.Point('po3')
particle_l = me.Particle('l', me.Point('l_pt'), sm.Symbol('m'))
particle_p1 = me.Particle('p1', me.Point('p1_pt'), sm.Symbol('m'))
particle_p2 = me.Particle('p2', me.Point('p2_pt'), sm.Symbol('m'))
particle_p3 = me.Particle('p3', me.Point('p3_pt'), sm.Symbol('m'))
body_s_cm = me.Point('s_cm')
body_s_cm.set_vel(frame_n, 0)
body_s_f = me.ReferenceFrame('s_f')
body_s = me.RigidBody('s', body_s_cm, body_s_f, sm.symbols('m'),
(me.outer(body_s_f.x, body_s_f.x), body_s_cm))
body_r1_cm = me.Point('r1_cm')
body_r1_cm.set_vel(frame_n, 0)
body_r1_f = me.ReferenceFrame('r1_f')
body_r1 = me.RigidBody('r1', body_r1_cm, body_r1_f, sm.symbols('m'),
(me.outer(body_r1_f.x, body_r1_f.x), body_r1_cm))
body_r2_cm = me.Point('r2_cm')
body_r2_cm.set_vel(frame_n, 0)
body_r2_f = me.ReferenceFrame('r2_f')
body_r2 = me.RigidBody('r2', body_r2_cm, body_r2_f, sm.symbols('m'),
(me.outer(body_r2_f.x, body_r2_f.x), body_r2_cm))
v4 = x1 * body_s_f.x + x2 * body_s_f.y + x3 * body_s_f.z
body_s_cm.set_pos(point_c, l * frame_n.x)
def get_symbolic_equations_of_motion(self, verbose=False):
"""
This returns the symbolic equation of motions of the robot (using the URDF). Internally, this used the
`sympy.mechanics` module.
"""
# gravity and time
g, t = sympy.symbols('g t')
# create the world inertial frame of reference and its origin
world_frame = mechanics.ReferenceFrame('Fw')
world_origin = mechanics.Point('Pw')
world_origin.set_vel(world_frame, mechanics.Vector(0))
# create the base frame (its position, orientation and velocities) + generalized coordinates and speeds
base_id = -1
# Check if the robot has a fixed base and create the generalized coordinates and speeds based on that,
# as well the base position, orientation and velocities
if self.has_fixed_base():
# generalized coordinates q(t) and speeds dq(t)
q = mechanics.dynamicsymbols('q:{}'.format(len(self.joints)))
dq = mechanics.dynamicsymbols('dq:{}'.format(len(self.joints)))
pos, orn = self.get_base_pose()
lin_vel, ang_vel = [0, 0, 0], [0, 0, 0] # 0 because fixed base
joint_id = 0
else:
# generalized coordinates q(t) and speeds dq(t)
q = mechanics.dynamicsymbols('q:{}'.format(7 + len(self.joints)))
dq = mechanics.dynamicsymbols('dq:{}'.format(6 + len(self.joints)))
pos, orn = q[:3], q[3:7]
lin_vel, ang_vel = dq[:3], dq[3:6]
joint_id = 7
# set the position, orientation and velocities of the base
base_frame = world_frame.orientnew('Fb', 'Quaternion',
[orn[3], orn[0], orn[1], orn[2]])
base_frame.set_ang_vel(
world_frame, ang_vel[0] * world_frame.x +
ang_vel[1] * world_frame.y + ang_vel[2] * world_frame.z)
base_origin = world_origin.locatenew(
'Pb', pos[0] * world_frame.x + pos[1] * world_frame.y +
pos[2] * world_frame.z)
base_origin.set_vel(
world_frame, lin_vel[0] * world_frame.x +
lin_vel[1] * world_frame.y + lin_vel[2] * world_frame.z)
# inputs u(t) (applied torques)
u = mechanics.dynamicsymbols('u:{}'.format(len(self.joints)))
joint_id_u = 0
# kinematics differential equations
kd_eqs = [q[i].diff(t) - dq[i] for i in range(len(self.joints))]
# define useful lists/dicts for later
bodies, loads = [], []
frames = {base_id: (base_frame, base_origin)}
# frames = {base_id: (worldFrame, worldOrigin)}
# go through each joint/link (each link is associated to a joint)
for link_id in range(self.num_links):
# get useful information about joint/link kinematics and dynamics from simulator
info = self.sim.get_dynamics_info(self.id, link_id)
mass, local_inertia_diagonal = info[0], info[2]
info = self.sim.get_link_state(self.id, link_id)
local_inertial_frame_position, local_inertial_frame_orientation = info[
2], info[3]
# worldLinkFramePosition, worldLinkFrameOrientation = info[4], info[5]
info = self.sim.get_joint_info(self.id, link_id)
joint_name, joint_type = info[1:3]
# jointDamping, jointFriction = info[6:8]
link_name, joint_axis_in_local_frame, parent_frame_position, parent_frame_orientation, \
parent_idx = info[-5:]
xl, yl, zl = joint_axis_in_local_frame
# get previous references
parent_frame, parent_point = frames[parent_idx]
# create a reference frame with its origin for each joint
# set frame orientation
if joint_type == self.sim.JOINT_REVOLUTE:
R = get_matrix_from_quaternion(parent_frame_orientation)
R1 = get_symbolic_matrix_from_axis_angle(
joint_axis_in_local_frame, q[joint_id])
R = R1.dot(R)
frame = parent_frame.orientnew('F' + str(link_id), 'DCM',
sympy.Matrix(R))
else:
x, y, z, w = parent_frame_orientation # orientation of the joint in parent CoM inertial frame
frame = parent_frame.orientnew('F' + str(link_id),
'Quaternion', [w, x, y, z])
# set frame angular velocity
ang_vel = 0
if joint_type == self.sim.JOINT_REVOLUTE:
ang_vel = dq[joint_id] * (xl * frame.x + yl * frame.y +
zl * frame.z)
frame.set_ang_vel(parent_frame, ang_vel)
# create origin of the reference frame
# set origin position
x, y, z = parent_frame_position # position of the joint in parent CoM inertial frame
pos = x * parent_frame.x + y * parent_frame.y + z * parent_frame.z
if joint_type == self.sim.JOINT_PRISMATIC:
pos += q[joint_id] * (xl * frame.x + yl * frame.y +
zl * frame.z)
origin = parent_point.locatenew('P' + str(link_id), pos)
# set origin velocity
if joint_type == self.sim.JOINT_PRISMATIC:
vel = dq[joint_id] * (xl * frame.x + yl * frame.y +
zl * frame.z)
origin.set_vel(world_frame, vel.express(world_frame))
else:
origin.v2pt_theory(parent_point, world_frame, parent_frame)
# define CoM frame and position (and velocities) wrt the local link frame
x, y, z, w = local_inertial_frame_orientation
com_frame = frame.orientnew('Fc' + str(link_id), 'Quaternion',
[w, x, y, z])
com_frame.set_ang_vel(frame, mechanics.Vector(0))
x, y, z = local_inertial_frame_position
com = origin.locatenew('C' + str(link_id),
x * frame.x + y * frame.y + z * frame.z)
com.v2pt_theory(origin, world_frame, frame)
# define com particle
# com_particle = mechanics.Particle('Pa' + str(linkId), com, mass)
# bodies.append(com_particle)
# save
# frames[linkId] = (frame, origin)
# frames[linkId] = (frame, origin, com_frame, com)
frames[link_id] = (com_frame, com)
# define mass and inertia
ixx, iyy, izz = local_inertia_diagonal
inertia = mechanics.inertia(com_frame,
ixx,
iyy,
izz,
ixy=0,
iyz=0,
izx=0)
inertia = (inertia, com)
# define rigid body associated to frame
body = mechanics.RigidBody(link_name, com, frame, mass, inertia)
bodies.append(body)
# define dynamical forces/torques acting on the body
# gravity force applied on the CoM
force = (com, -mass * g * world_frame.z)
loads.append(force)
# if prismatic joint, compute force
if joint_type == self.sim.JOINT_PRISMATIC:
force = (origin, u[joint_id_u] *
(xl * frame.x + yl * frame.y + zl * frame.z))
# force = (com, u[jointIdU] * (x * frame.x + y * frame.y + z * frame.z) - mass * g * worldFrame.z)
loads.append(force)
# if revolute joint, compute torque
if joint_type == self.sim.JOINT_REVOLUTE:
v = (xl * frame.x + yl * frame.y + zl * frame.z)
# torqueOnPrevBody = (parentFrame, - u[jointIdU] * v)
torque_on_prev_body = (parent_frame, -u[joint_id_u] * v)
torque_on_curr_body = (frame, u[joint_id_u] * v)
loads.append(torque_on_prev_body)
loads.append(torque_on_curr_body)
# if joint is not fixed increment the current joint id
if joint_type != self.sim.JOINT_FIXED:
joint_id += 1
joint_id_u += 1
if verbose:
print("\nLink name with type: {} - {}".format(
link_name, self.get_joint_types(joint_ids=link_id)))
print("------------------------------------------------------")
print("Position of joint frame wrt parent frame: {}".format(
origin.pos_from(parent_point)))
print("Orientation of joint frame wrt parent frame: {}".format(
frame.dcm(parent_frame)))
print("Linear velocity of joint frame wrt parent frame: {}".
format(origin.vel(world_frame).express(parent_frame)))
print("Angular velocity of joint frame wrt parent frame: {}".
format(frame.ang_vel_in(parent_frame)))
print("------------------------------------------------------")
print("Position of joint frame wrt world frame: {}".format(
origin.pos_from(world_origin)))
print("Orientation of joint frame wrt world frame: {}".format(
frame.dcm(world_frame).simplify()))
print("Linear velocity of joint frame wrt world frame: {}".
format(origin.vel(world_frame)))
print("Angular velocity of joint frame wrt parent frame: {}".
format(frame.ang_vel_in(world_frame)))
print("------------------------------------------------------")
# print("Local position of CoM wrt joint frame: {}".format(com.pos_from(origin)))
# print("Local linear velocity of CoM wrt joint frame: {}".format(com.vel(worldFrame).express(frame)))
# print("Local angular velocity of CoM wrt joint frame: {}".format(com_frame.ang_vel_in(frame)))
# print("------------------------------------------------------")
if joint_type == self.sim.JOINT_PRISMATIC:
print("Input value (force): {}".format(loads[-1]))
elif joint_type == self.sim.JOINT_REVOLUTE:
print(
"Input value (torque on previous and current bodies): {} and {}"
.format(loads[-2], loads[-1]))
print("")
if verbose:
print("Summary:")
print("Generalized coordinates: {}".format(q))
print("Generalized speeds: {}".format(dq))
print("Inputs: {}".format(u))
print("Kinematic differential equations: {}".format(kd_eqs))
print("Bodies: {}".format(bodies))
print("Loads: {}".format(loads))
print("")
# TODO: 1. account for external forces applied on different rigid-bodies (e.g. contact forces)
# TODO: 2. account for constraints (e.g. holonomic, non-holonomic, etc.)
# Get the Equation of Motion (EoM) using Kane's method
kane = mechanics.KanesMethod(world_frame,
q_ind=q,
u_ind=dq,
kd_eqs=kd_eqs)
kane.kanes_equations(bodies=bodies, loads=loads)
# get mass matrix and force vector (after simplifying) such that :math:`M(x,t) \dot{x} = f(x,t)`
M = sympy.trigsimp(kane.mass_matrix_full)
f = sympy.trigsimp(kane.forcing_full)
# mechanics.find_dynamicsymbols(M)
# mechanics.find_dynamicsymbols(f)
# save useful info for future use (by other methods)
constants = [g]
constant_values = [9.81]
parameters = (dict(zip(constants, constant_values)))
self.symbols = {
'q': q,
'dq': dq,
'kane': kane,
'parameters': parameters
}
# linearize
# parameters = dict(zip(constants, constant_values))
# M_, A_, B_, u_ = kane.linearize()
# A_ = A_.subs(parameters)
# B_ = B_.subs(parameters)
# M_ = kane.mass_matrix_full.subs(parameters)
# self.symbols = {'A_': A_, 'B_': B_, 'M_': M_}
# return M_, A_, B_, u_
return M, f
v = (v).express(frame_n)
point_o.set_pos(point_p, (point_o.pos_from(point_p)).express(frame_n))
frame_a.set_ang_vel(frame_n, c3 * frame_a.z)
print(frame_n.ang_vel_in(frame_a))
point_p.v2pt_theory(point_o, frame_n, frame_a)
particle_p1 = me.Particle("p1", me.Point("p1_pt"), sm.Symbol("m"))
particle_p2 = me.Particle("p2", me.Point("p2_pt"), sm.Symbol("m"))
particle_p2.point.v2pt_theory(particle_p1.point, frame_n, frame_a)
point_p.a2pt_theory(particle_p1.point, frame_n, frame_a)
body_b1_cm = me.Point("b1_cm")
body_b1_cm.set_vel(frame_n, 0)
body_b1_f = me.ReferenceFrame("b1_f")
body_b1 = me.RigidBody(
"b1",
body_b1_cm,
body_b1_f,
sm.symbols("m"),
(me.outer(body_b1_f.x, body_b1_f.x), body_b1_cm),
)
body_b2_cm = me.Point("b2_cm")
body_b2_cm.set_vel(frame_n, 0)
body_b2_f = me.ReferenceFrame("b2_f")
body_b2 = me.RigidBody(
"b2",
body_b2_cm,
body_b2_f,
sm.symbols("m"),
(me.outer(body_b2_f.x, body_b2_f.x), body_b2_cm),
)
g = sm.symbols("g", real=True)
force_p1 = particle_p1.mass * (g * frame_n.x)
# Set up the kinematic differential equations
kde = [
x_dot - x.diff(t), z_dot - z.diff(t), e_dot - e.diff(t),
theta_dot - theta.diff(t), phi_dot - phi.diff(t)
]
# Create the plate inertial tensor
I_plate = me.inertia(A, 0, Ip, 0)
inertia_plate = (I_plate, G)
# Create the rod inertial tensor
I_rod = me.inertia(A, 0, Ir, 0)
inertia_rod = (I_rod, Gr)
# Create the Rigid Bodies
Plate = me.RigidBody('Plate', G, B, M, inertia_plate)
Rod = me.RigidBody('Rod', Gr, C, m, inertia_rod)
# In[5]:
# These functions do not allow the springs or dampers to push
K1 = lambda LamLenK1: k * (LamLenK1 >= L1)
K2 = lambda LamLenK2: k * (LamLenK2 >= L2)
C1 = lambda LamLenC1: c * (LamLenC1 >= L1)
C2 = lambda LamLenC2: c * (LamLenC2 >= L2)
# In[6]:
# Creating the forces acting on the body
grav_force_plate = (G, M * g * A.z)
grav_force_rod = (Gr, m * g * A.z)
M.set_vel(N, 0)
O.set_vel(S, u * S.x + v * S.y + w * S.z)
O.v1pt_theory(M, N, S)
velocity_matrix = O.vel(N).to_matrix(N)
mass = sp.symbols('m')
I_xx, I_yy, I_zz = sp.symbols('I_xx, I_yy, I_zz')
body_inertia_dyadic = me.inertia(S, ixx=I_xx, iyy=I_yy, izz=I_zz)
body_central_inertia = (body_inertia_dyadic, O)
body = me.RigidBody('Rigid body',
masscenter=O,
frame=S,
mass=mass,
inertia=body_central_inertia)
fx, fy, fz, mx, my, mz = sp.symbols('f_x f_y f_z m_x m_y m_z')
force_vector = fx * S.x + fy * S.y + fz * S.z
torque_vector = mx * S.x + my * S.y + mz * S.z
force = (O, force_vector)
torque = (S, torque_vector)
kinematical_differential_equations = [
x0.diff() - velocity_matrix[0],
y0.diff() - velocity_matrix[1],
z0.diff() - velocity_matrix[2],
phi.diff() - phi1d,
theta.diff() - theta1d,
B = A.orientnew('B', 'Body', (q[6], q[7], q[8]), '123')
B.set_ang_vel(A, u[6] * A.x + u[7] * A.y + u[8] * A.z)
CMb = CMa.locatenew('CM_b', q[9] * A.x + q[10] * A.y + q[11] * A.z)
CMb.set_vel(A, u[9] * A.x + u[10] * A.y + u[11] * A.z)
CMb.set_vel(N, CMb.vel(A).express(N))
BA = [
CMb.locatenew('BA_0', L[36] * B.x - L[37] * B.y + L[38] * B.z),
CMb.locatenew('BA_1', -L[39] * B.x - L[40] * B.y + L[41] * B.z),
CMb.locatenew('BA_2', -L[42] * B.x - L[43] * B.y - L[44] * B.z),
CMb.locatenew('BA_3', L[45] * B.x - L[46] * B.y - L[47] * B.z)
]
In = [me.inertia(A, I[0], I[1], I[2]), me.inertia(B, I[3], I[4], I[5])]
bodies = [
me.RigidBody('Shell', CMa, A, ma, (In[0], CMa)),
me.RigidBody('Block', CMb, B, mb, (In[1], CMb))
]
loads = [(CMa, -ma * g * N.y), (CMb, -mb * g * N.y)]
# CMa.set_vel(N, CMa.pos_from(Orig).dt(N))
# CMb.set_vel(N, CMb.pos_from(Orig).dt(N))
# CMb.set_vel(A, CMb.pos_from(CMa).dt(A))
for _i in range(4):
OA[_i].v2pt_theory(Orig, N, N)
AO[_i].v2pt_theory(CMa, N, A)
AB[_i].v2pt_theory(CMa, N, A)
BA[_i].v2pt_theory(CMb, N, B)
BA[_i].v2pt_theory(CMb, A, B)
cg[1] * P10[i].vel(N0).dot(N0.y) * N0.y +
cg[2] * P10[i].vel(N0).dot(N0.z) * N0.z)))
loads.append((P12[i], -(cs[0] * P12[i].vel(N0).dot(N0.x) * N0.x +
cs[1] * P12[i].vel(N0).dot(N0.y) * N0.y +
cs[2] * P12[i].vel(N0).dot(N0.z) * N0.z)))
loads.append((P21[i], -(cs[0] * P21[i].vel(N0).dot(N0.x) * N0.x +
cs[1] * P21[i].vel(N0).dot(N0.y) * N0.y +
cs[2] * P21[i].vel(N0).dot(N0.z) * N0.z)))
np.vstack = (me.inertia(N2, I2[0], I2[1], I2[2]), P2)
In3 = (me.inertia(N3, I3[0], I3[1], I3[2]), P3)
In4 = (me.inertia(N3, I4[0], I4[1], I4[2]), P4)
In5 = (me.inertia(N5, I5[0], I5[1], I5[2]), P5)
In6 = (me.inertia(N6, I6[0], I6[1], I6[2]), P6)
body1 = me.RigidBody('b_1', P1, N1, m1, In1)
body2 = me.RigidBody('b_2', P2, N2, m2, In2)
body3 = me.RigidBody('b_3', P3, N3, m3, In3)
body4 = me.RigidBody('b_4', P4, N3, m4, In4)
body5 = me.RigidBody('b_5', P5, N5, m5, In5)
body6 = me.RigidBody('b_6', P6, N6, m6, In6)
kdes = [
q[0].diff() - u[0], q[1].diff() - u[1], q[2].diff() - u[2],
q[3].diff() - u[3], q[4].diff() - u[4], q[5].diff() - u[5],
q[6].diff() - u[6], q[7].diff() - u[7], q[8].diff() - u[8],
q[9].diff() - u[9], q[10].diff() - u[10], q[11].diff() - u[11],
q[12].diff() - u[12], q[13].diff() - u[13], q[14].diff() - u[14]
]
KM = me.KanesMethod(N0,
# The linear velocities can be specified the same way as the bob points.
P_link1.v2pt_theory(O, I, A)
P_link2.v2pt_theory(P1, I, B)
P_link3.v2pt_theory(P2, I, C)
points_rigid_body = [P_link1, P_link2, P_link3]
# The inertia tensors for the links are defined with respect to the mass
# center of the link and the link's reference frame.
inertia_link1 = (me.inertia(A, Ixx[0], Iyy[0], Izz[0]), P_link1)
inertia_link2 = (me.inertia(B, Ixx[1], Iyy[1], Izz[1]), P_link2)
inertia_link3 = (me.inertia(C, Ixx[2], Iyy[2], Izz[2]), P_link3)
# Now rigid bodies can be created for each link.
link1 = me.RigidBody('link1', P_link1, A, m_link[0], inertia_link1)
link2 = me.RigidBody('link2', P_link2, B, m_link[1], inertia_link2)
link3 = me.RigidBody('link3', P_link3, C, m_link[2], inertia_link3)
links = [link1, link2, link3]
# The only contributing forces to the system is the force due to gravity
# acting on each particle and body.
forces = []
for particle in particles:
mass = particle.get_mass()
point = particle.get_point()
forces.append((point, -mass * g * I.y))
for link in links:
mass = link.get_mass()
P.set_pos(O2, -(H - x) * N.x + y * N.y)
# P.set_pos(O1, x * N.x + y * N.y)
# Set the point's velocity
P.set_vel(N, x_dot * N.x + y_dot * N.y)
# Create the rod center of mass
G = P.locatenew('G', D / 2 * B.y)
# Set the velocity of G
G.v2pt_theory(P, N, B)
# Create the rod
I_rod = me.inertia(B, 0, 0, Izz)
rod = me.RigidBody('rod', G, B, m, (I_rod, G))
# Create the distance from the point to each attachment point
L1 = O1.pos_from(P).magnitude
L2 = O2.pos_from(P).magnitude
L1_vector = O1.pos_from(P).normalize
L2_vector = O2.pos_from(P).normalize
# Create the height from the center of gravity to the datum
h = G.pos_from(O1) & N.y
# The forces at the connection point
forceP = c * (x_dot + y_dot) * L1_vector() + c * (-x_dot + y_dot) * L2_vector()
# The forces on the beta frame
forceB = c_beta * beta_dot * N.z
def derive_sys(n,p):
"""
Derive the equations of motion using Kane's method
Inputs:
n - number of discrete elements to use in the model
p - packed parameters to create symbolic material and physical equations
Outputs:
Symbolic, nolinear equations of motion for the discretized catheter model
"""
#-------------------------------------------------
# Step 1: construct the catheter model
# Generalized coordinates and velocities
# (in this case, angular positions & velocities of each mass)
q = mechanics.dynamicsymbols('q:{0}'.format(n))
u = mechanics.dynamicsymbols('u:{0}'.format(n))
# Torques applied to each element due to external loads
Torque = dynamicsymbols('tau:{0}'.format(n))
# Force applied at the end of the catheter by the user
F_in = dynamicsymbols('F:{0}'.format(1))
# Unpack the system values
M, L, E, I = p
# Structural damping
damp = 0.05
# Assuming that the lengths and masses are evenly distributed
# (that is, the sytem is homogeneous), let's evenly divide the
# Lengths and masses along each discrete member
lengths = np.concatenate([np.broadcast_to(L / n,n)])
masses = np.concatenate([np.broadcast_to(M / n,n)])
# time symbol
t = symbols('t')
# The stiffness of the internal springs simulating material stiffness
stiffness = E * I
#--------------------------------------------------
# Step 2: build the model using Kane's Method
# Create pivot point reference frame
A = mechanics.ReferenceFrame('A')
P = mechanics.Point('P')
P.set_vel(A,0)
# lists to hold particles, forces, and kinetic ODEs
# for each pendulum in the chain
particles = []
forces = []
kinetic_odes = []
# Create a rotated reference frame for the first rigid link
Ar = A.orientnew('A' + str(0), 'axis', [q[0],A.z])
# Create a point at the center of gravity of the first link
Gr = P.locatenew('G' + str(0),(lengths[0] / 2) * Ar.x)
Gr.v2pt_theory(P,A,Ar)
# Create a point at the end of the link
Pr = P.locatenew('P' + str(0), lengths[0] * Ar.x)
Pr.v2pt_theory(P, A, Ar)
# Create the inertia for the first rigid link
Inertia_r = inertia(Ar,0,0,masses[0] * lengths[0]**2 / 12)
# Create a new particle of mass m[i] at this point
Par = mechanics.RigidBody('Pa' + str(0), Gr, Ar, masses[0], (Inertia_r,Gr))
particles.append(Par)
# Add an internal spring based on Euler-Bernoulli Beam theory
forces.append((Ar, -stiffness * (q[0]) / (lengths[0]) * Ar.z))
# Add a damping term
forces.append((Ar, (-damp * u[0]) * Ar.z))
# Add a new ODE term
kinetic_odes.append(q[0].diff(t) - u[0])
P = Pr
for i in range(1,n):
# Create a reference frame following the i^th link
Ai = A.orientnew('A' + str(i), 'Axis', [q[i],Ar.z])
Ai.set_ang_vel(A, u[i] * Ai.z)
# Set the center of gravity for this link
Gi = P.locatenew('G' + str(i),lengths[i] / 2 * Ai.x)
Gi.v2pt_theory(P,A,Ai)
# Create a point in this reference frame
Pi = P.locatenew('P' + str(i), lengths[i] * Ai.x)
Pi.v2pt_theory(P, A, Ai)
# Set the inertia for this link
Inertia_i = inertia(Ai,0,0,masses[i] * lengths[i]**2 / 12)
# Create a new particle of mass m[i] at this point
Pai = mechanics.RigidBody('Pa' + str(i), Gi, Ai, masses[i], (Inertia_i,Gi))
particles.append(Pai)
# The external torques influence neighboring links
if i + 1 < n:
next_torque = 0
for j in range(i,n):
next_torque += Torque[j]
else:
next_torque = 0.
forces.append((Ai,(Torque[i] + next_torque) * Ai.z))
# Add another internal spring
forces.append((Ai, (-stiffness * (q[i] - q[i-1]) / (2 * lengths[i])) * Ai.z))
# Add the damping term
forces.append((Ai, (-damp * u[i]) * Ai.z))
kinetic_odes.append(q[i].diff(t) - u[i])
P = Pi
# Add the user-defined input at the tip of the catheter, pointing normal to the
# last element
forces.append((P, F_in[0] * Ai.y))
# Generate equations of motion
KM = mechanics.KanesMethod(A, q_ind=q, u_ind=u,
kd_eqs=kinetic_odes)
fr, fr_star = KM.kanes_equations( particles, forces)
return KM, fr, fr_star, q, u, Torque, F_in, lengths, masses
# Setting up the total x equation
Fy = Fy1 + Fy2 - 9.81*m
# try 2 * Fy to make Fy's equal
Totalx = Fx1 - Fx2
Totaly = Fy1 + Fy2 - 9.81*m
Totalx = (Fx.subs({x:xxx, y:yyy, a:4.0,b:2.0, H:20.0, k1:100.0, k2:100.0})).evalf()
Totaly = (Fy.subs({x:xxx, y:yyy, a:4.0,b:2.0, H:20.0, k1:100.0, k2:100.0, m:12.0})).evalf()
abv = sympy.solve([sympy.Eq(Totalx, 0.0), sympy.Eq(Totaly, 0.0)], [Fsp1,Fsp2])
l1 = abv[Fsp1]
l2 = abv[Fsp2]
return l1,l2
I_plate = me.inertia(N, 0, 0, Izz)
inertia_plate = (I_plate, G)
Plate = me.RigidBody('Plate', G, B, M, inertia_plate)
grav_force_plate = (G, -M * g * N.y)
Length1 = P1.pos_from(O1).magnitude()
Length2 = P2.pos_from(O2).magnitude()
P1_vector = P1.pos_from(O1).normalize()
P2_vector = P2.pos_from(O2).normalize()
left_force = (P1, -(F1*P1_vector))
right_force = (P2, -(F2*P2_vector))
coordinates = [x, y, beta]
speeds= [x_dot, y_dot, beta_dot]
import sympy as sm
import math as m
import numpy as np
g, lb, w, h = sm.symbols("g lb w h", real=True)
theta, phi, omega, alpha = me.dynamicsymbols("theta phi omega alpha")
thetad, phid, omegad, alphad = me.dynamicsymbols("theta phi omega alpha", 1)
thetad2, phid2 = me.dynamicsymbols("theta phi", 2)
frame_n = me.ReferenceFrame("n")
body_a_cm = me.Point("a_cm")
body_a_cm.set_vel(frame_n, 0)
body_a_f = me.ReferenceFrame("a_f")
body_a = me.RigidBody(
"a",
body_a_cm,
body_a_f,
sm.symbols("m"),
(me.outer(body_a_f.x, body_a_f.x), body_a_cm),
)
body_b_cm = me.Point("b_cm")
body_b_cm.set_vel(frame_n, 0)
body_b_f = me.ReferenceFrame("b_f")
body_b = me.RigidBody(
"b",
body_b_cm,
body_b_f,
sm.symbols("m"),
(me.outer(body_b_f.x, body_b_f.x), body_b_cm),
)
body_a_f.orient(frame_n, "Axis", [theta, frame_n.y])
body_b_f.orient(body_a_f, "Axis", [phi, body_a_f.z])
import sympy.physics.mechanics as _me
import sympy as _sm
import math as m
import numpy as _np
frame_a = _me.ReferenceFrame('a')
c1, c2, c3 = _sm.symbols('c1 c2 c3', real=True)
a = _me.inertia(frame_a, 1, 1, 1)
particle_p1 = _me.Particle('p1', _me.Point('p1_pt'), _sm.Symbol('m'))
particle_p2 = _me.Particle('p2', _me.Point('p2_pt'), _sm.Symbol('m'))
body_r_cm = _me.Point('r_cm')
body_r_f = _me.ReferenceFrame('r_f')
body_r = _me.RigidBody('r', body_r_cm, body_r_f, _sm.symbols('m'), (_me.outer(body_r_f.x,body_r_f.x),body_r_cm))
frame_a.orient(body_r_f, 'DCM', _sm.Matrix([1,1,1,1,1,0,0,0,1]).reshape(3, 3))
point_o = _me.Point('o')
m1 = _sm.symbols('m1')
particle_p1.mass = m1
m2 = _sm.symbols('m2')
particle_p2.mass = m2
mr = _sm.symbols('mr')
body_r.mass = mr
i1 = _sm.symbols('i1')
i2 = _sm.symbols('i2')
i3 = _sm.symbols('i3')
body_r.inertia = (_me.inertia(body_r_f, i1, i2, i3, 0, 0, 0), body_r_cm)
point_o.set_pos(particle_p1.point, c1*frame_a.x)
point_o.set_pos(particle_p2.point, c2*frame_a.y)
point_o.set_pos(body_r_cm, c3*frame_a.z)
a = _me.inertia_of_point_mass(particle_p1.mass, particle_p1.point.pos_from(point_o), frame_a)
a = _me.inertia_of_point_mass(particle_p2.mass, particle_p2.point.pos_from(point_o), frame_a)
a = body_r.inertia[0] + _me.inertia_of_point_mass(body_r.mass, body_r.masscenter.pos_from(point_o), frame_a)
holonomic.diff(mec.dynamicsymbols._t).subs({q4d:u4, q5d:u5,
q7d:u7})]
#########
# Inertia
#########
Ic = mec.inertia(C, ic11, ic22, ic33, 0.0, 0.0, ic31)
Id = mec.inertia(D, id11, id22, id11, 0.0, 0.0, 0.0)
Ie = mec.inertia(E, ie11, ie22, ie33, 0.0, 0.0, ie31)
If = mec.inertia(F, if11, if22, if11, 0.0, 0.0, 0.0)
##############
# Rigid Bodies
##############
rearFrame = mec.RigidBody('Rear Frame', co, C, mc, (Ic, co))
rearWheel = mec.RigidBody('Rear Wheel', do, D, md, (Id, do))
frontFrame = mec.RigidBody('Front Frame', eo, E, me, (Ie, eo))
frontWheel = mec.RigidBody('Front Wheel', fo, F, mf, (If, fo))
bodyList = [rearFrame, rearWheel, frontFrame, frontWheel]
###########################
# Generalized Active Forces
###########################
# gravity
Fco = (co, mc * g * A['3'])
Fdo = (do, md * g * A['3'])
Feo = (eo, me * g * A['3'])
Ffo = (fo, mf * g * A['3'])
point_o = me.Point('o')
point_p = me.Point('p')
point_o.set_pos(point_p, c1 * frame_a.x)
v = (v).express(frame_n)
point_o.set_pos(point_p, (point_o.pos_from(point_p)).express(frame_n))
frame_a.set_ang_vel(frame_n, c3 * frame_a.z)
print(frame_n.ang_vel_in(frame_a))
point_p.v2pt_theory(point_o, frame_n, frame_a)
particle_p1 = me.Particle('p1', me.Point('p1_pt'), sm.Symbol('m'))
particle_p2 = me.Particle('p2', me.Point('p2_pt'), sm.Symbol('m'))
particle_p2.point.v2pt_theory(particle_p1.point, frame_n, frame_a)
point_p.a2pt_theory(particle_p1.point, frame_n, frame_a)
body_b1_cm = me.Point('b1_cm')
body_b1_cm.set_vel(frame_n, 0)
body_b1_f = me.ReferenceFrame('b1_f')
body_b1 = me.RigidBody('b1', body_b1_cm, body_b1_f, sm.symbols('m'),
(me.outer(body_b1_f.x, body_b1_f.x), body_b1_cm))
body_b2_cm = me.Point('b2_cm')
body_b2_cm.set_vel(frame_n, 0)
body_b2_f = me.ReferenceFrame('b2_f')
body_b2 = me.RigidBody('b2', body_b2_cm, body_b2_f, sm.symbols('m'),
(me.outer(body_b2_f.x, body_b2_f.x), body_b2_cm))
g = sm.symbols('g', real=True)
force_p1 = particle_p1.mass * (g * frame_n.x)
force_p2 = particle_p2.mass * (g * frame_n.x)
force_b1 = body_b1.mass * (g * frame_n.x)
force_b2 = body_b2.mass * (g * frame_n.x)
z = me.dynamicsymbols('z')
v = x * frame_a.x + y * frame_a.z
point_o.set_pos(point_p, x * frame_a.x + y * frame_a.y)
v = (v).subs({x: 2 * z, y: z})
point_o.set_pos(point_p, (point_o.pos_from(point_p)).subs({x: 2 * z, y: z}))
def system_model_generator(ct, param_deviation=False):
'''
Modeling inverted pendulum using Kane's Method
'''
# Defining symbolic Variables
n = ct.number_of_pendulums
q = me.dynamicsymbols('q:{}'.format(n + 1)) # generalized coordinates
qdot = me.dynamicsymbols('qdot:{}'.format(n + 1)) #generalized speeds
qdd = me.dynamicsymbols('qddot:{}'.format(n + 1))
f = me.dynamicsymbols('f')
u = sm.symbols('u')
m = sm.symbols('m:{}'.format(n + 1))
J = sm.symbols('J:{}'.format(n + 1))
l = sm.symbols('l1:{}'.format(n + 1)) # lenght of each pendlum
a = sm.symbols('a1:{}'.format(n + 1)) #location of Mass-centers
d = sm.symbols('d1:{}'.format(n + 1)) #viscous damping coef.
g, t = sm.symbols('g t')
dynamic_symbs = q + qdot + [u]
# intertial reference frame
In_frame = me.ReferenceFrame('In_frame')
# Origninal Point O in Reference Frame :
O = me.Point('O')
O.set_vel(In_frame, 0)
# The Center of Mass Point on cart :
C0 = me.Point('C0')
C0.set_pos(O, q[0] * In_frame.x)
C0.set_vel(In_frame, qdot[0] * In_frame.x)
cart_inertia_dyadic = me.inertia(In_frame, 0, 0, J[0])
cart_central_inertia = (cart_inertia_dyadic, C0)
cart = me.RigidBody('Cart', C0, In_frame, m[0], cart_central_inertia)
kindiffs = [q[0].diff(t) - qdot[0]] # entforcing qdot=Omega
frames = [In_frame]
mass_centers = [C0]
joint_centers = [C0]
central_inertias = [cart_central_inertia]
forces = [(C0, f * In_frame.x - m[0] * g * In_frame.y)]
# torques = []
# cart_potential = 1 / 3 * d[0] * qdot[1]**3
# potentials = [cart_potential]
# cart.potential_energy= cart_potential
rigid_bodies = [cart]
# Lagrangian0 = me.Lagrangian(In_frame, rigid_bodies[0])
# Lagrangians=[Lagrangian0]
for i in range(n):
#Creating new reference frame
Li = In_frame.orientnew('L' + str(i), 'Axis', [q[i + 1], In_frame.z])
Li.set_ang_vel(In_frame, qdot[i + 1] * In_frame.z)
frames.append(Li)
# Creating new Points representing mass_centers
Pi = joint_centers[-1].locatenew('a' + str(i + 1), a[i] * Li.y)
Pi.v2pt_theory(joint_centers[-1], In_frame, Li)
mass_centers.append(Pi)
#Creating new Points representing Joints
Jointi = joint_centers[-1].locatenew('jont' + str(i + 1), l[i] * Li.y)
Jointi.v2pt_theory(joint_centers[-1], In_frame, Li)
joint_centers.append(Jointi)
#adding forces
forces.append((Pi, -m[i + 1] * g * In_frame.y))
#adding cential inertias
IDi = me.inertia(frames[i + 1], 0, 0, J[i + 1])
ICi = (IDi, mass_centers[i + 1])
central_inertias.append(ICi)
LBodyi = me.RigidBody('L' + str(i + 1) + '_Body', mass_centers[i + 1],
frames[i + 1], m[i + 1], central_inertias[i + 1])
rigid_bodies.append(LBodyi)
kindiffs.append(q[i + 1].diff(t) - qdot[i + 1])
# add torques :
if n == 1:
torqueVector1 = -(d[0] * qdot[1]) * In_frame.z
torques = [(frames[1], torqueVector1)]
elif n == 2:
torqueVector1 = -(d[0] * qdot[1] - d[1] *
(qdot[1] - qdot[2])) * In_frame.z
torqueVector2 = -(d[1] * (qdot[2] - qdot[1])) * In_frame.z
torques = [(frames[1], torqueVector1), (frames[2], torqueVector2)]
elif n == 3:
torqueVector1 = (-d[0] * qdot[1] - d[1] *
(qdot[1] - qdot[2])) * In_frame.z
torqueVector2 = (-d[1] * (qdot[2] - qdot[1]) - d[2] *
(qdot[2] - qdot[3])) * In_frame.z
torqueVector3 = -d[2] * (qdot[3] - qdot[2]) * In_frame.z
torques = [(frames[1], torqueVector1), (frames[2], torqueVector2),
(frames[3], torqueVector3)]
#generalized force
loads = torques + forces
#Kane's Method --> Equation of motion
Kane = me.KanesMethod(In_frame, q, qdot, kd_eqs=kindiffs)
fr, frstar = Kane.kanes_equations(rigid_bodies, loads)
mass_matrix = sm.trigsimp(Kane.mass_matrix_full)
forcing_vector = sm.trigsimp(Kane.forcing_full)
#xdot_expr=(mass_matrix.inv()*forcing_vector)
# defining parameter values according to number of pendulum n :
if param_deviation == True:
param_dict_with_deviation = ct.model.param_dict_with_deviation
param_values = [param_dict_with_deviation[str(li)] for li in l] + [
param_dict_with_deviation[str(ai)] for ai in a
] + [param_dict_with_deviation[str(mi)] for mi in m] + [
param_dict_with_deviation[str(Ji)] for Ji in J
] + [param_dict_with_deviation[str(di)]
for di in d] + [param_dict_with_deviation['g']
] + [param_dict_with_deviation['f']]
else:
param_values = ct.parameter_values
param_symb = list(l + a + m + J + d + (g, f))
param_list = zip(param_symb, param_values)
param_dict = dict(param_list)
# substituting parameters to mass and forcing_vector
mass_matrix_simplified = mass_matrix.subs(param_dict).simplify()
forcing_vector_simplified = forcing_vector.subs(param_dict).simplify()
# finding fx and gx wiht qdd0 as input
mass_matrix44 = mass_matrix_simplified[len(q):, len(q):]
forcing_vector44 = forcing_vector_simplified[len(q):, 0]
fx, gx = generate_state_equ(mass_matrix44, forcing_vector44, dynamic_symbs)
print('system model succesfully finished !')
# returning the model in a container :
ct.model.t = t
ct.model.q = q
ct.model.qdot = qdot
ct.model.qdd = qdd
ct.model.dynamic_symbs = dynamic_symbs
ct.model.f = f
ct.model.u = u
ct.model.m = m
ct.model.J = J
ct.model.l = l
ct.model.a = a
ct.model.d = d
ct.model.g = g
ct.model.param_dict = param_dict
ct.model.frames = frames
ct.model.mass_centers = mass_centers
ct.model.origin_point = O
ct.model.joint_centers = joint_centers
ct.model.central_inertias = central_inertias
ct.model.rigid_bodies = rigid_bodies
ct.model.forces = forces
ct.model.torques = torques
ct.model.loads = loads
ct.model.kindiffs = kindiffs
ct.model.fr = fr
ct.model.fstar = frstar
ct.model.mass_matrix = mass_matrix
ct.model.mass_matrix_simplified = mass_matrix_simplified
ct.model.forcing_vector = forcing_vector
ct.model.forcing_vector_simplified = forcing_vector_simplified
ct.model.fx = fx
ct.model.gx = gx
# thats just a trick to be able to dump the results (there is a problem with dumping classes outside of the main!)
for qi in q:
qi.__class__.__module__ = '__main__'
for qdoti in qdot:
qdoti.__class__.__module__ = '__main__'
for qddi in qdd:
qddi.__class__.__module__ = '__main__'
f.__class__.__module__ = '__main__'
label = ct.label
# storing system model as binary file to be used later
param_tol_dict = ct.model.param_tol_dict if param_deviation else None
default_tol = ct.model.default_tol if param_deviation else '0.0'
sys_model = {
'fx': fx,
'gx': gx,
'q': q,
'qdot': qdot,
'qdd': qdd,
'dynamic_symbs': dynamic_symbs,
'param_dict': param_dict,
'param_tol_dict': param_tol_dict,
'default_tol': default_tol
}
if param_deviation == True:
model_file_name = ct.model.model_file_name
else:
model_file_name = 'sys_model_' + 'without_param_deviation_' + label + '.pkl'
dump_model(model_file_name, sys_model)
