def make_whisker(dim, q0, L, rbase=100e-6, taper=1.0/15, damping_ratio=None,
rho=1.0, E=3.3e9):
"""
Assembles the whisker based on material properties and initial
configuration. The default material and geometric properties from
elastica2d are used by default.
"""
assert dim in [2,3], "dimension must be 2 or 3"
print 19*'-'+'BUILD WHISKER (%dd)'%dim+19*'-'
print 'Getting parameters...',
if dim==2: N = len(q0)-2
else: N = (len(q0)-4)/2
I = calc_inertia(N, rbase, taper)
K = E*I/L
M = calc_mass(L, N, rho, rbase, taper)
if damping_ratio<0:
damping_ratio = np.append([1.0], get_interp_damping(L,N))
C = calc_damping(N, K, M, L, damping_ratio)
parameters = {'L': L, 'k': K[:-1], 'c': C, 'm': M, 'N':N}
print 'done'
print 'Building system...',
if dim==2:
whisker = Whisker2D(parameters)
whisker.reference_shape = q0
peg_frames = [ty('py', kinematic=True),
[tz('pz', kinematic=True, name='peg_frame')]]
else:
whisker = Whisker3D(parameters, q0)
peg_frames = [tx('px', kinematic=True), [ty('py', kinematic=True),
[tz('pz', kinematic=True, name='peg_frame')]]]
whisker.world_frame.import_frames(peg_frames)
print 'done'
return whisker
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 _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 point_mass_frames(self, num_links, masses):
frames = [tx('x_base'), [ty('y_base'), [tz('z_base', name='Base Point')]] ]
for j in range(num_links):
frames += [tx('x-%d' %j), [ty('y-%d' %j), [tz('z-%d' %j,
mass=masses[j], name='Link-%d' %j)]] ]
frames += [rz(0.0, name='Head')]
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 point_mass_frames(self, num_links, masses):
frames = [
tx('x_base'), [ty('y_base'), [tz('z_base', name='Base Point')]]
]
for j in range(num_links):
frames += [
tx('x-%d' % j),
[
ty('y-%d' % j),
[tz('z-%d' % j, mass=masses[j], name='Link-%d' % j)]
]
]
frames += [rz(0.0, name='Head')]
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 build_system():
system = trep.System()
frames = [
tx('xs', name='x-stylus', kinematic=True), [
ty('ys', name='y-stylus', kinematic=True), [
tz('zs', name='z-stylus', kinematic=True)]],
tx('xm', name='x-mass'), [
ty('ym', name='y-mass'), [
tz('zm', name=MASSFRAME, mass=M)]]]
system.import_frames(frames)#### adds children frame to the world frame system
trep.constraints.Distance(system, MASSFRAME, 'z-stylus', L, name="Link") ###enforces constraint between between the mass frame and the end of the stylus
###use two PointOnPlane constraints to constrain point to a line
###trep.constraints.PointOnPlane(system, MASSFRAME, 'z-stylus', L, name="Link") ###enforces constraint between between the mass frame and the end of the stylus
trep.potentials.Gravity(system, (0,0,-g))
trep.forces.Damping(system, B)
return system
def create_system(self):
# define system:
system = trep.System()
frames = [
tx('xm', name='x-mass'), [
ty('ym', name='y-mass'), [
tz('zm', name='z-mass', mass=self.mass) ]],
ty(h0, name='robot_plane'), [
tx('xr', name='x-robot', kinematic=True), [
tz('zr', name='z-robot', kinematic=True) ]]]
system.import_frames(frames)
trep.potentials.Gravity(system, (0, -g, 0))
trep.forces.Damping(system, 0.05)
# add string constraint as a kinematic configuration var
trep.constraints.Distance(system, 'z-mass', 'z-robot','r')
return system
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 make_string_frames(self):
# Add the string plane
self.world_frame.import_frames(
[tz(self.dimensions['string_plane_height'], name='string_plane')])
self.string_plane = self.get_frame('string_plane')
# Add the string hook frames
self.string_hooks = {}
for name, hook_location in self.dimensions['strings'].iteritems():
self.string_hooks[name] = name + '_hook'
self.get_frame(hook_location[0]).import_frames(
[const_txyz(hook_location[1], name=self.string_hooks[name])])
def make_string_frames(self):
# Add the string plane
self.world_frame.import_frames([
tz(self.dimensions['string_plane_height'], name='string_plane')])
self.string_plane = self.get_frame('string_plane')
# Add the string hook frames
self.string_hooks = {}
for name, hook_location in self.dimensions['strings'].iteritems():
self.string_hooks[name] = name + '_hook'
self.get_frame(hook_location[0]).import_frames([
const_txyz(hook_location[1], name=self.string_hooks[name])])
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
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 __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 __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)
import trep
import trep_collisions as tc
from puppetmvi import PuppetMVI
import numpy as np
# Creat the system and add the frames.
s = trep.System()
s.import_frames([trep.tz('z_ref', name='ref', kinematic=True),[trep.tz('z', mass=1.0, name='ball')]])
trep.potentials.Gravity(s)
# Create the surfaces - the ground and a string connecting the two frames.
collision_surfaces = tc.surfaces.global_surfaces(tc.surfaces.Ground, s, kwargs={'dim': 2}, impact_frames=[s.get_frame('ball')])
strings = [tc.surfaces.Distance(s, 'ref', 'ball', 1.0, invalid='short')]
# Function for kinematically controlling the hand.
def kin_func(t):
if t <= 2.5:
z = 3.0-1.0*t
else:
z = -2.0+1.0*t
return (z, )
pmvi = PuppetMVI(s, collision_surfaces, strings, kin_func=kin_func)
s.get_config('z').q = - 1.0
s.get_config('z_ref').q = 3.0
pmvi.initialize_state(0.0, s.q, [0.0], lambda1=np.zeros(pmvi.nc))
pmvi.add_all_strings()
tf = 6.0
dt = 0.013
(t, q, lam) = tc.simulate(pmvi, tf, dt)
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)]]]]]
import trep
from trep import tx,ty,tz,rx,ry,rz
system = trep.System()
system.import_frames([
ry("theta"), [
tz(2, mass=1)
]])
trep.potentials.ConfigSpring(system, 'theta', k=20, q0=0.7)
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
# Create and initialize the variational integrator
import trep
from trep import tx,ty,tz,rx,ry,rz
system = trep.System()
system.import_frames([
ry('theta1'), [
tz(2, mass=1, name='pend1')
],
tx(1), [
ry('theta2'), [
tz(2, mass=1, name='pend2')
]]
])
trep.potentials.LinearSpring(system, 'pend1', 'pend2', k=20, x0=0.9)
# 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
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
import trep.visual as visual
import math
import numpy as np
from math import sin, cos, exp, pi
from scipy.integrate import odeint
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.35, name='pend1', mass=0.25),
[
tz(-.35),
[
rx('theta2'),
[
tz(-0.35, name='pend2', mass=0.25),
[
tz(-.35),
[
rx('theta3'),
[
tz(-0.5, name='legConnection'),
[
tz(-0.5, mass=0.25, name='COMLeg'),
[tz(-1, name='pend3')]
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
# 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'), [
tz('x-9', name='I', mass=1)]]]]]
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])
mvi=trep.MidpointVI(system)
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
xBar = dsys.build_state(qBar) # Create desired state configuration
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):
x[0] = np.fmod(x[0]+np.pi, 2.0*np.pi)
# 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
xBar = dsys.build_state(qBar) # Create desired state configuration
# 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),
tz(-2), [
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')]
]
]
]
]
# This script shows different ways of initializing the GMVI
# variational integrator
import sys
from math import sin
import trep
from trep import tx,ty,tz,rx,ry,rz
tf = 10.0
dt = 0.01
# Define a simple 2 link pendulum with a force input on the middle joint
system = trep.System()
frames = [
tz(-1), # Provided as an angle reference
ry("theta1"), [
tz(-2, mass=1), [
ry("theta2"), [
tz(-2, mass=1)]]]
]
system.import_frames(frames)
trep.potentials.Gravity(system, (0, 0, -9.8))
trep.forces.Damping(system, 1.2)
trep.forces.ConfigForce(system, 'theta2', 'theta2-torque')
mvi = trep.MidpointVI(system) # Create the variational integrator
# Define a function to run the simulation and return the result
def simulate(mvi, u_func, dt, tf):
q = [mvi.q2]
p = [mvi.p2]
t = [mvi.t2]
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
trep.forces.ConfigForce(system, 'theta2',
'theta2-torque') # Add input to theta2
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()
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')]
]
]
]
]
# This script shows different ways of initializing the GMVI
# variational integrator
import sys
from math import sin
import trep
from trep import tx, ty, tz, rx, ry, rz
tf = 10.0
dt = 0.01
# Define a simple 2 link pendulum with a force input on the middle joint
system = trep.System()
frames = [
tz(-1), # Provided as an angle reference
ry("theta1"),
[tz(-2, mass=1), [ry("theta2"), [tz(-2, mass=1)]]]
]
system.import_frames(frames)
trep.potentials.Gravity(system, (0, 0, -9.8))
trep.forces.Damping(system, 1.2)
trep.forces.ConfigForce(system, 'theta2', 'theta2-torque')
mvi = trep.MidpointVI(system) # Create the variational integrator
# Define a function to run the simulation and return the result
def simulate(mvi, u_func, dt, tf):
q = [mvi.q2]
p = [mvi.p2]
t = [mvi.t2]
while mvi.t1 < tf:
mvi.step(mvi.t2 + dt, u_func(mvi.t1))
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
""" Not working because cosine surface not rewritten yet.
"""
import trep
from trep import ty, tz, rx
from trep.visual import *
import numpy as np
import trep_collisions as tc
# Create the system.
ball = trep.System()
ball.import_frames([ty('y'), [tz('z', mass=1.0)]])
trep.potentials.Gravity(ball)
# Create the surface.
surface = tc.surfaces.Cosine(ball, 1, 1)
# Initialize the integrator.
mvi = tc.CollisionMVI(ball, surface)
# Give the initial condition.
ball.get_config('z').q = 7.0
ball.get_config('y').q = 0.5
mvi.initialize_state(0.0, ball.q, tc.util.D2L2(mvi))
# Simulate.
tf = 5.0
dt = 0.01
(t, q, lam) = tc.simulate(mvi, tf, dt)
# 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.
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
import trep
from trep import tx, ty, tz, rx, ry, rz
system = trep.System()
system.import_frames([ry("theta"), [tz(2, mass=1)]])
trep.potentials.ConfigSpring(system, 'theta', k=20, q0=0.7)
# 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'),
[
# define initial config and velocity
q0 = np.array([0, np.pi]) # q = [x_cart, theta]
dq0 = np.array([0, 0]) # dq = [xdot, thetadot]
# define time parameters:
dt = 0.0167
tf = 2.0
# create system
system = trep.System()
# define frames
frames = [
trep.tx("x_cart", name="CartFrame", mass=mc), [
trep.ry("theta", name="PendulumBase"), [
trep.tz(l, name="Pendulum", mass=m)]]]
# add frames to system
system.import_frames(frames)
# add gravity potential
trep.potentials.Gravity(system, (0,0,-g))
# add a horizontal force on the cart
trep.forces.ConfigForce(system, "x_cart", "cart_force")
sacsys = sactrep.Sac(system)
sacsys.T = 1.2
sacsys.lam = -5
sacsys.maxdt = 0.2
sacsys.ts = dt
sacsys.usat = [[10, -10]]
sacsys.calc_tm = 0.0
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
ty(-1), [
import scipy
from scipy.integrate import odeint
import trep.visual as visual
# Define the length of the simulation and the time step.
tf = 10.0
dt = 0.01
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
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
# This is a basic humanoid puppet with a fixed head. It has no
# 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), [
def h_dq(self, q_i):
if q_i == self.angle_config:
return self.pitch
elif q_i == self.offset_config:
return -1.0
else:
return 0.0
def h_dqdq(self, q_i, q_j):
return 0.0
system = trep.System()
frames = [
tz('hel-x', kinematic=True, name='hel-mid', mass=(1,1,1,1)), [
rz('hel-angle', name='hel-part', mass=(1,1,1,1)), [tx(1)]]
]
# Add the frames to the system.
system.import_frames(frames)
# Add gravity
trep.potentials.Gravity(system, (0, 0, -9.8))
Screw(system, "hel-angle", "hel-x", 1.0/5.0)
# Define a function that we'll use to drive the kinematic variable.
def calc_x(t):
return 0.75 + 0.75*sin(t)
# Calculate an initial condition that is consistent with the constraints.
system.q = 0.0
"""
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
# 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')]
]
# (qU, qtrep, footPose) = geomFK(motor_angles, lengths, 1)
# print footPose
# angles = subchainIK(footPose, lengths)
# footPose[1] = -0.25
# print footPose
# 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'),
