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(torque_force=False):
cart_mass = 1
pendulum_length = 1.0
pendulum_mass = 0.1
system = trep.System()
frames = [
rz('theta', name="pendulumShoulder"),
[
tx(-pendulum_length, name="pendulumArm1", mass=pendulum_mass),
[
rz('x', name="pendulumElbow"),
[
tx(-pendulum_length,
name="pendulumArm2",
mass=pendulum_mass)
]
]
]
]
system.import_frames(frames)
## Because 2D system
trep.potentials.Gravity(system, (0, -9.8, 0))
trep.forces.Damping(system, 10)
trep.forces.ConfigForce(system, 'x', 'x-force')
if torque_force:
trep.forces.ConfigForce(system, 'theta', 'theta-force')
return system
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 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
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 MassSystem2D():
# define system:
system = trep.System()
frames = [
tx('xm', name='x-mass'), [ty('ym', name='y-mass', mass=BALL_MASS)],
ty(1, name='robot_plane'), [tx('xr', name='x-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, 'y-mass', 'x-robot', 'r')
return system
def MassSystem2D():
# define system:
system = trep.System()
frames = [
tx('xm', name='x-mass'), [
ty('ym', name='y-mass', mass=BALL_MASS) ],
ty(1, name='robot_plane'), [
tx('xr', name='x-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, 'y-mass', 'x-robot','r')
return system
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):
"""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(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 build_system(torque_force=False):
cart_mass = 10.0
pendulum_length = 1.0
pendulum_mass = 1.0
system = trep.System()
frames = [
tx('x', name='Cart', mass=cart_mass), [
rz('theta', name="PendulumBase"), [
ty(-pendulum_length, name="Pendulum", mass=pendulum_mass)]]]
system.import_frames(frames)
trep.potentials.Gravity(system, (0, -9.8, 0))
trep.forces.Damping(system, 0.01)
trep.forces.ConfigForce(system, 'x', 'x-force')
if torque_force:
trep.forces.ConfigForce(system, 'theta', 'theta-force')
return system
def build_system(torque_force=False):
cart_mass = 10.0
pendulum_length = 1.0
pendulum_mass = 1.0
system = trep.System()
frames = [
tx('x', name='Cart', mass=cart_mass), [
rz('theta', name="PendulumBase"), [
ty(-pendulum_length, name="Pendulum", mass=pendulum_mass)]]]
system.import_frames(frames)
trep.potentials.Gravity(system, (0, -9.8, 0))
trep.forces.Damping(system, 0.01)
trep.forces.ConfigForce(system, 'x', 'x-force')
if torque_force:
trep.forces.ConfigForce(system, 'theta', 'theta-force')
return system
def make_string_constraints(self):
for name, hook_point in self.string_hooks.iteritems():
info = {
'name' : name,
'x' : name + '-x', # Name of X kinematic config variable
'y' : name + '-y', # Name of Y kinematic config variable
'length' : name + '-length', # Name of length kinematic config variable
# Name of the frames connected by the strings
'control_hook' : name + '_control',
'hook' : hook_point
}
# Add frames from the control_hook
self.string_plane.import_frames([
tx(info['x'], kinematic=True), [
ty(info['y'], kinematic=True, name=info['control_hook'])
]])
trep.constraints.Distance(self, info['hook'],
info['control_hook'], info['length'],
name=name)
self.string_constraints[name] = info
def make_string_constraints(self):
for name, hook_point in self.string_hooks.iteritems():
info = {
'name': name,
'x': name + '-x', # Name of X kinematic config variable
'y': name + '-y', # Name of Y kinematic config variable
'length':
name + '-length', # Name of length kinematic config variable
# Name of the frames connected by the strings
'control_hook': name + '_control',
'hook': hook_point
}
# Add frames from the control_hook
self.string_plane.import_frames([
tx(info['x'], kinematic=True),
[ty(info['y'], kinematic=True, name=info['control_hook'])]
])
trep.constraints.Distance(self,
info['hook'],
info['control_hook'],
info['length'],
name=name)
self.string_constraints[name] = info
# set mass, length, and gravity:
m = 1.0; l = 1.0; g = 9.81; mc = 1.0;
# 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
################################################################################
W = 0.148 / 2.0 # 1/2 the track width in meters
D = 3.0 * 0.0254 # wheel diamater in meters
# 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),
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)
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
# set mass, length, and gravity:
m = 1.0; l = 1.0; g = 9.8;
# create system
system = trep.System()
# define frames
frames = [
trep.rz("theta_1", name="Link1"), [
trep.ty(-l, name="Mass1", mass=m), [
trep.rz("theta_2", name="Link2"), [
trep.ty(-l, name="Mass2", mass=m)]]],
trep.tx(2*l, name="Link3Anchor")]
# add frames to system
system.import_frames(frames)
# add link 3 as a distance constraint
trep.constraints.Distance(system, "Mass2", "Link3Anchor", l)
# set gravity
trep.potentials.Gravity(system, (0, -g, 0))
# add and set torque input on theta_1
trep.forces.ConfigForce(system, "theta_1", "torque1")
system.get_input('torque1').u = 2.0
# solve for equilibrium configuration
# 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), [
D = 3.0*0.0254 # wheel diamater in meters
# 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')]]]]]]]],
import trep
from trep import tx,ty,tz,rx,ry,rz
# Create a sytem with one mass that moves in the x direction.
system = trep.System()
system.import_frames([tx('x', mass=1, name='block')])
trep.potentials.LinearSpring(system, 'World', 'block', k=20, x0=1)
# Remember to avoid x = 0 in simulation.
system.get_config('x').q = 0.5
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
system.get_config('hel-x').q = calc_x(dt)
from trep import tx, ty, tz, rx, ry, rz
from math import sin, cos, pi as mpi, exp
import numpy as np
from numpy import matrix, array
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),
