def make_kane_eom(dynamic, setup, fbd):
"""Formulate the equations of motion using Kane's method.
Paramters
---------
The dictionaries that are returned from formulated_nchain_parameters
Returns
-------
"""
# equations of motion using Kane's method
kane = me.KanesMethod(frame=setup['frames'][0],
q_ind=dynamic['q'],
u_ind=dynamic['u'],
kd_eqs=fbd['kd'],
q_dependent=[],
configuration_constraints=[],
u_dependent=[],
velocity_constraints=[])
(fr, frstar) = kane.kanes_equations(fbd['fl'], fbd['bodies'])
kanezero = fr + frstar
mass = kane.mass_matrix_full
forcing = kane.forcing_full
eom = dict(kane=kane, fr=fr, frstar=frstar, mass=mass, forcing=forcing, kanezero=kanezero)
return eom
def _generate_eoms(self):
self.kane = me.KanesMethod(self.frames['inertial'],
list(self.coordinates.values()),
list(self.speeds.values()),
self.kin_diff_eqs)
fr, frstar = self.kane.kanes_equations(
list(self.rigid_bodies.values()), list(self.loads.values()))
fr_plus_frstar = sy.trigsimp(fr + frstar)
sub = {
self.speeds['left_hip'].diff(self.time):
self.accelerations['left_hip'],
self.speeds['left_knee'].diff(self.time):
self.accelerations['left_knee'],
self.speeds['right_hip'].diff(self.time):
self.accelerations['right_hip'],
self.speeds['right_knee'].diff(self.time):
self.accelerations['right_knee']
}
self.fr_plus_frstar = fr_plus_frstar.xreplace(sub)
return self.fr_plus_frstar, self.kane
def test_create_static_html():
# derive simple system
mass, stiffness, damping, gravity = symbols('m, k, c, g')
position, speed = me.dynamicsymbols('x v')
positiond = me.dynamicsymbols('x', 1)
ceiling = me.ReferenceFrame('N')
origin = me.Point('origin')
origin.set_vel(ceiling, 0)
center = origin.locatenew('center', position * ceiling.x)
center.set_vel(ceiling, speed * ceiling.x)
block = me.Particle('block', center, mass)
kinematic_equations = [speed - positiond]
total_force = mass * gravity - stiffness * position - damping * speed
forces = [(center, total_force * ceiling.x)]
particles = [block]
kane = me.KanesMethod(ceiling,
q_ind=[position],
u_ind=[speed],
kd_eqs=kinematic_equations)
kane.kanes_equations(forces, particles)
sys = System(kane,
initial_conditions={
position: 0.1,
speed: -1.0
},
constants={
mass: 1.0,
stiffness: 1.0,
damping: 0.2,
gravity: 9.8
})
# integrate eoms
t = linspace(0.0, 10.0, 100)
sys.times = t
y = sys.integrate()
# create visualization
sphere = Sphere()
viz_frame = VisualizationFrame(ceiling, block, sphere)
scene = Scene(ceiling, origin, viz_frame)
scene.generate_visualization_json_system(sys, outfile_prefix="test")
# test static dir creation
scene.create_static_html(overwrite=True)
assert os.path.exists('static')
assert os.path.exists('static/index.html')
assert os.path.exists('static/test_scene_desc.json')
assert os.path.exists('static/test_simulation_data.json')
# test static dir deletion
scene.remove_static_html(force=True)
assert not os.path.exists('static')
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 setup(self):
"""Setups a simple 1 DoF mass spring damper system visualization."""
mass, stiffness, damping, gravity = symbols('m, k, c, g')
position, speed = me.dynamicsymbols('x v')
positiond = me.dynamicsymbols('x', 1)
kinematic_equations = [speed - positiond]
ceiling = me.ReferenceFrame('N')
origin = me.Point('origin')
origin.set_vel(ceiling, 0)
center = origin.locatenew('center', position * ceiling.x)
center.set_vel(ceiling, speed * ceiling.x)
block = me.Particle('block', center, mass)
particles = [block]
total_force = mass * gravity - stiffness * position - damping * speed
forces = [(center, total_force * ceiling.x)]
kane = me.KanesMethod(ceiling,
q_ind=[position],
u_ind=[speed],
kd_eqs=kinematic_equations)
if sympy_newer_than('1.0'):
kane.kanes_equations(particles, forces)
else:
kane.kanes_equations(forces, particles)
self.sys = System(kane)
self.sys.initial_conditions = {position: 0.1, speed: -1.0}
self.sys.constants = {
mass: 1.0,
stiffness: 2.0,
damping: 3.0,
gravity: 9.8
}
self.sys.times = np.linspace(0.0, 0.01, 2)
sphere = Sphere()
self.ref_frame = ceiling
self.origin = origin
self.viz_frame = VisualizationFrame(ceiling, block, sphere)
self.viz_frame_sym_shape = VisualizationFrame(
ceiling, block, Sphere(radius=mass / 10.0))
def test_create_static_html():
# derive simple system
mass, stiffness, damping, gravity = symbols('m, k, c, g')
position, speed = me.dynamicsymbols('x v')
positiond = me.dynamicsymbols('x', 1)
ceiling = me.ReferenceFrame('N')
origin = me.Point('origin')
origin.set_vel(ceiling, 0)
center = origin.locatenew('center', position * ceiling.x)
center.set_vel(ceiling, speed * ceiling.x)
block = me.Particle('block', center, mass)
kinematic_equations = [speed - positiond]
total_force = mass * gravity - stiffness * position - damping * speed
forces = [(center, total_force * ceiling.x)]
particles = [block]
kane = me.KanesMethod(ceiling,
q_ind=[position],
u_ind=[speed],
kd_eqs=kinematic_equations)
kane.kanes_equations(forces, particles)
mass_matrix = kane.mass_matrix_full
forcing_vector = kane.forcing_full
constants = (mass, stiffness, damping, gravity)
coordinates = (position, )
speeds = (speed, )
system = (mass_matrix, forcing_vector, constants, coordinates, speeds)
# integrate eoms
evaluate_ode = generate_ode_function(*system)
x0 = array([0.1, -1.0])
args = {'constants': array([1.0, 1.0, 0.2, 9.8])}
t = linspace(0.0, 10.0, 100)
y = odeint(evaluate_ode, x0, t, args=(args, ))
# create visualization
sphere = Sphere()
viz_frame = VisualizationFrame(ceiling, block, sphere)
scene = Scene(ceiling, origin, viz_frame)
scene.generate_visualization_json([position, speed], list(constants), y,
args['constants'])
# test static dir creation
scene.create_static_html(overwrite=True)
assert os.path.exists('static')
assert os.path.exists('static/index.html')
assert os.path.exists('static/data.json')
# test static dir deletion
scene.remove_static_html(force=True)
assert not os.path.exists('static')
def generate_equation(self):
"""
New attributes:
rhs : right hand side
:return:
"""
print('symbolic dynamic: generating symbolic equations')
kane = me.KanesMethod(self.frames[0], self.q, self.q_dot, self.kde)
fr, frstar = kane.kanes_equations(self.loads, self.bodies)
mass_matrix = kane.mass_matrix_full
forcing_vector = kane.forcing_full
right_hand_side = generate_ode_function(forcing_vector, self.q, self.q_dot,
[], mass_matrix=mass_matrix,
specifieds=self.tau)
self.kane = kane
self.rhs = right_hand_side
def setup(self):
# This is taken from the example in KanesMethod docstring
# System state variables
q, u = me.dynamicsymbols('q u')
qd, ud = me.dynamicsymbols('q u', 1)
# Other system variables
m, c, k = symbols('m c k')
# Set up the reference frame
N = me.ReferenceFrame('N')
# Set up the point and particle
P = me.Point('P')
P.set_vel(N, u * N.x)
pa = me.Particle('pa', P, m)
# Create the list of kinematic differential equations, force list and
# list of bodies/particles
kd = [qd - u]
force_list = [(P, (-k * q - c * u) * N.x)]
body_list = [pa]
# Create an instance of KanesMethod
self.KM = me.KanesMethod(N, q_ind=[q], u_ind=[u], kd_eqs=kd)
# Account for the new method of input to kanes_equations, i.e. the
# order of the args in the old API is forces, bodies and the new API
# is bodies, forces.
try:
self.KM.kanes_equations(body_list, force_list)
self.first_input = body_list
self.second_input = force_list
except TypeError:
self.first_input = force_list
self.second_input = body_list
def _generate_eoms(self, simplify=False):
self.kane = me.KanesMethod(self.frames['inertial'],
list(self.coordinates.values()),
list(self.speeds.values()),
self.kin_diff_eqs)
fr, frstar = self.kane.kanes_equations(list(self.loads.values()),
list(self.rigid_bodies.values()))
sub = {self.specified['platform_speed'].diff(self.time):
self.specified['platform_acceleration']}
if simplify:
fr_plus_frstar = sy.trigsimp(fr + frstar)
else:
fr_plus_frstar = fr + frstar
self.fr_plus_frstar = fr_plus_frstar.xreplace(sub)
udots = sy.Matrix([s.diff(self.time) for s in self.speeds.values()])
m1 = self.fr_plus_frstar.diff(udots[0])
m2 = self.fr_plus_frstar.diff(udots[1])
# M x' = F
self.mass_matrix = -m1.row_join(m2)
self.forcing_vector = sy.simplify(self.fr_plus_frstar +
self.mass_matrix * udots)
M_top_rows = sy.eye(2).row_join(sy.zeros(2))
F_top_rows = sy.Matrix(list(self.speeds.values()))
tmp = sy.zeros(2).row_join(self.mass_matrix)
self.mass_matrix_full = M_top_rows.col_join(tmp)
self.forcing_vector_full = F_top_rows.col_join(self.forcing_vector)
# Storing the forces and respective points in tuple
damping_1 = (P1, P1_damp)
damping_2 = (P2, P2_damp)
# These are the damping forces acting on the rod and on the plate from the rod
damping_rod = (Z_G, -c_rod * e_dot * B.y)
damping_rod_on_plate = (G, c_rod * e_dot * B.y)
# This is the moment that needs to be applied in order for the plate to stay in
# static equilibrium
moment = (B, Lengths_and_Moments(x, y)[2] * N.z)
# Setting up the coordinates speeds and creating the calling KanesMethod
coordinates = [x, y, beta, e]
speeds = [x_dot, y_dot, beta_dot, e_dot]
kane = me.KanesMethod(N, coordinates, speeds, kde)
loads = [
spring_1_force_P1, spring_2_force_P2, grav_force_plate, grav_force_rod,
spring_force_rod, spring_force_rod_on_plate, damping_rod,
damping_rod_on_plate, damping_1, damping_2, moment
]
fr, frstar = kane.kanes_equations(loads, [Plate, rod])
Mass = kane.mass_matrix_full
f = kane.forcing_full
sys = System(kane)
sys.constants = {
m: mass_of_rod,
def __init__(self, n, lengths=None, masses=1):
#-------------------------------------------------
# Step 1: construct the pendulum 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))
f = mechanics.dynamicsymbols('f:{0}'.format(n))
# mass and length
m = symbols('m:{0}'.format(n))
l = symbols('l:{0}'.format(n))
# gravity and time symbols
g, t = symbols('g,t')
#--------------------------------------------------
# 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 = []
for i in range(n):
# Create a reference frame following the i^th mass
Ai = A.orientnew('A' + str(i), 'Axis', [q[i], A.z])
Ai.set_ang_vel(A, u[i] * A.z)
# Create a point in this reference frame
Pi = P.locatenew('P' + str(i), l[i] * Ai.x)
Pi.v2pt_theory(P, A, Ai)
# Create a new particle of mass m[i] at this point
Pai = mechanics.Particle('Pa' + str(i), Pi, m[i])
particles.append(Pai)
# Set forces & compute kinematic ODE
forces.append((Pi, m[i] * g * A.x))
# Add external torque:
forces.append((Ai, f[i] * A.z))
kinetic_odes.append(q[i].diff(t) - u[i])
P = Pi
# 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)
#-----------------------------------------------------
# Step 3: numerically evaluate equations
# lengths and masses
if lengths is None:
lengths = np.ones(n) / n
lengths = np.broadcast_to(lengths, n)
masses = np.broadcast_to(masses, n)
# Fixed parameters: gravitational constant, lengths, and masses
parameters = [g] + list(l) + list(m)
parameter_vals = [9.81] + list(lengths) + list(masses)
# define symbols for unknown parameters
unknowns = [Dummy() for i in q + u + f]
unknown_dict = dict(zip(q + u + f, unknowns))
kds = KM.kindiffdict()
# substitute unknown symbols for qdot terms
mm_sym = KM.mass_matrix_full.subs(kds).subs(unknown_dict)
fo_sym = KM.forcing_full.subs(kds).subs(unknown_dict)
# create functions for numerical calculation
self.mm_func = lambdify(unknowns + parameters, mm_sym)
self.fo_func = lambdify(unknowns + parameters, fo_sym)
self.args = parameter_vals
A, B, _ = KM.linearize(A_and_B=True)
parameter_dict = dict(zip(parameters, parameter_vals))
self.A = A.subs(parameter_dict)
self.B = B.subs(parameter_dict)
self.state = q + u
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
kane = me.KanesMethod(N,
q_ind=[theta, phi],
u_ind=[omega, alpha],
kd_eqs=kinDiffs)
# calculate Kane's equations
fr, frstar = kane.kanes_equations(forceList, bodyList)
zero = fr + frstar
# solve Kane's equations for the derivatives of the speeds
eom = sym.solvers.solve(zero, omegad, alphad)
# add the kinematical differential equations to get the equations of motion
eom.update(kane.kindiffdict())
# print the results
me.mprint(eom)
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,
psi.diff() - psi1d,
]
coordinates = [x0, y0, z0, phi, theta, psi]
speeds = [u, v, w, phi1d, theta1d, psi1d]
kane = me.KanesMethod(N, coordinates, speeds,
kinematical_differential_equations)
loads = [force, torque]
bodies = [body]
fr, frstar = kane.kanes_equations(bodies, loads)
constants = [I_xx, I_yy, I_zz, mass]
specified = [fx, fy, fz, mx, my, mz] # External force/torque
right_hand_side = generate_ode_function(kane.forcing_full,
coordinates,
speeds,
constants,
mass_matrix=kane.mass_matrix_full,
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
point_pn.set_vel(frame_n, 0)
theta1 = sm.atan(q2 / q1)
frame_a = me.ReferenceFrame('a')
frame_a.orient(frame_n, 'Axis', [theta1, frame_n.z])
particle_p = me.Particle('p', me.Point('p_pt'), sm.Symbol('m'))
particle_p.point.set_pos(point_pn, q1 * frame_n.x + q2 * frame_n.y)
particle_p.mass = m
particle_p.point.set_vel(frame_n,
(point_pn.pos_from(particle_p.point)).dt(frame_n))
f_v = me.dot((particle_p.point.vel(frame_n)).express(frame_a), frame_a.x)
dependent = sm.Matrix([[0]])
dependent[0] = f_v
velocity_constraints = [i for i in dependent]
u_q1d = me.dynamicsymbols('u_q1d')
u_q2d = me.dynamicsymbols('u_q2d')
kd_eqs = [q1d - u_q1d, q2d - u_q2d]
forceList = [(particle_p.point, particle_p.mass * (g * frame_n.x))]
kane = me.KanesMethod(frame_n,
q_ind=[q1, q2],
u_ind=[u_q2d],
u_dependent=[u_q1d],
kd_eqs=kd_eqs,
velocity_constraints=velocity_constraints)
fr, frstar = kane.kanes_equations([particle_p], forceList)
zero = fr + frstar
f_c = point_pn.pos_from(particle_p.point).magnitude() - l
config = sm.Matrix([[0]])
config[0] = f_c
zero = zero.row_insert(zero.shape[0], sm.Matrix([[0]]))
zero[zero.shape[0] - 1] = config[0]
def n_link_pendulum_on_cart(n=1, cart_force=True, joint_torques=False):
"""Returns the system containing the symbolic first order equations of
motion for a 2D n-link pendulum on a sliding cart under the influence of
gravity.
::
|
o y v
\ 0 ^ g
\ |
--\-|----
| \| |
F-> | o --|---> x
| |
---------
o o
Parameters
----------
n : integer
The number of links in the pendulum.
cart_force : boolean, default=True
If true an external specified lateral force is applied to the cart.
joint_torques : boolean, default=False
If true joint torques will be added as specified inputs at each
joint.
Returns
-------
kane : sympy.physics.mechanics.kane.KanesMethod
A KanesMethod object.
Notes
-----
The degrees of freedom of the system are n + 1, i.e. one for each
pendulum link and one for the lateral motion of the cart.
M x' = F, where x = [u0, ..., un+1, q0, ..., qn+1]
The joint angles are all defined relative to the ground where the x axis
defines the ground line and the y axis points up. The joint torques are
applied between each adjacent link and the between the cart and the
lower link where a positive torque corresponds to positive angle.
"""
if n <= 0:
raise ValueError('The number of links must be a positive integer.')
q = me.dynamicsymbols('q:{}'.format(n + 1))
u = me.dynamicsymbols('u:{}'.format(n + 1))
if joint_torques is True:
T = me.dynamicsymbols('T1:{}'.format(n + 1))
m = sm.symbols('m:{}'.format(n + 1))
l = sm.symbols('l:{}'.format(n))
g, t = sm.symbols('g t')
I = me.ReferenceFrame('I')
O = me.Point('O')
O.set_vel(I, 0)
P0 = me.Point('P0')
P0.set_pos(O, q[0] * I.x)
P0.set_vel(I, u[0] * I.x)
Pa0 = me.Particle('Pa0', P0, m[0])
frames = [I]
points = [P0]
particles = [Pa0]
forces = [(P0, -m[0] * g * I.y)]
kindiffs = [q[0].diff(t) - u[0]]
if cart_force is True or joint_torques is True:
specified = []
else:
specified = None
for i in range(n):
Bi = I.orientnew('B{}'.format(i), 'Axis', [q[i + 1], I.z])
Bi.set_ang_vel(I, u[i + 1] * I.z)
frames.append(Bi)
Pi = points[-1].locatenew('P{}'.format(i + 1), l[i] * Bi.y)
Pi.v2pt_theory(points[-1], I, Bi)
points.append(Pi)
Pai = me.Particle('Pa' + str(i + 1), Pi, m[i + 1])
particles.append(Pai)
forces.append((Pi, -m[i + 1] * g * I.y))
if joint_torques is True:
specified.append(T[i])
if i == 0:
forces.append((I, -T[i] * I.z))
if i == n - 1:
forces.append((Bi, T[i] * I.z))
else:
forces.append((Bi, T[i] * I.z - T[i + 1] * I.z))
kindiffs.append(q[i + 1].diff(t) - u[i + 1])
if cart_force is True:
F = me.dynamicsymbols('F')
forces.append((P0, F * I.x))
specified.append(F)
kane = me.KanesMethod(I, q_ind=q, u_ind=u, kd_eqs=kindiffs)
kane.kanes_equations(particles, forces)
return kane
import sympy.physics.mechanics as me
mass, stiffness, damping, gravity = symbols('m, k, c, g')
position, speed = me.dynamicsymbols('x v')
positiond = me.dynamicsymbols('x', 1)
force = me.dynamicsymbols('F')
ceiling = me.ReferenceFrame('N')
origin = me.Point('origin')
origin.set_vel(ceiling, 0)
center = origin.locatenew('center', position * ceiling.x)
center.set_vel(ceiling, speed * ceiling.x)
block = me.Particle('block', center, mass)
kinematic_equations = [speed - positiond]
force_magnitude = mass * gravity - stiffness * position - damping * speed + force
forces = [(center, force_magnitude * ceiling.x)]
particles = [block]
kane = me.KanesMethod(ceiling,
q_ind=[position],
u_ind=[speed],
kd_eqs=kinematic_equations)
kane.kanes_equations(forces, particles)
import sympy as _sm
import math as m
import numpy as _np
m, k, b, g = _sm.symbols('m k b g', real=True)
position, speed = _me.dynamicsymbols('position speed')
position_d, speed_d = _me.dynamicsymbols('position_ speed_', 1)
o = _me.dynamicsymbols('o')
force = o*_sm.sin(_me.dynamicsymbols._t)
frame_ceiling = _me.ReferenceFrame('ceiling')
point_origin = _me.Point('origin')
point_origin.set_vel(frame_ceiling, 0)
particle_block = _me.Particle('block', _me.Point('block_pt'), _sm.Symbol('m'))
particle_block.point.set_pos(point_origin, position*frame_ceiling.x)
particle_block.mass = m
particle_block.point.set_vel(frame_ceiling, speed*frame_ceiling.x)
force_magnitude = m*g-k*position-b*speed+force
force_block = (force_magnitude*frame_ceiling.x).subs({position_d:speed})
kd_eqs = [position_d - speed]
forceList = [(particle_block.point,(force_magnitude*frame_ceiling.x).subs({position_d:speed}))]
kane = _me.KanesMethod(frame_ceiling, q_ind=[position], u_ind=[speed], kd_eqs = kd_eqs)
fr, frstar = kane.kanes_equations([particle_block], forceList)
zero = fr+frstar
from pydy.system import System
sys = System(kane, constants = {m:1.0, k:1.0, b:0.2, g:9.8},
specifieds={_me.dynamicsymbols('t'):lambda x, t: t, o:2},
initial_conditions={position:0.1, speed:-1*1.0},
times = _np.linspace(0.0, 10.0, 10.0/0.01))
y=sys.integrate()
# 自己独立练习的
import sympy as sym
import sympy.physics.mechanics as me
# 定义变量
x, v = me.dynamicsymbols('x v')
m, c, k, g, t = sym.symbols('m c k g t')
# 定义坐标系
ceiling = me.ReferenceFrame('Ceil')
# 定义关键点
O = me.Point('O')
P = me.Point('P')
# 定义点与坐标系的关系
O.set_vel(ceiling, 0)
P.set_pos(O, x * ceiling.x)
P.set_vel(ceiling, v * ceiling.x)
# 外力的表达式，kane方程中将所有的重力视为外力
damping = -c * P.vel(ceiling) # 与天花板的速度
stiffness = -k * P.pos_from(O) # 与O点的距离
gravity = m * g * ceiling.x # 重力方向为x
forces = damping + stiffness + gravity
# 添加质量点
mass = me.particle('mass', P, m)
# Kane方法求解
kane = me.KanesMethod(ceiling, q_ind=[x], u_ind=[v], kd_eqs=[v - x.diff(t)])
fr, frstar = kane.kanes_equations([mass], [(P, forces)])
M = kane.mass_matrix_full
f = kane.forcing_full
u_ind = u[[0, 3, 4]].tolist()
q_dep = q[[1, 2]].tolist()
u_dep = u[[1, 2]].tolist()
q_cons = [q[1] - q[0], q[2] - q[1]]
u_cons = [u[1] - u[0], u[2] - u[1]]
#q_cons = [0, 0]
#u_cons = [0, 0]
# equations of motion using Kane's method
kane = me.KanesMethod(frame=setup['frames'][0],
q_ind=q_ind,
u_ind=u_ind,
kd_eqs=fbd['kd'],
q_dependent=q_dep,
configuration_constraints=q_cons,
u_dependent=u_dep,
velocity_constraints=u_cons)
(fr, frstar) = kane.kanes_equations(fbd['fl'], fbd['bodies'])
kanezero = fr + frstar
mass = kane.mass_matrix_full
forcing = kane.forcing_full
eom = dict(kane=kane,
fr=fr,
frstar=frstar,
mass=mass,
forcing=forcing,
kanzero=kanezero)
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
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,
q[0:13],
u[0:13],
kdes,
configuration_constraints=cfgconstr,
q_dependent=[q[13], q[14]],
u_auxiliary=[u[13], u[14]])
fr, frstar = KM.kanes_equations([body1, body2, body3, body4, body5, body6],
loads)
consts = {
I1[0]: 1.0,
I1[1]: 1.0,
I1[2]: 1.0,
I2[0]: 1.0,
I2[1]: 1.0,
I2[2]: 1.0,
I3[0]: 1.0,
I3[1]: 1.0,
# AB[_i].set_vel(N, AB[_i].vel(N).subs(qdots).simplify())
# BA[_i].set_vel(N, BA[_i].vel(N).subs(qdots).simplify())
rhs = [
eqs[0].rhs, eqs[1].rhs, eqs[2].rhs, eqs[3].rhs, eqs[4].rhs, eqs[5].rhs,
eqs[6].rhs, eqs[7].rhs, eqs[8].rhs, eqs[9].rhs, eqs[10].rhs, eqs[11].rhs
]
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]
]
kane = me.KanesMethod(N, q, u, kd_eqs=kdes)
Fr, Frst = kane.kanes_equations(bodies, loads=loads)
sys = System(kane)
sys.constants = {
I[0]: 1.0,
I[1]: 1.0,
I[2]: 1.0,
I[3]: 1.0,
I[4]: 1.0,
I[5]: 1.0,
L[0]: 0.3,
L[1]: 0.0,
L[2]: 0.5,
Td = (D, T6 * C['2'])
Te = (E, T7 * C['3'])
forces = [Fco, Fdo, Feo, Ffo, Tc, Td, Te]
###############
# Kane's Method
###############
print("Generating Kane's equations.")
kane = mec.KanesMethod(
N,
[q3, q4, q7], # yaw, roll, steer
[u4, u6, u7], # roll rate, rear wheel rate, steer rate
kd_eqs=kinematical,
q_dependent=[q5], # pitch angle
configuration_constraints=[holonomic],
u_dependent=[u3, u5, u8], # yaw rate, pitch rate, front wheel rate
velocity_constraints=nonholonomic)
fr, frstar = kane.kanes_equations(forces, bodies)
# Validation of non-linear equations
print('Loading numerical input parameters.')
from dtk import bicycle
bp = bicycle.benchmark_parameters()
mp = bicycle.benchmark_to_moore(bp)
def multi_mass_spring_damper(n=1,
apply_gravity=False,
apply_external_forces=False):
"""Returns a system containing the symbolic equations of motion and
associated variables for a simple mutli-degree of freedom point mass,
spring, damper system with optional gravitational and external
specified forces. For example, a two mass system under the influence of
gravity and external forces looks like:
::
----------------
| | | | g
\ | | | V
k0 / --- c0 |
| | | x0, v0
--------- V
| m0 | -----
--------- |
| | | |
\ v | | |
k1 / f0 --- c1 |
| | | x1, v1
--------- V
| m1 | -----
---------
| f1
V
Parameters
----------
n : integer
The number of masses in the serial chain.
apply_gravity : boolean
If true, gravity will be applied to each mass.
apply_external_forces : boolean
If true, a time varying external force will be applied to each mass.
Returns
-------
kane : sympy.physics.mechanics.kane.KanesMethod
A KanesMethod object.
"""
mass = sm.symbols('m:{}'.format(n))
stiffness = sm.symbols('k:{}'.format(n))
damping = sm.symbols('c:{}'.format(n))
acceleration_due_to_gravity = sm.symbols('g')
coordinates = me.dynamicsymbols('x:{}'.format(n))
speeds = me.dynamicsymbols('v:{}'.format(n))
specifieds = me.dynamicsymbols('f:{}'.format(n))
ceiling = me.ReferenceFrame('N')
origin = me.Point('origin')
origin.set_vel(ceiling, 0)
points = [origin]
kinematic_equations = []
particles = []
forces = []
for i in range(n):
center = points[-1].locatenew('center{}'.format(i),
coordinates[i] * ceiling.x)
center.set_vel(ceiling,
points[-1].vel(ceiling) + speeds[i] * ceiling.x)
points.append(center)
block = me.Particle('block{}'.format(i), center, mass[i])
kinematic_equations.append(speeds[i] - coordinates[i].diff())
total_force = (-stiffness[i] * coordinates[i] - damping[i] * speeds[i])
try:
total_force += (stiffness[i + 1] * coordinates[i + 1] +
damping[i + 1] * speeds[i + 1])
except IndexError: # no force from below on last mass
pass
if apply_gravity:
total_force += mass[i] * acceleration_due_to_gravity
if apply_external_forces:
total_force += specifieds[i]
forces.append((center, total_force * ceiling.x))
particles.append(block)
kane = me.KanesMethod(ceiling,
q_ind=coordinates,
u_ind=speeds,
kd_eqs=kinematic_equations)
kane.kanes_equations(particles, forces)
return kane
frame_a = _me.ReferenceFrame('a')
frame_b = _me.ReferenceFrame('b')
frame_a.orient(frame_n, 'Axis', [q1, frame_n.z])
frame_b.orient(frame_n, 'Axis', [q2, frame_n.z])
frame_a.set_ang_vel(frame_n, u1*frame_n.z)
frame_b.set_ang_vel(frame_n, u2*frame_n.z)
point_o = _me.Point('o')
particle_p = _me.Particle('p', _me.Point('p_pt'), _sm.Symbol('m'))
particle_r = _me.Particle('r', _me.Point('r_pt'), _sm.Symbol('m'))
particle_p.point.set_pos(point_o, l*frame_a.x)
particle_r.point.set_pos(particle_p.point, l*frame_b.x)
point_o.set_vel(frame_n, 0)
particle_p.point.v2pt_theory(point_o,frame_n,frame_a)
particle_r.point.v2pt_theory(particle_p.point,frame_n,frame_b)
particle_p.mass = m
particle_r.mass = m
force_p = particle_p.mass*(g*frame_n.x)
force_r = particle_r.mass*(g*frame_n.x)
kd_eqs = [q1_d - u1, q2_d - u2]
forceList = [(particle_p.point,particle_p.mass*(g*frame_n.x)), (particle_r.point,particle_r.mass*(g*frame_n.x))]
kane = _me.KanesMethod(frame_n, q_ind=[q1,q2], u_ind=[u1, u2], kd_eqs = kd_eqs)
fr, frstar = kane.kanes_equations([particle_p, particle_r], forceList)
zero = fr+frstar
from pydy.system import System
sys = System(kane, constants = {l:1, m:1, g:9.81},
specifieds={},
initial_conditions={q1:.1, q2:.2, u1:0, u2:0},
times = _np.linspace(0.0, 10, 10/.01))
y=sys.integrate()
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
mb = sm.symbols('mb')
body_b.mass = mb
iaxx = 1/12*ma*(2*la)**2
iayy = iaxx
iazz = 0
ibxx = 1/12*mb*h**2
ibyy = 1/12*mb*(w**2+h**2)
ibzz = 1/12*mb*w**2
body_a.inertia = (me.inertia(body_a_f, iaxx, iayy, iazz, 0, 0, 0), body_a_cm)
body_b.inertia = (me.inertia(body_b_f, ibxx, ibyy, ibzz, 0, 0, 0), body_b_cm)
force_a = body_a.mass*(g*frame_n.z)
force_b = body_b.mass*(g*frame_n.z)
kd_eqs = [thetad - omega, phid - alpha]
forceList = [(body_a.masscenter,body_a.mass*(g*frame_n.z)), (body_b.masscenter,body_b.mass*(g*frame_n.z))]
kane = me.KanesMethod(frame_n, q_ind=[theta,phi], u_ind=[omega, alpha], kd_eqs = kd_eqs)
fr, frstar = kane.kanes_equations([body_a, body_b], forceList)
zero = fr+frstar
from pydy.system import System
sys = System(kane, constants = {g:9.81, lb:0.2, w:0.2, h:0.1, ma:0.01, mb:0.1},
specifieds={},
initial_conditions={theta:np.deg2rad(90), phi:np.deg2rad(0.5), omega:0, alpha:0},
times = np.linspace(0.0, 10, 10/0.02))
y=sys.integrate()
def example_pydy(ax=None, **options):
"""
Example from the documentation of
:epkg:`pydy`.
@param ax matplotlib axis
@parm options extra options
@return ax
"""
# part 1
from sympy import symbols
import sympy.physics.mechanics as me
mass, stiffness, damping, gravity = symbols('m, k, c, g')
position, speed = me.dynamicsymbols('x v')
positiond = me.dynamicsymbols('x', 1)
force = me.dynamicsymbols('F')
ceiling = me.ReferenceFrame('N')
origin = me.Point('origin')
origin.set_vel(ceiling, 0)
center = origin.locatenew('center', position * ceiling.x)
center.set_vel(ceiling, speed * ceiling.x)
block = me.Particle('block', center, mass)
kinematic_equations = [speed - positiond]
force_magnitude = mass * gravity - stiffness * position - damping * speed + force
forces = [(center, force_magnitude * ceiling.x)]
particles = [block]
kane = me.KanesMethod(ceiling,
q_ind=[position],
u_ind=[speed],
kd_eqs=kinematic_equations)
kane.kanes_equations(forces, particles)
# part 2
from numpy import linspace, sin
from pydy.system import System
sys = System(kane,
constants={
mass: 1.0,
stiffness: 1.0,
damping: 0.2,
gravity: 9.8
},
specifieds={
force: lambda x, t: sin(t)
},
initial_conditions={
position: 0.1,
speed: -1.0
},
times=linspace(0.0, 10.0, 1000))
y = sys.integrate()
# part 3
import matplotlib.pyplot as plt
if ax is None:
_, ax = plt.subplots(nrows=1,
ncols=1,
figsize=options.get('figsize', (5, 5)))
ax.plot(sys.times, y)
ax.legend((str(position), str(speed)))
return ax
def integrate_pendulum(n,
times,
initial_positions=135,
initial_velocities=0,
lengths=None,
masses=1):
"""Integrate a multi-pendulum with `n` sections"""
# -------------------------------------------------
# Step 1: construct the pendulum 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))
# mass and length
m = symbols('m:{0}'.format(n))
l = symbols('l:{0}'.format(n))
# gravity and time symbols
g, t = symbols('g,t')
# --------------------------------------------------
# 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 = []
for i in range(n):
# Create a reference frame following the i^th mass
Ai = A.orientnew('A' + str(i), 'Axis', [q[i], A.z])
Ai.set_ang_vel(A, u[i] * A.z)
# Create a point in this reference frame
Pi = P.locatenew('P' + str(i), l[i] * Ai.x)
Pi.v2pt_theory(P, A, Ai)
# Create a new particle of mass m[i] at this point
Pai = mechanics.Particle('Pa' + str(i), Pi, m[i])
particles.append(Pai)
# Set forces & compute kinematic ODE
forces.append((Pi, m[i] * g * A.x))
kinetic_odes.append(q[i].diff(t) - u[i])
P = Pi
# Generate equations of motion
KM = mechanics.KanesMethod(A, q_ind=q, u_ind=u, kd_eqs=kinetic_odes)
fr, fr_star = KM.kanes_equations(forces, particles)
# -----------------------------------------------------
# Step 3: numerically evaluate equations and integrate
# initial positions and velocities – assumed to be given in degrees
y0 = np.deg2rad(
np.concatenate([
np.broadcast_to(initial_positions, n),
np.broadcast_to(initial_velocities, n)
]))
# lengths and masses
if lengths is None:
lengths = np.ones(n) / n
lengths = np.broadcast_to(lengths, n)
masses = np.broadcast_to(masses, n)
# Fixed parameters: gravitational constant, lengths, and masses
parameters = [g] + list(l) + list(m)
parameter_vals = [9.81] + list(lengths) + list(masses)
# define symbols for unknown parameters
unknowns = [Dummy() for i in q + u]
unknown_dict = dict(zip(q + u, unknowns))
kds = KM.kindiffdict()
# substitute unknown symbols for qdot terms
mm_sym = KM.mass_matrix_full.subs(kds).subs(unknown_dict)
fo_sym = KM.forcing_full.subs(kds).subs(unknown_dict)
# create functions for numerical calculation
mm_func = lambdify(unknowns + parameters, mm_sym)
fo_func = lambdify(unknowns + parameters, fo_sym)
# function which computes the derivatives of parameters
def gradient(y, t, args):
vals = np.concatenate((y, args))
sol = np.linalg.solve(mm_func(*vals), fo_func(*vals))
return np.array(sol).T[0]
# ODE integration
return odeint(gradient, y0, times, args=(parameter_vals, ))
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,
q_ind=(theta, phi),
u_ind=(omega, alpha),
kd_eqs=kinDiffs)
# calculate Kane's equations
fr, frstar = kane.kanes_equations(loads, bodies)
sys = System(kane)
sys.constants = {
lB: 0.2, # m
h: 0.1, # m
w: 0.2, # m
mA: 0.01, # kg
mB: 0.1, # kg
g: 9.81, # m/s**2
}
