コード例 #1
0
ファイル: radial.py プロジェクト: MurpheyLab/trep
    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)
コード例 #2
0
ファイル: whisker.py プロジェクト: elliothevel/whisker_sim
    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
コード例 #3
0
 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
コード例 #4
0
    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)
コード例 #5
0
ファイル: whisker.py プロジェクト: elliothevel/whisker_sim
    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
コード例 #6
0
ファイル: whisker.py プロジェクト: elliothevel/whisker_sim
    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
コード例 #7
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
コード例 #8
0
ファイル: whisker.py プロジェクト: elliothevel/whisker_sim
    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
コード例 #9
0
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
コード例 #10
0
ファイル: radial.py プロジェクト: MurpheyLab/trep
    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)
コード例 #11
0
    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)
コード例 #12
0
ファイル: trepset.py プロジェクト: fitzsi28/mda_force_cart
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
コード例 #13
0
    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
コード例 #14
0
# 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
コード例 #15
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
コード例 #16
0
import trep
from trep import tx, ty, tz, rx, ry, rz
import trep.potentials
import time
import trep.visual as visual
import math
import numpy as np
from math import sin

dt = 0.01
tf = 10.0
# Set up the system frames
system = trep.System()
system.import_frames([
    # Define the 3 Center Links
    rx('theta1', kinematic=True),
    [
        tz(-0.7, name='pend1'),
        [
            rx('theta2'),
            [
                tz(-0.7, name='pend2'),
                [
                    rx('theta3'),
                    [
                        tz(-0.5, name='legConnection'),
                        [
                            tz(-0.5, mass=1, name='COMLeg'),
                            [tz(-1, name='pend3')]
                        ]
                    ]
コード例 #17
0
ファイル: dual_pendulums.py プロジェクト: MurpheyLab/trep
    mvi = trep.MidpointVI(system)
    mvi.initialize_from_configs(0.0, q0, dt, q0)

    # This is our simulation loop.  We save the results in two lists.
    q = [mvi.q2]
    t = [mvi.t2]
    while mvi.t1 < tf:
        mvi.step(mvi.t2+dt)
        q.append(mvi.q2)
        t.append(mvi.t2)

    return (t,q)

system = trep.System()
system.import_frames([
    rx('theta1'), [
        tz(2, mass=1, name='pend1')
        ],
    ty(1), [
        rx('theta2'), [
            tz(2, mass=1, name='pend2')
            ]]
    ])

trep.potentials.LinearSpring(system, 'pend1', 'pend2', k=20, x0=1)
trep.forces.LinearDamper(system, 'pend1', 'pend2', c=1)
trep.potentials.Gravity(system, name="Gravity")

system.q = [3,-3]

# Simulate
コード例 #18
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'), [
コード例 #19
0
ファイル: rat_trep.py プロジェクト: fitzsi28/CPG_project
MW = 0.05 #kg
Hp = 0.025 #m
d = 0.01 #m
dw = 0.1 #m
LEFTWHISK = "Left whisker"
RIGHTWHISK = "Right whisker"
Aw = math.pi/3
An = math.pi/3 #(math.pi/2.0)

q0= np.array([0,0,0]) #
dq0 = np.array([0,0,0])


system = trep.System()
frames = [#1
    rx('theta', name ="neck",kinematic=True),[#2
        tz(Hp, name = 'head', mass = MH),[#3
            ty(-d,name = "Lcheek"),[
                rx('phiL',name = LEFTWHISK, kinematic = True),[
                    ty(-dw, name = "Lwhisk", mass = MW)]],
            ty(d,name = "Rcheek"),[
                rx('phiR',name = RIGHTWHISK, kinematic = True),[
                    ty(dw, name = "Rwhisk", mass = MW)]]]]]
system.import_frames(frames)

trep.potentials.Gravity(system,(0,0,-g))
trep.forces.Damping(system,B)

print "configs",system.configs

def proj_func(x):
コード例 #20
0
L3 = 0.97
L4 = 0.983
L5 = L1
L6 = L2
L7 = 0.692
L8 = 1.018
B1x = 0.573
B1y = 0.082
B2x = B1x
B2y = B1y

# Set up the system frames
system = trep.System()
system.import_frames([
    # Define the 3 Center Links
    rx('theta1'),
    [
        tz(-L3, name='pend1'),
        [
            rx('theta2'),
            [
                tz(-L4, name='pend2'),
                [
                    rx('theta3'),
                    [
                        tz(-L7, name='legConnection'),
                        [
                            tz(-(((L7 + L8) / 2) - L7), mass=1, name='COMLeg'),
                            [tz(-((L7 + L8) / 2), name='pend3')]
                        ]
                    ]
コード例 #21
0
ファイル: walker.py プロジェクト: fitzsi28/CPG_project
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])
コード例 #22
0
ファイル: pccd.py プロジェクト: MurpheyLab/trep
# and Murphey).  In general, however, we only need to name frames
# that we reference later in constraints.

import sys
import trep
from trep import tx, ty, tz, rx, ry, rz
import trep.visual as visual

tf = 10.0
dt = 0.01


# Define the mechanical system
system = trep.System()
frames = [
    rx('J', name='J'), [
        tz(-0.5, name='I', mass=1),
        tz(-1), [
            rx('H', name='H'), [
                tz(-1, name='G', mass=1),
                tz(-2, name='O2')]]],
    ty(1.5), [
        rx('K', name='K'), [
            tz(-1, name='L', mass=1),
            tz(-2), [
                rx('M', name='M'), [
                    tz(-0.5, name='N', mass=1),
                    tz(-1.0, name='O')]]]],
    ty(-1.5), [
        rx('A', name='A'), [
            tz(-1, name='B', mass=1),
コード例 #23
0
# final_angles = subchainIK(footPose, lengths)
# print final_angles
# footPose[1] = -0.18
# init_angles = subchainIK(footPose, lengths)
# print init_angles

# set up robot model in stance
hopper_stance = trep.System()
hopper_stance.import_frames([
    # center of mass
    tz('z_com', mass=0.001, name='CoM_robot'),
    [
        # middle links
        tz(-0.1),
        [
            rx('motor1', name='motor1'),
            [
                tz(-0.0843, name='center_link1'),
                [
                    rx('center_theta1'),
                    [
                        tz(-0.0856, name='center_link2'),
                        [
                            rx('center_theta2'),
                            [
                                tz(-0.0691, mass=0, name='center_attach'),
                                [tz(-0.1016, name='foot', mass=0)]
                            ]
                        ]
                    ]
                ]
コード例 #24
0
m = 1.0 # Mass of pendulum
l = 1.0 # Length of pendulum
q0 = 0. # Initial configuration of pendulum
t0 = 0.0 # Initial time
tf = 10.0 # Final time
dt = 0.01 # Sampling time
qBar = pi # Desired configuration
Qi = np.eye(2) # Cost weights for states
Ri = np.eye(1) # Cost weights for inputs
uMax = 5.0 # Max absolute input value
tau = 5.0 # Switching time

system = trep.System() # Initialize system

frames = [
    rx('theta', name="pendulumShoulder"), [
        tz(-l, name="pendulumArm", mass=m)]]
system.import_frames(frames) # Add frames

# Add forces to the system
trep.potentials.Gravity(system, (0, 0, -9.8)) # Add gravity
trep.forces.Damping(system, 1.0) # Add damping
trep.forces.ConfigForce(system, 'theta', 'theta-torque') # Add input

# Create and initialize the variational integrator
mvi = trep.MidpointVI(system)
mvi.initialize_from_configs(t0, np.array([q0]), t0+dt, np.array([q0]))

# Create discrete system
TVec = np.arange(t0, tf+dt, dt) # Initialize discrete time vector
dsys = trep.discopt.DSystem(mvi, TVec) # Initialize discrete system
コード例 #25
0
ファイル: damped-pendulum.py プロジェクト: strawburry17/trep
# 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.
コード例 #26
0
import numpy as np
from numpy import dot
from math import sin
import csv
import pylab

csvPath = '/Users/gregniederschulte/Documents/GitHub/Hop3r/trep/hopper_model/JointVelocities.csv'

t0 = 0.0
dt = 0.01
tf = 10.0
# Set up the system frames
system = trep.System()
system.import_frames([
    # Define the 3 Center Links
    rx('theta1', kinematic=True),[
        tz(-0.75, name='pend1'),[
            rx('theta2'),[
                tz(-0.75, name='pend2'),[
                    rx('theta3'),[
                        tz(-0.5, name='legConnection'),[
                            tz(-0.5, mass=1, name='COMLeg'),[
                                tz(-1, name='pend3')]]]]]]],
    # Define the 2 Right Links
    ty(1), [
        rx('theta4', kinematic=True),[
            tz(-0.5, mass=1, name='COM4'),[
                tz(-0.5),[
                    rx('theta5'),[
                        tz(-1, name='pend5')]]]]],
    # Define the 2 Left Links
コード例 #27
0
ファイル: pyconfig-spring.py プロジェクト: MurpheyLab/trep
            return self.k
        else:
            return 0.0

    def V_dqdqdq(self, q1, q2, q3):
        return 0.0

    
tf = 10.0
dt = 0.01

# Here we define the mechanical system
system = trep.System()
frames = [
    ty(3), # Provided as an angle reference
    rx("theta"), [ty(3, mass=1)]
    ]
system.import_frames(frames)
#trep.potentials.Gravity(system, (0, 0, -9.8))
PyConfigSpring(system, 'theta', x0=0.7, k=20)
trep.forces.Damping(system, 1.2)

# These are the initial conditions for the variational integrator.
q0 = [0]
q1 = [0]

# Now we create and initialize a variational integrator for the system.
mvi = trep.MidpointVI(system)
mvi.initialize_from_configs(0.0, q0, dt, q1)

# This is the actual simulation loop.  We will store the results in
コード例 #28
0
# Build a pendulum system
m = 1.0  # Mass of pendulum
l = 1.0  # Length of pendulum
q0 = (1. / 4. * pi, 3. / 4. * pi)  # Initial configuration of pendulum
t0 = 0.0  # Initial time
tf = 5.0  # Final time
dt = 0.1  # Sampling time
qBar = (pi, pi / 2)  # Desired configuration
Q = np.eye(4)  # Cost weights for states
R = np.eye(2)  # Cost weights for inputs

system = trep.System()  # Initialize system

frames = [
    rx('theta1', name="pendulumShoulder1"),
    [
        tz(-l, name="pendulumArm1", mass=m),
        [
            rx('theta2', name="pendulumShoulder2"),
            [tz(-l, name="pendulumArm2", mass=m)]
        ]
    ]
]
system.import_frames(frames)  # Add frames

# Add forces to the system
trep.potentials.Gravity(system, (0, 0, -9.8))  # Add gravity
trep.forces.Damping(system, 1.0)  # Add damping
trep.forces.ConfigForce(system, 'theta1',
                        'theta1-torque')  # Add input to theta1
コード例 #29
0
ファイル: puppets.py プロジェクト: MurpheyLab/trep
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
コード例 #30
0
dt = 0.01  # Sampling time
qBar = pi  # Desired configuration
Qi = np.eye(4)  # Cost weights for states
Ri = np.eye(1)  # Cost weights for inputs
uMax = 5.0  # Max absolute input value
tau = 5.0  # Switching time

system = trep.System()  # Initialize system

m1 = 1.0367
m2 = 0.5549
l1 = 0.1508
l2 = 0.2667

frames = [
    rx('theta', name="pendulumShoulder"),
    [
        tz(-l, name="pendulumArm", mass=m1),
        [
            rx('theta2', name="pendulumElbow"),
            [tz(-l, name="pendulumForeArm", mass=m2)]
        ]
    ]
]

system.import_frames(frames)  # Add frames

# Add forces to the system
trep.potentials.Gravity(system, (0, 0, -9.8))  # Add gravity
trep.forces.Damping(system, 0.0)  # Add damping
trep.forces.ConfigForce(system, 'theta', 'theta-torque')  # Add input
コード例 #31
0
            return self.k
        else:
            return 0.0

    def V_dqdqdq(self, q1, q2, q3):
        return 0.0


tf = 10.0
dt = 0.01

# Here we define the mechanical system
system = trep.System()
frames = [
    ty(3),  # Provided as an angle reference
    rx("theta"),
    [ty(3, mass=1)]
]
system.import_frames(frames)
#trep.potentials.Gravity(system, (0, 0, -9.8))
PyConfigSpring(system, 'theta', x0=0.7, k=20)
trep.forces.Damping(system, 1.2)

# These are the initial conditions for the variational integrator.
q0 = [0]
q1 = [0]

# Now we create and initialize a variational integrator for the system.
mvi = trep.MidpointVI(system)
mvi.initialize_from_configs(0.0, q0, dt, q1)
コード例 #32
0
 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)]]]]]
コード例 #33
0
# 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
コード例 #34
0
# 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'),
                                    [
コード例 #35
0
ファイル: ff_trep.py プロジェクト: fitzsi28/CPG_project
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
コード例 #36
0
# a torsional spring instead of gravity.

import sys
import trep
from trep import tx,ty,tz,rx,ry,rz
import trep.constraints
import trep.visual as visual

tf = 10.0
dt = 0.01

# Here we define the mechanical system
system = trep.System()
frames = [
    ty(3), # Provided as an angle reference
    rx("theta"), [ty(3, mass=1)]
    ]
system.import_frames(frames)
trep.potentials.ConfigSpring(system, 'theta', k=20, q0=0.6)

# These are the initial conditions for the variational integrator.
q0 = (0,)   # Note the comma, this is how you create a tuple with 
q1 = (0,)   # a single element

# Now we create and initialize a variational integrator for the system.
mvi = trep.MidpointVI(system)
mvi.initialize_from_configs(0.0, q0, dt, q1)

# This is the actual simulation loop.  We will store the results in
# two lists.
q = [mvi.q2]
コード例 #37
0
# using a network of linear springs.  Three muscles pull on branches
# of the tendon with constant force and direction and the tendon
# settles into a steady state position.

import sys
import trep
from trep import tx, ty, tz, rx, ry, rz
import trep.visual as visual

tf = 10.0
dt = 0.01

# Define the mechanical structure of the tendon.
system = trep.System()
frames = [
    rx('theta-1'),
    [
        tz('x-1', name='A', mass=1),
        [
            rx('theta-2'),
            [
                tz('x-2', name='B', mass=1),
                [rx('theta-3'), [tz('x-3', name='C', mass=1)]]
            ],
            rx('theta-4'),
            [
                tz('x-4', name='D', mass=1),
                [rx('theta-5'), [tz('x-5', name='E', mass=1)]]
            ]
        ]
    ],
コード例 #38
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
コード例 #39
0
ファイル: pendulumSystem.py プロジェクト: strawburry17/trep
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
コード例 #40
0
# of the tendon with constant force and direction and the tendon
# settles into a steady state position.

import sys
import trep
from trep import tx, ty, tz, rx, ry, rz
import trep.visual as visual


tf = 10.0
dt = 0.01

# Define the mechanical structure of the tendon.
system = trep.System()
frames = [
    rx('theta-1'), [
        tz('x-1', name='A', mass=1), [
            rx('theta-2'), [
                tz('x-2', name='B', mass=1), [
                    rx('theta-3'), [
                        tz('x-3', name='C', mass=1)]]],
            rx('theta-4'), [
                tz('x-4', name='D', mass=1), [
                    rx('theta-5'), [
                        tz('x-5', name='E', mass=1)]]]]],
            
    rx('theta-6'), [
        tz('x-6', name='F', mass=1), [
            rx('theta-8'), [
                tz('x-8', name='H', mass=1), [
                    rx('theta-9'), [
コード例 #41
0
# we have named each frame to correspond to a paper figure (Johnson
# and Murphey).  In general, however, we only need to name frames
# that we reference later in constraints.

import sys
import trep
from trep import tx, ty, tz, rx, ry, rz
import trep.visual as visual

tf = 10.0
dt = 0.01

# Define the mechanical system
system = trep.System()
frames = [
    rx('J', name='J'),
    [
        tz(-0.5, name='I', mass=1),
        tz(-1),
        [rx('H', name='H'), [tz(-1, name='G', mass=1),
                             tz(-2, name='O2')]]
    ],
    ty(1.5),
    [
        rx('K', name='K'),
        [
            tz(-1, name='L', mass=1),
            tz(-2),
            [
                rx('M', name='M'),
                [tz(-0.5, name='N', mass=1),
コード例 #42
0
    # This is our simulation loop.  We save the results in two lists.
    q = [mvi.q2]
    t = [mvi.t2]
    while mvi.t1 < tf:
        mvi.step(mvi.t2 + dt, u_func(mvi.t1))
        q.append(mvi.q2)
        t.append(mvi.t2)
    return (t, q)


# Set up the system frames
system = trep.System()
system.import_frames([
    # Define the 3 Center Links
    rx('theta1'),
    [
        tz(-0.7, name='pend1'),
        [
            rx('theta2'),
            [
                tz(-0.7, name='pend2'),
                [
                    rx('theta3'),
                    [
                        tz(-0.4, name='legConnection'),
                        [
                            tz(-0.6, mass=1, name='COMLeg'),
                            [tz(-1, name='pend3')]
                        ]
                    ]