def _add_piston(self, n, N):
ang = n*2*math.pi/N
frames = [
rx(ang), [
tz(B), [
rx('rod%d-theta' % n, name='rod%d-pivot' % n), [
tz(ROD_LENGTH/2, mass=m0),
tz(ROD_LENGTH), [
rx('piston%d-theta' % n, name='piston%d-base' % n), [
tz(C, name='piston%d-center' % n, mass=m0)
]]]]]]
# Attach the new rod and piston to the master rod's base
self.get_frame('master-pivot').import_frames(frames)
# Add a frame to define the axis of this piston's cylinder.
# The tz() frame is unnecessary, but when using the auto
# drawing visual model, it gives a visual indication of the
# cylinder.
frames = [rx(ang, name='cylinder%d' % n),[tz(CRANK_OFFSET/2)]]
self.import_frames(frames)
# Constrain the piston to the cylinder.
trep.constraints.PointOnPlane(self, 'cylinder%d' % n, (0,1,0), 'piston%d-base' % n)
trep.constraints.PointOnPlane(self, 'cylinder%d' % n, (0,1,0), 'piston%d-center' % n)
# Add the combustion force.
PistonForce(self, 'crank-theta', ang, 'piston%d-center' % n, self.spline)
def whisker_frames(self, curvature, base_pos, base_rot):
""" Creates a list of frames that define the whisker. """
if curvature is None:
ref_angles = np.zeros(self.num_links)
else:
ref_angles = get_angles_from_curvature(self.lengths, curvature)
frames = []
for j in reversed(range(1, self.num_links)):
frames = [tx(self.lengths[j], name='Link-%d' %j), frames]
frames = [rz('theta-%d_z' %j), frames]
frames = [ry('theta-%d_y' %j), frames]
#frames = [rx('theta-%d_x' %j), frames]
frames = [rz(-ref_angles[j]), frames]
frames = [tx(self.lengths[0], name='Link-0'), frames]
frames = [rz('theta-0_z', name='Moving Base Point'), frames]
frames = [ry('theta-0_y'), frames]
frames = [rx('theta-0_x'), frames]
frames = [tz('z'), [ty('y'), [tx('x'), frames]]]
(X, Y, Z) = base_pos
(theta, phi, zeta) = base_rot
# Rotate to the correct position.
frames = [rz(theta), [ry(phi), [rx(zeta), frames]]]
# Place the whisker at the correct spot on the mystacial pad and add an angle
# that drives the whisking motion.
frames = [tx(X), [ty(Y),[ tz(Z, name='Base Point'),
[rz('Driving Angle', name="Driving Angle"), frames]]]]
frames = [ rz(0.0, name='Head'), frames]
return frames
def whisker_frames(self, L):
"""Creates a list of frames that define the whisker."""
frames = []
for j in reversed(range(1, self.num_links)):
frames = [ty(L, name='Link-%d' %j), frames]
frames = [rx('theta-%d' %j, name='f%d' %j), frames]
frames = [rx('curvature-%d' %j, kinematic=True), frames]
frames = [ty(L, name='Link-0'), frames]
frames = [tz('z'), [ty('y', name='Base_Point'), frames]]
frames = [rx('theta-0', name='f0'), frames]
frames = [tz('z0', kinematic=True), [ty('y0', kinematic=True), frames]]
frames = [rx('curvature-0', kinematic=True), frames]
return frames
def __init__(self, cylinders=5):
super(RadialEngine, self).__init__()
# This is the spline we use to define a combustion force.
self.spline = trep.Spline(piston_force_curve)
# Create the crank shaft and master rod
frames = [
rx('crank-theta', mass=m0, name='crank-shaft'),
[
tz(CRANK_OFFSET, name='master-origin'),
[
rx('rod0-theta', name='master-pivot'),
[
tz(B),
[
tz(ROD_LENGTH / 2, mass=m0),
tz(ROD_LENGTH),
[
rx('piston0-theta', name='piston0-base'),
[tz(C, name='piston0-center', mass=m0)]
]
],
],
],
],
rx(0.0, name='cylinder0'),
[tz(CRANK_OFFSET / 2)],
]
# Add the frames to the system.
self.import_frames(frames)
# Add some damping
trep.forces.Damping(self, 0.2)
# Constrain the master piston
trep.constraints.PointOnPlane(self, 'cylinder0', (0, 1, 0),
'piston0-base')
trep.constraints.PointOnPlane(self, 'cylinder0', (0, 1, 0),
'piston0-center')
# Add the combustion force for the master piston.
PistonForce(self, 'crank-theta', 0.0, 'piston0-center', self.spline)
# Add the other pistons.
for i in range(1, cylinders):
self._add_piston(i, cylinders)
def whisker_frames(self, curvature):
""" Creates a list of frames that define the whisker. """
if curvature is None:
ref_angles = np.zeros(self.num_links)
else:
ref_angles = get_angles_from_curvature(self.lengths, curvature)
frames = []
for j in reversed(range(1, self.num_links)):
frames = [ty(self.lengths[j], name='Link-%d' % j), frames]
frames = [rx('theta-%d' % j), frames]
frames = [rx(ref_angles[j]), frames]
frames = [ty(self.lengths[0], name='Link-0'), frames]
frames = [tz('z'), [ty('y', name='Base_Point'), frames]]
frames = [rx('theta-0'), frames]
frames = [rx(ref_angles[0]), frames]
return frames
def whisker_frames(self, curvature):
""" Creates a list of frames that define the whisker. """
if curvature is None:
ref_angles = np.zeros(self.num_links)
else:
ref_angles = get_angles_from_curvature(self.lengths, curvature)
frames = []
for j in reversed(range(1, self.num_links)):
frames = [ty(self.lengths[j], name='Link-%d' %j), frames]
frames = [rx('theta-%d' %j), frames]
frames = [rx(ref_angles[j]), frames]
frames = [ty(self.lengths[0], name='Link-0'), frames]
frames = [tz('z'), [ty('y', name='Base_Point'), frames]]
frames = [rx('theta-0'), frames]
frames = [rx(ref_angles[0]), frames]
return frames
def whisker_frames(self):
"""Creates a list of frames that define the whisker."""
x0, y0, z0, th_x0, th_y0, th_z0 = extract_angles(self._ref)
L = self._link_length
frames = []
for j in reversed(range(1, self.num_links)):
frames = [tx(L, name='Link-%d' %j), frames]
frames = [rz('theta_z-%d' %j), [ry('theta_y-%d' %j), frames]]
frames = [rz(th_z0[j]),[ry(th_y0[j]),frames]]
frames = [tx(L, name='Link-0'), frames]
frames = [tx('xb'), [ty('yb'), [tz('zb', name='Rotated_Base_Point'),
frames]]]
frames = [rz('theta_z-0'), [ry('theta_y-0'), [rx('theta_x-0'),
frames]]]
frames = [rz(th_z0[0]),[ry(th_y0[0]), [rx(th_x0[0]), frames]]]
frames = [tx(x0), [ty(y0), [tz(z0, name='Base_Point'), frames]]]
return frames
def whisker_frames(self, curvature, base_pos, base_rot):
""" Creates a list of frames that define the whisker. """
if curvature is None:
ref_angles = np.zeros(self.num_links)
else:
ref_angles = get_angles_from_curvature(self.lengths, curvature)
frames = []
for j in reversed(range(1, self.num_links)):
frames = [tx(self.lengths[j], name='Link-%d' % j), frames]
frames = [rz('theta-%d_z' % j), frames]
frames = [ry('theta-%d_y' % j), frames]
#frames = [rx('theta-%d_x' %j), frames]
frames = [rz(-ref_angles[j]), frames]
frames = [tx(self.lengths[0], name='Link-0'), frames]
frames = [rz('theta-0_z', name='Moving Base Point'), frames]
frames = [ry('theta-0_y'), frames]
frames = [rx('theta-0_x'), frames]
frames = [tz('z'), [ty('y'), [tx('x'), frames]]]
(X, Y, Z) = base_pos
(theta, phi, zeta) = base_rot
# Rotate to the correct position.
frames = [rz(theta), [ry(phi), [rx(zeta), frames]]]
# Place the whisker at the correct spot on the mystacial pad and add an angle
# that drives the whisking motion.
frames = [
tx(X),
[
ty(Y),
[
tz(Z, name='Base Point'),
[rz('Driving Angle', name="Driving Angle"), frames]
]
]
]
frames = [rz(0.0, name='Head'), frames]
return frames
def build_system():
sys = trep.System()
frames = [
ty('yc',name=CARTFRAME, mass=M), [
rx('theta', name="pendulumShoulder"), [
tz(L, name=MASSFRAME, mass=M)]]]
sys.import_frames(frames)
trep.potentials.Gravity(sys, (0,0,-g))
trep.forces.Damping(sys, B)
trep.forces.ConfigForce(sys,'yc','cart-force')
return sys
def __init__(self, cylinders=5):
super(RadialEngine, self).__init__()
# This is the spline we use to define a combustion force.
self.spline = trep.Spline(piston_force_curve)
# Create the crank shaft and master rod
frames = [
rx('crank-theta', mass=m0, name='crank-shaft'), [
tz(CRANK_OFFSET, name='master-origin'), [
rx('rod0-theta', name='master-pivot'), [
tz(B), [
tz(ROD_LENGTH/2, mass=m0),
tz(ROD_LENGTH),
[
rx('piston0-theta', name='piston0-base'), [
tz(C, name='piston0-center', mass=m0)
]]],
],
],
],
rx(0.0, name='cylinder0'), [tz(CRANK_OFFSET/2)],
]
# Add the frames to the system.
self.import_frames(frames)
# Add some damping
trep.forces.Damping(self, 0.2)
# Constrain the master piston
trep.constraints.PointOnPlane(self, 'cylinder0', (0,1,0), 'piston0-base')
trep.constraints.PointOnPlane(self, 'cylinder0', (0,1,0), 'piston0-center')
# Add the combustion force for the master piston.
PistonForce(self, 'crank-theta', 0.0, 'piston0-center', self.spline)
# Add the other pistons.
for i in range(1, cylinders):
self._add_piston(i,cylinders)
def _add_piston(self, n, N):
ang = n * 2 * math.pi / N
frames = [
rx(ang),
[
tz(B),
[
rx('rod%d-theta' % n, name='rod%d-pivot' % n),
[
tz(ROD_LENGTH / 2, mass=m0),
tz(ROD_LENGTH),
[
rx('piston%d-theta' % n, name='piston%d-base' % n),
[tz(C, name='piston%d-center' % n, mass=m0)]
]
]
]
]
]
# Attach the new rod and piston to the master rod's base
self.get_frame('master-pivot').import_frames(frames)
# Add a frame to define the axis of this piston's cylinder.
# The tz() frame is unnecessary, but when using the auto
# drawing visual model, it gives a visual indication of the
# cylinder.
frames = [rx(ang, name='cylinder%d' % n), [tz(CRANK_OFFSET / 2)]]
self.import_frames(frames)
# Constrain the piston to the cylinder.
trep.constraints.PointOnPlane(self, 'cylinder%d' % n, (0, 1, 0),
'piston%d-base' % n)
trep.constraints.PointOnPlane(self, 'cylinder%d' % n, (0, 1, 0),
'piston%d-center' % n)
# Add the combustion force.
PistonForce(self, 'crank-theta', ang, 'piston%d-center' % n,
self.spline)
def build_system(torque_force=False):
sys = trep.System()
frames = [
tx('xs', name=XCARTFRAME, kinematic=True), [
ty('yc',name=CARTFRAME, mass=M,kinematic=True), [
rx('theta', name="Shoulder1"), [
ry('phi',name="shoulder2"),[
tz(L, name=MASSFRAME, mass=M)]]]]]
sys.import_frames(frames)
trep.potentials.Gravity(sys, (0,0,-g))
trep.forces.Damping(sys, B)
if torque_force:
trep.forces.ConfigForce(sys, 'theta', 'theta-force')
return sys
mvi.initialize_from_configs(0.0, q0, dt, q0)
# This is our simulation loop. We save the results in two lists.
q = [mvi.q2]
t = [mvi.t2]
while mvi.t1 < tf:
mvi.step(mvi.t2 + dt)
q.append(mvi.q2)
t.append(mvi.t2)
return (t, q)
system = trep.System()
system.import_frames([
rx('theta1'), [tz(2, mass=1, name='pend1')],
ty(1), [rx('theta2'), [tz(2, mass=1, name='pend2')]]
])
trep.potentials.LinearSpring(system, 'pend1', 'pend2', k=20, x0=1)
trep.forces.LinearDamper(system, 'pend1', 'pend2', c=1)
trep.potentials.Gravity(system, name="Gravity")
system.q = [3, -3]
# Simulate
start = time.clock()
(t, q) = simulate_system(system)
finish = time.clock()
# Display
# transform from center of geometry of the cover out to the string hooks. The
# base frame has x forward, y out the left side of the robot, and z out the top
# (all in the robot's pov).
cover_to_left_string = [-0.010, 0.039, 0.041]
cover_to_right_string = [-0.010, -0.039, 0.041]
################################################################################
# Now we are ready to define the system
################################################################################
system = trep.System()
frames = [
###### PUPPET ######
tx('TorsoX'), [ty('TorsoY'), [tz('TorsoZ'), [
rz('TorsoPsi'), [ry('TorsoTheta'), [rx('TorsoPhi',name='Torso'), [
tz(-torso_height_2/2, mass=torso_mass),
tx(-torso_width_1/2), [tz(torso_height_3, name='RightShoulderHook')],
tx( torso_width_1/2), [tz(torso_height_3, name= 'LeftShoulderHook')],
tz(torso_height_4, name='Head'), [tz(head_length/2, mass=head_mass)],
# Define the left arm
tx(torso_width/2), [tz(torso_height_1), [
rz('LShoulderPsi'), [ry('LShoulderTheta'), [rx('LShoulderPhi', name='LeftShoulder'), [
tz(-humerus_length/2, name='LeftHumerus', mass=humerus_mass),
tz(-humerus_length), [
rx('LElbowPhi', name='LeftElbow'), [
tz(-radius_length/2, name='LeftRadius', mass=radius_mass),
tz(-radius_length), [
tz(-hand_length/2, mass=hand_mass),
tz(-hand_length, name='LeftFinger')]]]]]]]],
# Define the right arm
"""
This example solves the problem of a multilink pendulum impacting a circle in 2D.
It's goal is to demonstrate how to use the trep_collisions package.
"""
import trep
from trep.visual import *
import trep_collisions as tc
# Define the system in trep. There is gravity and damping at each of the links.
L = .50
M = 1.0
s = trep.System()
frames = ([trep.rx('theta1'),[
trep.tz(-L, mass=M), [trep.rx('theta2'), [
trep.tz(-L, mass=M), [trep.rx('theta3'), [
trep.tz(-L, mass=M), [trep.rx('theta4'), [
trep.tz(-L, mass=M), [trep.rx('theta5'), [
trep.tz(-L, mass=M), [trep.rx('theta6'), [
trep.tz(-.50, mass=1.0)]]]]]]]]]]]])
s.import_frames(frames)
trep.potentials.Gravity(s)
trep.forces.Damping(s, .50)
# Define the collision surface.
surface = tc.surfaces.global_surfaces(tc.surfaces.Circle, s, kwargs={'Y': -2, 'Z': -1.7, 'R': 1.0}, impact_frames=s.masses)
# Create the variational integrator with collisions.
mvi = tc.CollisionMVI(s, surface, release_method='interp', impact_method='root')
#mvi._releases_off = True
import trep
from trep import tx, ty, tz, rx, ry, rz
import trep.potentials
import time
import trep.visual as visual
import math
import numpy as np
from math import sin
dt = 0.01
tf = 10.0
# Set up the system frames
system = trep.System()
system.import_frames([
# Define the 3 Center Links
rx('theta1', kinematic=True),
[
tz(-0.7, name='pend1'),
[
rx('theta2'),
[
tz(-0.7, name='pend2'),
[
rx('theta3'),
[
tz(-0.5, name='legConnection'),
[
tz(-0.5, mass=1, name='COMLeg'),
[tz(-1, name='pend3')]
]
]
mvi = trep.MidpointVI(system)
mvi.initialize_from_configs(0.0, q0, dt, q0)
# This is our simulation loop. We save the results in two lists.
q = [mvi.q2]
t = [mvi.t2]
while mvi.t1 < tf:
mvi.step(mvi.t2+dt)
q.append(mvi.q2)
t.append(mvi.t2)
return (t,q)
system = trep.System()
system.import_frames([
rx('theta1'), [
tz(2, mass=1, name='pend1')
],
ty(1), [
rx('theta2'), [
tz(2, mass=1, name='pend2')
]]
])
trep.potentials.LinearSpring(system, 'pend1', 'pend2', k=20, x0=1)
trep.forces.LinearDamper(system, 'pend1', 'pend2', c=1)
trep.potentials.Gravity(system, name="Gravity")
system.q = [3,-3]
# Simulate
# inputs.
import sys
import trep
from trep import tx, ty, tz, rx, ry, rz
import trep.visual as visual
# Set the length of simulation and the time step.
tf = 10.0
dt = 0.01
# Define the puppet's mechanical structure
system = trep.System()
frames = [
tx('TorsoX'), [ty('TorsoY'), [tz('TorsoZ'), [
rz('TorsoPsi'), [ry('TorsoTheta'), [rx('TorsoPhi',name='Torso'), [
tz(-1.5, mass=50),
tx(-1.011), [tz(0.658, name='Right Torso Hook')],
tx( 1.011), [tz(0.658, name= 'Left Torso Hook')],
tz(0.9, name='Head'), [tz(0.5, mass=(10,1,1,1))],
# Define the left arm
tx(1.3), [tz(0.4), [
rz('LShoulderPsi'), [ry('LShoulderTheta'), [rx('LShoulderPhi', name='Left Shoulder'), [
tz(-0.95, name='Left Humerus', mass=(5,1,1,1)),
tz(-1.9), [
rx('LElbowTheta', name='Left Elbow'), [
tz(-1, name='Left Radius', mass=(4,1,1,1)),
tz(-2.001), [tx(0.14), [ty(-0.173, name='Left Finger')]]]]]]]]],
# Define the right arm
tx(-1.3), [tz(0.4), [
rz('RShoulderPsi'), [ry('RShoulderTheta'), [rx('RShoulderPhi', name='Right Shoulder'), [
MW = 0.05 #kg
Hp = 0.025 #m
d = 0.01 #m
dw = 0.1 #m
LEFTWHISK = "Left whisker"
RIGHTWHISK = "Right whisker"
Aw = math.pi/3
An = math.pi/3 #(math.pi/2.0)
q0= np.array([0,0,0]) #
dq0 = np.array([0,0,0])
system = trep.System()
frames = [#1
rx('theta', name ="neck",kinematic=True),[#2
tz(Hp, name = 'head', mass = MH),[#3
ty(-d,name = "Lcheek"),[
rx('phiL',name = LEFTWHISK, kinematic = True),[
ty(-dw, name = "Lwhisk", mass = MW)]],
ty(d,name = "Rcheek"),[
rx('phiR',name = RIGHTWHISK, kinematic = True),[
ty(dw, name = "Rwhisk", mass = MW)]]]]]
system.import_frames(frames)
trep.potentials.Gravity(system,(0,0,-g))
trep.forces.Damping(system,B)
print "configs",system.configs
def proj_func(x):
L3 = 0.97
L4 = 0.983
L5 = L1
L6 = L2
L7 = 0.692
L8 = 1.018
B1x = 0.573
B1y = 0.082
B2x = B1x
B2y = B1y
# Set up the system frames
system = trep.System()
system.import_frames([
# Define the 3 Center Links
rx('theta1'),
[
tz(-L3, name='pend1'),
[
rx('theta2'),
[
tz(-L4, name='pend2'),
[
rx('theta3'),
[
tz(-L7, name='legConnection'),
[
tz(-(((L7 + L8) / 2) - L7), mass=1, name='COMLeg'),
[tz(-((L7 + L8) / 2), name='pend3')]
]
]
MH = 0.5 #kg
MUL =0.2 #kg
MLL = 0.1 #kg
UL = 0.25 #m
LL = 0.25 #m
HIP = "hip joint"
ULEG = "UpperLeg"
LLEG = "LowerLeg"
system = trep.System()
frames = [#1
ty('yb', name ="body", mass = MH),[#2
rx('theta1', name = HIP,),[#3
tz(-UL ,name = ULEG, mass = MUL),[
rx('theta2', name = 'knee'),[
tz(-LL, name = LLEG, mass = MLL)]]]]]
system.import_frames(frames)
trep.potentials.Gravity(system,(0,0,-g))
trep.forces.Damping(system,B)
trep.forces.ConfigForce(system, 'theta1','hip-torque')
print "configs",system.nu
q0= np.array([0,-np.pi/2,0]) #
dq0 = np.array([0,0,0])
# and Murphey). In general, however, we only need to name frames
# that we reference later in constraints.
import sys
import trep
from trep import tx, ty, tz, rx, ry, rz
import trep.visual as visual
tf = 10.0
dt = 0.01
# Define the mechanical system
system = trep.System()
frames = [
rx('J', name='J'), [
tz(-0.5, name='I', mass=1),
tz(-1), [
rx('H', name='H'), [
tz(-1, name='G', mass=1),
tz(-2, name='O2')]]],
ty(1.5), [
rx('K', name='K'), [
tz(-1, name='L', mass=1),
tz(-2), [
rx('M', name='M'), [
tz(-0.5, name='N', mass=1),
tz(-1.0, name='O')]]]],
ty(-1.5), [
rx('A', name='A'), [
tz(-1, name='B', mass=1),
# final_angles = subchainIK(footPose, lengths)
# print final_angles
# footPose[1] = -0.18
# init_angles = subchainIK(footPose, lengths)
# print init_angles
# set up robot model in stance
hopper_stance = trep.System()
hopper_stance.import_frames([
# center of mass
tz('z_com', mass=0.001, name='CoM_robot'),
[
# middle links
tz(-0.1),
[
rx('motor1', name='motor1'),
[
tz(-0.0843, name='center_link1'),
[
rx('center_theta1'),
[
tz(-0.0856, name='center_link2'),
[
rx('center_theta2'),
[
tz(-0.0691, mass=0, name='center_attach'),
[tz(-0.1016, name='foot', mass=0)]
]
]
]
]
m = 1.0 # Mass of pendulum
l = 1.0 # Length of pendulum
q0 = 0. # Initial configuration of pendulum
t0 = 0.0 # Initial time
tf = 10.0 # Final time
dt = 0.01 # Sampling time
qBar = pi # Desired configuration
Qi = np.eye(2) # Cost weights for states
Ri = np.eye(1) # Cost weights for inputs
uMax = 5.0 # Max absolute input value
tau = 5.0 # Switching time
system = trep.System() # Initialize system
frames = [
rx('theta', name="pendulumShoulder"), [
tz(-l, name="pendulumArm", mass=m)]]
system.import_frames(frames) # Add frames
# Add forces to the system
trep.potentials.Gravity(system, (0, 0, -9.8)) # Add gravity
trep.forces.Damping(system, 1.0) # Add damping
trep.forces.ConfigForce(system, 'theta', 'theta-torque') # Add input
# Create and initialize the variational integrator
mvi = trep.MidpointVI(system)
mvi.initialize_from_configs(t0, np.array([q0]), t0+dt, np.array([q0]))
# Create discrete system
TVec = np.arange(t0, tf+dt, dt) # Initialize discrete time vector
dsys = trep.discopt.DSystem(mvi, TVec) # Initialize discrete system
# This script simulates a damped pendulum with a single link.
import sys
import trep
from trep import tx,ty,tz,rx,ry,rz
import trep.constraints
import trep.visual as visual
tf = 10.0
dt = 0.01
# Here we define the mechanical system
system = trep.System()
frames = [
ty(3), # Provided as an angle reference
rx("theta"), [tz(-3, mass=1)]
]
system.import_frames(frames)
trep.potentials.Gravity(system, (0, 0, -9.8))
trep.forces.Damping(system, 1.2)
# These are the initial conditions for the variational integrator.
q0 = (0.23,) # Note the comma, this is how you create a tuple with
q1 = (0.24,) # a single element
# Now we create and initialize a variational integrator for the system.
mvi = trep.MidpointVI(system)
mvi.initialize_from_configs(0.0, q0, dt, q1)
# This is the actual simulation loop. We will store the results in
# two lists.
import numpy as np
from numpy import dot
from math import sin
import csv
import pylab
csvPath = '/Users/gregniederschulte/Documents/GitHub/Hop3r/trep/hopper_model/JointVelocities.csv'
t0 = 0.0
dt = 0.01
tf = 10.0
# Set up the system frames
system = trep.System()
system.import_frames([
# Define the 3 Center Links
rx('theta1', kinematic=True),[
tz(-0.75, name='pend1'),[
rx('theta2'),[
tz(-0.75, name='pend2'),[
rx('theta3'),[
tz(-0.5, name='legConnection'),[
tz(-0.5, mass=1, name='COMLeg'),[
tz(-1, name='pend3')]]]]]]],
# Define the 2 Right Links
ty(1), [
rx('theta4', kinematic=True),[
tz(-0.5, mass=1, name='COM4'),[
tz(-0.5),[
rx('theta5'),[
tz(-1, name='pend5')]]]]],
# Define the 2 Left Links
return self.k
else:
return 0.0
def V_dqdqdq(self, q1, q2, q3):
return 0.0
tf = 10.0
dt = 0.01
# Here we define the mechanical system
system = trep.System()
frames = [
ty(3), # Provided as an angle reference
rx("theta"), [ty(3, mass=1)]
]
system.import_frames(frames)
#trep.potentials.Gravity(system, (0, 0, -9.8))
PyConfigSpring(system, 'theta', x0=0.7, k=20)
trep.forces.Damping(system, 1.2)
# These are the initial conditions for the variational integrator.
q0 = [0]
q1 = [0]
# Now we create and initialize a variational integrator for the system.
mvi = trep.MidpointVI(system)
mvi.initialize_from_configs(0.0, q0, dt, q1)
# This is the actual simulation loop. We will store the results in
# Build a pendulum system
m = 1.0 # Mass of pendulum
l = 1.0 # Length of pendulum
q0 = (1. / 4. * pi, 3. / 4. * pi) # Initial configuration of pendulum
t0 = 0.0 # Initial time
tf = 5.0 # Final time
dt = 0.1 # Sampling time
qBar = (pi, pi / 2) # Desired configuration
Q = np.eye(4) # Cost weights for states
R = np.eye(2) # Cost weights for inputs
system = trep.System() # Initialize system
frames = [
rx('theta1', name="pendulumShoulder1"),
[
tz(-l, name="pendulumArm1", mass=m),
[
rx('theta2', name="pendulumShoulder2"),
[tz(-l, name="pendulumArm2", mass=m)]
]
]
]
system.import_frames(frames) # Add frames
# Add forces to the system
trep.potentials.Gravity(system, (0, 0, -9.8)) # Add gravity
trep.forces.Damping(system, 1.0) # Add damping
trep.forces.ConfigForce(system, 'theta1',
'theta1-torque') # Add input to theta1
def make_skeleton(dimensions={}, joints={}):
dim = fill_dimensions(dimensions)
joints = fill_joints(joints)
frames = [
tx(joints['torso_tx']), [ty(joints['torso_ty']), [tz(joints['torso_tz']), [
rz(joints['torso_rz']), [ ry(joints['torso_ry']), [
rx(joints['torso_rx'], name='pelvis', mass=dim['pelvis_mass']), [
tx(-dim['lhip_width'], name='lhip'), [
rz(joints['lhip_rz']), [ry(joints['lhip_ry']), [rx(joints['lhip_rx'], name='lfemur'), [
tz(-dim['lfemur_length']/2, name='lfemur_mass', mass=dim['femur_mass']),
tz(-dim['lfemur_length'], name='lfemur_end'), [
rx(joints['lknee_rx'], name='ltibia'), [
tz(-dim['ltibia_length']/2, name='ltibia_mass', mass=dim['tibia_mass']),
tz(-dim['ltibia_length'], name='ltibia_end'), [
rz(joints['lfoot_rz']), [ry(joints['lfoot_ry']), [
rx(joints['lfoot_rx'], name='lfoot'), [
ty(dim['lfoot_length'], name='lfoot_end')]]]]]]]]]],
tx(dim['rhip_width'], name='rhip'), [
rz(joints['rhip_rz']), [ry(joints['rhip_ry']), [rx(joints['rhip_rx'], name='rfemur'), [
tz(-dim['rfemur_length']/2, name='rfemur_mass', mass=dim['femur_mass']),
tz(-dim['rfemur_length'], name='rfemur_end'), [
rx(joints['rknee_rx'], name='rtibia'), [
tz(-dim['rtibia_length']/2, name='rtibia_mass', mass=dim['tibia_mass']),
tz(-dim['rtibia_length'], name='rtibia_end'), [
rz(joints['rfoot_rz']), [ry(joints['rfoot_ry']), [
rx(joints['rfoot_rx'], name='rfoot'), [
ty(dim['rfoot_length'], name='rfoot_end')]]]]]]]]]],
tz(dim['upper_torso_length'], name='spine_top'), [
tz(dim['neck_length'], name='neck'), [
rz(joints['neck_rz']), [ry(joints['neck_ry']), [
rx(joints['neck_rz'], name='neck_joint'), [
tz(dim['lower_head_length'], name='head'), [
tz(dim['upper_head_length'], name='head_center', mass=dim['head_mass']), [
tz(dim['final_head_length'], name='head_end')]]]]]],
tx(-dim['lshoulder_width'], name='lshoulder'), [
rz(joints['lshoulder_rz']), [ry(joints['lshoulder_ry']), [
rx(joints['lshoulder_rx'], name='lhumerus'), [
tz(-dim['lhumerus_length']/2, name='lhumerus_mass', mass=dim['humerus_mass']),
tz(-dim['lhumerus_length'], name='lhumerus_end'), [
rx(joints['lelbow_rx'], name='lradius'), [
tz(-dim['lradius_length']/2, name='lradius_mass', mass=dim['radius_mass']),
tz(-dim['lradius_length'], name='lradius_end'), [
rz(joints['lhand_rz']), [ry(joints['lhand_ry']), [
rx(joints['lhand_rx'], name='lhand'), [
tz(-dim['lhand_length'], name='lhand_end')]]]]]]]]]],
tx(dim['rshoulder_width'], name='rshoulder'), [
rz(joints['rshoulder_rz']), [ry(joints['rshoulder_ry']), [
rx(joints['rshoulder_rx'], name='rhumerus'), [
tz(-dim['rhumerus_length']/2, name='rhumerus_mass', mass=dim['humerus_mass']),
tz(-dim['rhumerus_length'], name='rhumerus_end'), [
rx(joints['relbow_rx'], name='rradius'), [
tz(-dim['rradius_length']/2, name='rradius_mass', mass=dim['radius_mass']),
tz(-dim['rradius_length'], name='rradius_end'), [
rz(joints['rhand_rz']), [ry(joints['rhand_ry']), [
rx(joints['rhand_rx'], name='rhand'), [
tz(-dim['rhand_length'], name='rhand_end')]]]]]]]]]]
]]]]]]]]
return frames
dt = 0.01 # Sampling time
qBar = pi # Desired configuration
Qi = np.eye(4) # Cost weights for states
Ri = np.eye(1) # Cost weights for inputs
uMax = 5.0 # Max absolute input value
tau = 5.0 # Switching time
system = trep.System() # Initialize system
m1 = 1.0367
m2 = 0.5549
l1 = 0.1508
l2 = 0.2667
frames = [
rx('theta', name="pendulumShoulder"),
[
tz(-l, name="pendulumArm", mass=m1),
[
rx('theta2', name="pendulumElbow"),
[tz(-l, name="pendulumForeArm", mass=m2)]
]
]
]
system.import_frames(frames) # Add frames
# Add forces to the system
trep.potentials.Gravity(system, (0, 0, -9.8)) # Add gravity
trep.forces.Damping(system, 0.0) # Add damping
trep.forces.ConfigForce(system, 'theta', 'theta-torque') # Add input
return self.k
else:
return 0.0
def V_dqdqdq(self, q1, q2, q3):
return 0.0
tf = 10.0
dt = 0.01
# Here we define the mechanical system
system = trep.System()
frames = [
ty(3), # Provided as an angle reference
rx("theta"),
[ty(3, mass=1)]
]
system.import_frames(frames)
#trep.potentials.Gravity(system, (0, 0, -9.8))
PyConfigSpring(system, 'theta', x0=0.7, k=20)
trep.forces.Damping(system, 1.2)
# These are the initial conditions for the variational integrator.
q0 = [0]
q1 = [0]
# Now we create and initialize a variational integrator for the system.
mvi = trep.MidpointVI(system)
mvi.initialize_from_configs(0.0, q0, dt, q1)
def make_frames(self, m, a):
return [ty('y'), [tz('z', mass=0.2*m, name='center'),
[rx('theta', mass=(0, m*a**2/6, 0, 0)),
[rx(0.0), [tz(a, mass=0.2*m)],
rx(np.radians(120)), [tz(a, mass=0.2*m)],
rx(np.radians(240)), [tz(a, mass=0.2*m)]]]]]
# Build a pendulum system
m = 1.0 # Mass of pendulum
l = 1.0 # Length of pendulum
q0 = 3./4.*pi # Initial configuration of pendulum
t0 = 0.0 # Initial time
tf = 5.0 # Final time
dt = 0.1 # Sampling time
qBar = pi # Desired configuration
Q = np.eye(2) # Cost weights for states
R = np.eye(1) # Cost weights for inputs
system = trep.System() # Initialize system
frames = [
rx('theta', name="pendulumShoulder"), [
tz(-l, name="pendulumArm", mass=m)]]
system.import_frames(frames) # Add frames
# Add forces to the system
trep.potentials.Gravity(system, (0, 0, -9.8)) # Add gravity
trep.forces.Damping(system, 1.0) # Add damping
trep.forces.ConfigForce(system, 'theta', 'theta-torque') # Add input
# Create and initialize the variational integrator
mvi = trep.MidpointVI(system)
mvi.initialize_from_configs(t0, np.array([q0]), t0+dt, np.array([q0]))
# Create discrete system
TVec = np.arange(t0, tf+dt, dt) # Initialize discrete time vector
dsys = trep.discopt.DSystem(mvi, TVec) # Initialize discrete system
# Now we are ready to define the system
################################################################################
system = trep.System()
frames = [
###### PUPPET ######
tx('TorsoX'),
[
ty('TorsoY'),
[
tz('TorsoZ'),
[
rz('TorsoPsi'),
[
ry('TorsoTheta'),
[
rx('TorsoPhi', name='Torso'),
[
tz(-torso_height_2 / 2, mass=torso_mass),
tx(-torso_width_1 / 2),
[tz(torso_height_3, name='RightShoulderHook')],
tx(torso_width_1 / 2),
[tz(torso_height_3, name='LeftShoulderHook')],
tz(torso_height_4, name='Head'),
[tz(head_length / 2, mass=head_mass)],
# Define the left arm
tx(torso_width / 2),
[
tz(torso_height_1),
[
rz('LShoulderPsi'),
[
from trep import tx,ty,tz,rx,ry,rz
from pylab import *
# System Parameters
tf=10.0 # Final time
dt = 0.01 # Timestep
tc = 0.0 # t_calc for SAC
g = 9.81 # acceleration due to gravity
B=0.002 #damping
M = 0.5 # Mass of Hip (kg)
L = 0.25 # Length of upper leg (m)
HIP = "hip joint"
LEG = "Leg"
system = trep.System() # Initialize system
frames = [#1
rx('theta1', name = HIP,),[#2
tz(-L ,name = LEG, mass = M)]]
system.import_frames(frames) # Add frames
# Add forces to the system
trep.potentials.Gravity(system,(0,0,-g)) # Add gravity
trep.forces.Damping(system,B) # Add damping
trep.forces.ConfigForce(system, 'theta1','hip-torque') # Add input
# a torsional spring instead of gravity.
import sys
import trep
from trep import tx,ty,tz,rx,ry,rz
import trep.constraints
import trep.visual as visual
tf = 10.0
dt = 0.01
# Here we define the mechanical system
system = trep.System()
frames = [
ty(3), # Provided as an angle reference
rx("theta"), [ty(3, mass=1)]
]
system.import_frames(frames)
trep.potentials.ConfigSpring(system, 'theta', k=20, q0=0.6)
# These are the initial conditions for the variational integrator.
q0 = (0,) # Note the comma, this is how you create a tuple with
q1 = (0,) # a single element
# Now we create and initialize a variational integrator for the system.
mvi = trep.MidpointVI(system)
mvi.initialize_from_configs(0.0, q0, dt, q1)
# This is the actual simulation loop. We will store the results in
# two lists.
q = [mvi.q2]
# using a network of linear springs. Three muscles pull on branches
# of the tendon with constant force and direction and the tendon
# settles into a steady state position.
import sys
import trep
from trep import tx, ty, tz, rx, ry, rz
import trep.visual as visual
tf = 10.0
dt = 0.01
# Define the mechanical structure of the tendon.
system = trep.System()
frames = [
rx('theta-1'),
[
tz('x-1', name='A', mass=1),
[
rx('theta-2'),
[
tz('x-2', name='B', mass=1),
[rx('theta-3'), [tz('x-3', name='C', mass=1)]]
],
rx('theta-4'),
[
tz('x-4', name='D', mass=1),
[rx('theta-5'), [tz('x-5', name='E', mass=1)]]
]
]
],
def make_skeleton(dimensions={}, joints={}):
dim = fill_dimensions(dimensions)
joints = fill_joints(joints)
frames = [
tx(joints['torso_tx']),
[
ty(joints['torso_ty']),
[
tz(joints['torso_tz']),
[
rz(joints['torso_rz']),
[
ry(joints['torso_ry']),
[
rx(joints['torso_rx'],
name='pelvis',
mass=dim['pelvis_mass']),
[
tx(-dim['lhip_width'], name='lhip'),
[
rz(joints['lhip_rz']),
[
ry(joints['lhip_ry']),
[
rx(joints['lhip_rx'],
name='lfemur'),
[
tz(-dim['lfemur_length'] / 2,
name='lfemur_mass',
mass=dim['femur_mass']),
tz(-dim['lfemur_length'],
name='lfemur_end'),
[
rx(joints['lknee_rx'],
name='ltibia'),
[
tz(-dim['ltibia_length']
/ 2,
name='ltibia_mass',
mass=dim[
'tibia_mass']),
tz(-dim[
'ltibia_length'],
name='ltibia_end'),
[
rz(joints[
'lfoot_rz']),
[
ry(joints[
'lfoot_ry']
),
[
rx(joints[
'lfoot_rx'],
name=
'lfoot'
),
[
ty(dim[
'lfoot_length'],
name=
'lfoot_end'
)
]
]
]
]
]
]
]
]
]
],
tx(dim['rhip_width'], name='rhip'),
[
rz(joints['rhip_rz']),
[
ry(joints['rhip_ry']),
[
rx(joints['rhip_rx'],
name='rfemur'),
[
tz(-dim['rfemur_length'] / 2,
name='rfemur_mass',
mass=dim['femur_mass']),
tz(-dim['rfemur_length'],
name='rfemur_end'),
[
rx(joints['rknee_rx'],
name='rtibia'),
[
tz(-dim['rtibia_length']
/ 2,
name='rtibia_mass',
mass=dim[
'tibia_mass']),
tz(-dim[
'rtibia_length'],
name='rtibia_end'),
[
rz(joints[
'rfoot_rz']),
[
ry(joints[
'rfoot_ry']
),
[
rx(joints[
'rfoot_rx'],
name=
'r_foot'
),
[
ty(dim[
'rfoot_length'],
name=
'rfoot_end'
)
]
]
]
]
]
]
]
]
]
],
tz(dim['upper_torso_length'],
name='spine_top'),
[
tz(dim['neck_length'], name='neck'),
[
rz(joints['neck_rz']),
[
ry(joints['neck_ry']),
[
rx(joints['neck_rz'],
name='neck_joint'),
[
tz(dim[
'lower_head_length'],
name='head'),
[
tz(dim[
'upper_head_length'],
name='head_center',
mass=dim[
'head_mass']),
[
tz(dim[
'final_head_length'],
name='head_end')
]
]
]
]
]
],
tx(-dim['lshoulder_width'],
name='lshoulder'),
[
rz(joints['lshoulder_rz']),
[
ry(joints['lshoulder_ry']),
[
rx(joints['lshoulder_rx'],
name='lhumerus'),
[
tz(-dim['lhumerus_length']
/ 2,
name='lhumerus_mass',
mass=dim['humerus_mass']
),
tz(-dim['lhumerus_length'],
name='lhumerus_end'),
[
rx(joints['lelbow_rx'],
name='lradius'),
[
tz(-dim[
'lradius_length']
/ 2,
name=
'lradius_mass',
mass=dim[
'radius_mass']
),
tz(-dim[
'lradius_length'],
name=
'lradius_end'),
[
rz(joints[
'lhand_rz']
),
[
ry(joints[
'lhand_ry']
),
[
rx(joints[
'lhand_rx'],
name=
'lhand'
),
[
tz(-dim[
'lhand_length'],
name
='lhand_end'
)
]
]
]
]
]
]
]
]
]
],
tx(dim['rshoulder_width'],
name='rshoulder'),
[
rz(joints['rshoulder_rz']),
[
ry(joints['rshoulder_ry']),
[
rx(joints['rshoulder_rx'],
name='right_humerus'),
[
tz(-dim['rhumerus_length']
/ 2,
name='rhumerus_mass',
mass=dim['humerus_mass']
),
tz(-dim['rhumerus_length'],
name='rhumerus_end'),
[
rx(joints['relbow_rx'],
name='rradius'),
[
tz(-dim[
'rradius_length']
/ 2,
name=
'rradius_mass',
mass=dim[
'radius_mass']
),
tz(-dim[
'rradius_length'],
name=
'rradius_end'),
[
rz(joints[
'rhand_rz']
),
[
ry(joints[
'rhand_ry']
),
[
rx(joints[
'rhand_rx'],
name=
'right_hand'
),
[
tz(-dim[
'rhand_length'],
name
='right_hand_end'
)
]
]
]
]
]
]
]
]
]
]
]
]
]
]
]
]
]
]
return frames
import numpy as np
import trep
from trep import tx, ty, tz, rx, ry, rz
# Build a pendulum system
m = 1.0 # Mass of pendulum
l = 1.0 # Length of pendulum
q0 = 3. / 4. * pi # Initial configuration of pendulum
t0 = 0.0 # Initial time
tf = 5.0 # Final time
dt = 0.1 # Timestep
system = trep.System() # Initialize system
frames = [
rx('theta', name="pendulumShoulder"), [tz(-l, name="pendulumArm", mass=m)]
]
system.import_frames(frames) # Add frames
# Add forces to the system
trep.potentials.Gravity(system, (0, 0, -9.8)) # Add gravity
trep.forces.Damping(system, 1.0) # Add damping
trep.forces.ConfigForce(system, 'theta', 'theta-torque') # Add input
# Create and initialize the variational integrator
mvi = trep.MidpointVI(system)
mvi.initialize_from_configs(t0, np.array([q0]), t0 + dt, np.array([q0]))
# Simulate the system forward
T = [mvi.t1] # List to hold time values
Q = [mvi.q1] # List to hold configuration values
# of the tendon with constant force and direction and the tendon
# settles into a steady state position.
import sys
import trep
from trep import tx, ty, tz, rx, ry, rz
import trep.visual as visual
tf = 10.0
dt = 0.01
# Define the mechanical structure of the tendon.
system = trep.System()
frames = [
rx('theta-1'), [
tz('x-1', name='A', mass=1), [
rx('theta-2'), [
tz('x-2', name='B', mass=1), [
rx('theta-3'), [
tz('x-3', name='C', mass=1)]]],
rx('theta-4'), [
tz('x-4', name='D', mass=1), [
rx('theta-5'), [
tz('x-5', name='E', mass=1)]]]]],
rx('theta-6'), [
tz('x-6', name='F', mass=1), [
rx('theta-8'), [
tz('x-8', name='H', mass=1), [
rx('theta-9'), [
# we have named each frame to correspond to a paper figure (Johnson
# and Murphey). In general, however, we only need to name frames
# that we reference later in constraints.
import sys
import trep
from trep import tx, ty, tz, rx, ry, rz
import trep.visual as visual
tf = 10.0
dt = 0.01
# Define the mechanical system
system = trep.System()
frames = [
rx('J', name='J'),
[
tz(-0.5, name='I', mass=1),
tz(-1),
[rx('H', name='H'), [tz(-1, name='G', mass=1),
tz(-2, name='O2')]]
],
ty(1.5),
[
rx('K', name='K'),
[
tz(-1, name='L', mass=1),
tz(-2),
[
rx('M', name='M'),
[tz(-0.5, name='N', mass=1),
# This is our simulation loop. We save the results in two lists.
q = [mvi.q2]
t = [mvi.t2]
while mvi.t1 < tf:
mvi.step(mvi.t2 + dt, u_func(mvi.t1))
q.append(mvi.q2)
t.append(mvi.t2)
return (t, q)
# Set up the system frames
system = trep.System()
system.import_frames([
# Define the 3 Center Links
rx('theta1'),
[
tz(-0.7, name='pend1'),
[
rx('theta2'),
[
tz(-0.7, name='pend2'),
[
rx('theta3'),
[
tz(-0.4, name='legConnection'),
[
tz(-0.6, mass=1, name='COMLeg'),
[tz(-1, name='pend3')]
]
]