Exemplo n.º 1
0
    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
Exemplo n.º 2
0
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
Exemplo n.º 3
0
 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
Exemplo n.º 4
0
    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
Exemplo n.º 5
0
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
Exemplo n.º 6
0
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)
Exemplo n.º 7
0
# 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))
Exemplo n.º 8
0
# 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'), [
Exemplo n.º 9
0
# 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),
                            [
                                tz(0.4),
                                [
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
sacsys.usat = [[10, -10]]
Exemplo n.º 11
0
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
Exemplo n.º 12
0
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)
Exemplo n.º 13
0
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)
Exemplo n.º 14
0
# 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]