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 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 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 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 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 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
import trep.discopt as discopt
# set mass, length, and gravity:
m = 1.0; l = 1.0; g = 9.8;
# set state and step conditions:
pk = 0.5 # discrete generalized momentum
qk = 0.2 # theta config
uk = 0.8 # input torque
dt = 0.1 # timestep
# create system
system = trep.System()
# define frames
frames = [
trep.rz('theta_1', name="PendAngle"), [
trep.ty(-l, name="PendMass", mass=m)]]
# add frames to system
system.import_frames(frames)
# add gravity potential
trep.potentials.Gravity(system, (0,-g,0))
# add a torque at the base
trep.forces.ConfigForce(system, 'theta_1', 'tau')
# create and initialize variational integrator
mvi = trep.MidpointVI(system)
mvi.initialize_from_state(0, np.array([qk]), np.array([pk]))
# take single step with VI:
mvi.step(mvi.t1+dt, np.array([uk])) # args are t2, u1
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),
[
# 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 = 10.0
# create system
system = trep.System()
# define frames
frames = [
trep.tx("x_cart", name="CartFrame", mass=mc),
[
trep.rz("theta", name="PendulumBase"),
[trep.ty(l, name="Pendulum", mass=m)]
]
]
# add frames to system
system.import_frames(frames)
# add gravity potential
trep.potentials.Gravity(system, (0, -g, 0))
# add a horizontal force on the cart
trep.forces.ConfigForce(system, "x_cart", "cart_force")
#############
# SAC STUFF #
#############
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
import trep.discopt as discopt
# set mass, length, and gravity:
m = 1.0; l = 1.0; g = 9.8;
# set state and step conditions:
pk = 0.5 # discrete generalized momentum
qk = 0.2 # theta config
uk = 0.8 # input torque
dt = 0.1 # timestep
# create system
system = trep.System()
# define frames
frames = [
trep.rz("theta_1", name="PendAngle"), [
trep.ty(-l, name="PendMass", mass=m)]]
# add frames to system
system.import_frames(frames)
# add gravity potential
trep.potentials.Gravity(system, (0,-g,0))
# add a torque at the base
trep.forces.ConfigForce(system, "theta_1", "tau")
# create and initialize variational integrator
mvi = trep.MidpointVI(system)
mvi.initialize_from_state(0, np.array([qk]), np.array([pk]))
# take single step with VI:
mvi.step(mvi.t1+dt, np.array([uk])) # args are t2, u1
# 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'), [
# 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
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)
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
tx(1.3),
[
