def build_system(torque_force=False): cart_mass = 1 pendulum_length = 1.0 pendulum_mass = 0.1 system = trep.System() frames = [ rz('theta', name="pendulumShoulder"), [ tx(-pendulum_length, name="pendulumArm1", mass=pendulum_mass), [ rz('x', name="pendulumElbow"), [ tx(-pendulum_length, name="pendulumArm2", mass=pendulum_mass) ] ] ] ] system.import_frames(frames) ## Because 2D system trep.potentials.Gravity(system, (0, -9.8, 0)) trep.forces.Damping(system, 10) trep.forces.ConfigForce(system, 'x', 'x-force') if torque_force: trep.forces.ConfigForce(system, 'theta', 'theta-force') return system
def make_skeleton(dimensions={}, joints={}): dim = fill_dimensions(dimensions) joints = fill_joints(joints) frames = [ tx(joints['torso_tx']), [ty(joints['torso_ty']), [tz(joints['torso_tz']), [ rz(joints['torso_rz']), [ ry(joints['torso_ry']), [ rx(joints['torso_rx'], name='pelvis', mass=dim['pelvis_mass']), [ tx(-dim['lhip_width'], name='lhip'), [ rz(joints['lhip_rz']), [ry(joints['lhip_ry']), [rx(joints['lhip_rx'], name='lfemur'), [ tz(-dim['lfemur_length']/2, name='lfemur_mass', mass=dim['femur_mass']), tz(-dim['lfemur_length'], name='lfemur_end'), [ rx(joints['lknee_rx'], name='ltibia'), [ tz(-dim['ltibia_length']/2, name='ltibia_mass', mass=dim['tibia_mass']), tz(-dim['ltibia_length'], name='ltibia_end'), [ rz(joints['lfoot_rz']), [ry(joints['lfoot_ry']), [ rx(joints['lfoot_rx'], name='lfoot'), [ ty(dim['lfoot_length'], name='lfoot_end')]]]]]]]]]], tx(dim['rhip_width'], name='rhip'), [ rz(joints['rhip_rz']), [ry(joints['rhip_ry']), [rx(joints['rhip_rx'], name='rfemur'), [ tz(-dim['rfemur_length']/2, name='rfemur_mass', mass=dim['femur_mass']), tz(-dim['rfemur_length'], name='rfemur_end'), [ rx(joints['rknee_rx'], name='rtibia'), [ tz(-dim['rtibia_length']/2, name='rtibia_mass', mass=dim['tibia_mass']), tz(-dim['rtibia_length'], name='rtibia_end'), [ rz(joints['rfoot_rz']), [ry(joints['rfoot_ry']), [ rx(joints['rfoot_rx'], name='rfoot'), [ ty(dim['rfoot_length'], name='rfoot_end')]]]]]]]]]], tz(dim['upper_torso_length'], name='spine_top'), [ tz(dim['neck_length'], name='neck'), [ rz(joints['neck_rz']), [ry(joints['neck_ry']), [ rx(joints['neck_rz'], name='neck_joint'), [ tz(dim['lower_head_length'], name='head'), [ tz(dim['upper_head_length'], name='head_center', mass=dim['head_mass']), [ tz(dim['final_head_length'], name='head_end')]]]]]], tx(-dim['lshoulder_width'], name='lshoulder'), [ rz(joints['lshoulder_rz']), [ry(joints['lshoulder_ry']), [ rx(joints['lshoulder_rx'], name='lhumerus'), [ tz(-dim['lhumerus_length']/2, name='lhumerus_mass', mass=dim['humerus_mass']), tz(-dim['lhumerus_length'], name='lhumerus_end'), [ rx(joints['lelbow_rx'], name='lradius'), [ tz(-dim['lradius_length']/2, name='lradius_mass', mass=dim['radius_mass']), tz(-dim['lradius_length'], name='lradius_end'), [ rz(joints['lhand_rz']), [ry(joints['lhand_ry']), [ rx(joints['lhand_rx'], name='lhand'), [ tz(-dim['lhand_length'], name='lhand_end')]]]]]]]]]], tx(dim['rshoulder_width'], name='rshoulder'), [ rz(joints['rshoulder_rz']), [ry(joints['rshoulder_ry']), [ rx(joints['rshoulder_rx'], name='rhumerus'), [ tz(-dim['rhumerus_length']/2, name='rhumerus_mass', mass=dim['humerus_mass']), tz(-dim['rhumerus_length'], name='rhumerus_end'), [ rx(joints['relbow_rx'], name='rradius'), [ tz(-dim['rradius_length']/2, name='rradius_mass', mass=dim['radius_mass']), tz(-dim['rradius_length'], name='rradius_end'), [ rz(joints['rhand_rz']), [ry(joints['rhand_ry']), [ rx(joints['rhand_rx'], name='rhand'), [ tz(-dim['rhand_length'], name='rhand_end')]]]]]]]]]] ]]]]]]]] return frames
def point_mass_frames(self, num_links, masses): frames = [tx('x_base'), [ty('y_base'), [tz('z_base', name='Base Point')]] ] for j in range(num_links): frames += [tx('x-%d' %j), [ty('y-%d' %j), [tz('z-%d' %j, mass=masses[j], name='Link-%d' %j)]] ] frames += [rz(0.0, name='Head')] return frames
def whisker_frames(self): """Creates a list of frames that define the whisker.""" x0, y0, z0, th_x0, th_y0, th_z0 = extract_angles(self._ref) L = self._link_length frames = [] for j in reversed(range(1, self.num_links)): frames = [tx(L, name='Link-%d' %j), frames] frames = [rz('theta_z-%d' %j), [ry('theta_y-%d' %j), frames]] frames = [rz(th_z0[j]),[ry(th_y0[j]),frames]] frames = [tx(L, name='Link-0'), frames] frames = [tx('xb'), [ty('yb'), [tz('zb', name='Rotated_Base_Point'), frames]]] frames = [rz('theta_z-0'), [ry('theta_y-0'), [rx('theta_x-0'), frames]]] frames = [rz(th_z0[0]),[ry(th_y0[0]), [rx(th_x0[0]), frames]]] frames = [tx(x0), [ty(y0), [tz(z0, name='Base_Point'), frames]]] return frames
def whisker_frames(self, curvature, base_pos, base_rot): """ Creates a list of frames that define the whisker. """ if curvature is None: ref_angles = np.zeros(self.num_links) else: ref_angles = get_angles_from_curvature(self.lengths, curvature) frames = [] for j in reversed(range(1, self.num_links)): frames = [tx(self.lengths[j], name='Link-%d' %j), frames] frames = [rz('theta-%d_z' %j), frames] frames = [ry('theta-%d_y' %j), frames] #frames = [rx('theta-%d_x' %j), frames] frames = [rz(-ref_angles[j]), frames] frames = [tx(self.lengths[0], name='Link-0'), frames] frames = [rz('theta-0_z', name='Moving Base Point'), frames] frames = [ry('theta-0_y'), frames] frames = [rx('theta-0_x'), frames] frames = [tz('z'), [ty('y'), [tx('x'), frames]]] (X, Y, Z) = base_pos (theta, phi, zeta) = base_rot # Rotate to the correct position. frames = [rz(theta), [ry(phi), [rx(zeta), frames]]] # Place the whisker at the correct spot on the mystacial pad and add an angle # that drives the whisking motion. frames = [tx(X), [ty(Y),[ tz(Z, name='Base Point'), [rz('Driving Angle', name="Driving Angle"), frames]]]] frames = [ rz(0.0, name='Head'), frames] return frames
def point_mass_frames(self, num_links, masses): frames = [ tx('x_base'), [ty('y_base'), [tz('z_base', name='Base Point')]] ] for j in range(num_links): frames += [ tx('x-%d' % j), [ ty('y-%d' % j), [tz('z-%d' % j, mass=masses[j], name='Link-%d' % j)] ] ] frames += [rz(0.0, name='Head')] return frames
def build_system(torque_force=False): cart_mass = 10.0 pendulum_length = 1.0 pendulum_mass = 1.0 system = trep.System() frames = [ tx('x', name='Cart', mass=cart_mass), [ rz('theta', name="PendulumBase"), [ ty(-pendulum_length, name="Pendulum", mass=pendulum_mass)]]] system.import_frames(frames) trep.potentials.Gravity(system, (0, -9.8, 0)) trep.forces.Damping(system, 0.01) trep.forces.ConfigForce(system, 'x', 'x-force') if torque_force: trep.forces.ConfigForce(system, 'theta', 'theta-force') return system
def whisker_frames(self, curvature, base_pos, base_rot): """ Creates a list of frames that define the whisker. """ if curvature is None: ref_angles = np.zeros(self.num_links) else: ref_angles = get_angles_from_curvature(self.lengths, curvature) frames = [] for j in reversed(range(1, self.num_links)): frames = [tx(self.lengths[j], name='Link-%d' % j), frames] frames = [rz('theta-%d_z' % j), frames] frames = [ry('theta-%d_y' % j), frames] #frames = [rx('theta-%d_x' %j), frames] frames = [rz(-ref_angles[j]), frames] frames = [tx(self.lengths[0], name='Link-0'), frames] frames = [rz('theta-0_z', name='Moving Base Point'), frames] frames = [ry('theta-0_y'), frames] frames = [rx('theta-0_x'), frames] frames = [tz('z'), [ty('y'), [tx('x'), frames]]] (X, Y, Z) = base_pos (theta, phi, zeta) = base_rot # Rotate to the correct position. frames = [rz(theta), [ry(phi), [rx(zeta), frames]]] # Place the whisker at the correct spot on the mystacial pad and add an angle # that drives the whisking motion. frames = [ tx(X), [ ty(Y), [ tz(Z, name='Base Point'), [rz('Driving Angle', name="Driving Angle"), frames] ] ] ] frames = [rz(0.0, name='Head'), frames] return frames
import trep.discopt as discopt # set mass, length, and gravity: m = 1.0; l = 1.0; g = 9.8; # set state and step conditions: pk = 0.5 # discrete generalized momentum qk = 0.2 # theta config uk = 0.8 # input torque dt = 0.1 # timestep # create system system = trep.System() # define frames frames = [ trep.rz('theta_1', name="PendAngle"), [ trep.ty(-l, name="PendMass", mass=m)]] # add frames to system system.import_frames(frames) # add gravity potential trep.potentials.Gravity(system, (0,-g,0)) # add a torque at the base trep.forces.ConfigForce(system, 'theta_1', 'tau') # create and initialize variational integrator mvi = trep.MidpointVI(system) mvi.initialize_from_state(0, np.array([qk]), np.array([pk])) # take single step with VI: mvi.step(mvi.t1+dt, np.array([uk])) # args are t2, u1
cover_to_left_string = [-0.010, 0.039, 0.041] cover_to_right_string = [-0.010, -0.039, 0.041] ################################################################################ # Now we are ready to define the system ################################################################################ system = trep.System() frames = [ ###### PUPPET ###### tx('TorsoX'), [ ty('TorsoY'), [ tz('TorsoZ'), [ rz('TorsoPsi'), [ ry('TorsoTheta'), [ rx('TorsoPhi', name='Torso'), [ tz(-torso_height_2 / 2, mass=torso_mass), tx(-torso_width_1 / 2), [tz(torso_height_3, name='RightShoulderHook')], tx(torso_width_1 / 2), [tz(torso_height_3, name='LeftShoulderHook')], tz(torso_height_4, name='Head'), [tz(head_length / 2, mass=head_mass)], # Define the left arm tx(torso_width / 2), [
# define initial config and velocity q0 = np.array([0, np.pi]) # q = [x_cart, theta] dq0 = np.array([0, 0]) # dq = [xdot, thetadot] # define time parameters: dt = 0.0167 tf = 10.0 # create system system = trep.System() # define frames frames = [ trep.tx("x_cart", name="CartFrame", mass=mc), [ trep.rz("theta", name="PendulumBase"), [trep.ty(l, name="Pendulum", mass=m)] ] ] # add frames to system system.import_frames(frames) # add gravity potential trep.potentials.Gravity(system, (0, -g, 0)) # add a horizontal force on the cart trep.forces.ConfigForce(system, "x_cart", "cart_force") ############# # SAC STUFF # #############
def make_skeleton(dimensions={}, joints={}): dim = fill_dimensions(dimensions) joints = fill_joints(joints) frames = [ tx(joints['torso_tx']), [ ty(joints['torso_ty']), [ tz(joints['torso_tz']), [ rz(joints['torso_rz']), [ ry(joints['torso_ry']), [ rx(joints['torso_rx'], name='pelvis', mass=dim['pelvis_mass']), [ tx(-dim['lhip_width'], name='lhip'), [ rz(joints['lhip_rz']), [ ry(joints['lhip_ry']), [ rx(joints['lhip_rx'], name='lfemur'), [ tz(-dim['lfemur_length'] / 2, name='lfemur_mass', mass=dim['femur_mass']), tz(-dim['lfemur_length'], name='lfemur_end'), [ rx(joints['lknee_rx'], name='ltibia'), [ tz(-dim['ltibia_length'] / 2, name='ltibia_mass', mass=dim[ 'tibia_mass']), tz(-dim[ 'ltibia_length'], name='ltibia_end'), [ rz(joints[ 'lfoot_rz']), [ ry(joints[ 'lfoot_ry'] ), [ rx(joints[ 'lfoot_rx'], name= 'lfoot' ), [ ty(dim[ 'lfoot_length'], name= 'lfoot_end' ) ] ] ] ] ] ] ] ] ] ], tx(dim['rhip_width'], name='rhip'), [ rz(joints['rhip_rz']), [ ry(joints['rhip_ry']), [ rx(joints['rhip_rx'], name='rfemur'), [ tz(-dim['rfemur_length'] / 2, name='rfemur_mass', mass=dim['femur_mass']), tz(-dim['rfemur_length'], name='rfemur_end'), [ rx(joints['rknee_rx'], name='rtibia'), [ tz(-dim['rtibia_length'] / 2, name='rtibia_mass', mass=dim[ 'tibia_mass']), tz(-dim[ 'rtibia_length'], name='rtibia_end'), [ rz(joints[ 'rfoot_rz']), [ ry(joints[ 'rfoot_ry'] ), [ rx(joints[ 'rfoot_rx'], name= 'r_foot' ), [ ty(dim[ 'rfoot_length'], name= 'rfoot_end' ) ] ] ] ] ] ] ] ] ] ], tz(dim['upper_torso_length'], name='spine_top'), [ tz(dim['neck_length'], name='neck'), [ rz(joints['neck_rz']), [ ry(joints['neck_ry']), [ rx(joints['neck_rz'], name='neck_joint'), [ tz(dim[ 'lower_head_length'], name='head'), [ tz(dim[ 'upper_head_length'], name='head_center', mass=dim[ 'head_mass']), [ tz(dim[ 'final_head_length'], name='head_end') ] ] ] ] ] ], tx(-dim['lshoulder_width'], name='lshoulder'), [ rz(joints['lshoulder_rz']), [ ry(joints['lshoulder_ry']), [ rx(joints['lshoulder_rx'], name='lhumerus'), [ tz(-dim['lhumerus_length'] / 2, name='lhumerus_mass', mass=dim['humerus_mass'] ), tz(-dim['lhumerus_length'], name='lhumerus_end'), [ rx(joints['lelbow_rx'], name='lradius'), [ tz(-dim[ 'lradius_length'] / 2, name= 'lradius_mass', mass=dim[ 'radius_mass'] ), tz(-dim[ 'lradius_length'], name= 'lradius_end'), [ rz(joints[ 'lhand_rz'] ), [ ry(joints[ 'lhand_ry'] ), [ rx(joints[ 'lhand_rx'], name= 'lhand' ), [ tz(-dim[ 'lhand_length'], name ='lhand_end' ) ] ] ] ] ] ] ] ] ] ], tx(dim['rshoulder_width'], name='rshoulder'), [ rz(joints['rshoulder_rz']), [ ry(joints['rshoulder_ry']), [ rx(joints['rshoulder_rx'], name='right_humerus'), [ tz(-dim['rhumerus_length'] / 2, name='rhumerus_mass', mass=dim['humerus_mass'] ), tz(-dim['rhumerus_length'], name='rhumerus_end'), [ rx(joints['relbow_rx'], name='rradius'), [ tz(-dim[ 'rradius_length'] / 2, name= 'rradius_mass', mass=dim[ 'radius_mass'] ), tz(-dim[ 'rradius_length'], name= 'rradius_end'), [ rz(joints[ 'rhand_rz'] ), [ ry(joints[ 'rhand_ry'] ), [ rx(joints[ 'rhand_rx'], name= 'right_hand' ), [ tz(-dim[ 'rhand_length'], name ='right_hand_end' ) ] ] ] ] ] ] ] ] ] ] ] ] ] ] ] ] ] ] return frames
import numpy as np import trep # set mass, length, and gravity: m = 1.0; l = 1.0; g = 9.8; # create system system = trep.System() # define frames frames = [ trep.rz("theta_1", name="Link1"), [ trep.ty(-l, name="Mass1", mass=m), [ trep.rz("theta_2", name="Link2"), [ trep.ty(-l, name="Mass2", mass=m)]]], trep.tx(2*l, name="Link3Anchor")] # add frames to system system.import_frames(frames) # add link 3 as a distance constraint trep.constraints.Distance(system, "Mass2", "Link3Anchor", l) # set gravity trep.potentials.Gravity(system, (0, -g, 0)) # add and set torque input on theta_1 trep.forces.ConfigForce(system, "theta_1", "torque1") system.get_input('torque1').u = 2.0 # solve for equilibrium configuration
import trep.discopt as discopt # set mass, length, and gravity: m = 1.0; l = 1.0; g = 9.8; # set state and step conditions: pk = 0.5 # discrete generalized momentum qk = 0.2 # theta config uk = 0.8 # input torque dt = 0.1 # timestep # create system system = trep.System() # define frames frames = [ trep.rz("theta_1", name="PendAngle"), [ trep.ty(-l, name="PendMass", mass=m)]] # add frames to system system.import_frames(frames) # add gravity potential trep.potentials.Gravity(system, (0,-g,0)) # add a torque at the base trep.forces.ConfigForce(system, "theta_1", "tau") # create and initialize variational integrator mvi = trep.MidpointVI(system) mvi.initialize_from_state(0, np.array([qk]), np.array([pk])) # take single step with VI: mvi.step(mvi.t1+dt, np.array([uk])) # args are t2, u1
# inputs. import sys import trep from trep import tx, ty, tz, rx, ry, rz import trep.visual as visual # Set the length of simulation and the time step. tf = 10.0 dt = 0.01 # Define the puppet's mechanical structure system = trep.System() frames = [ tx('TorsoX'), [ty('TorsoY'), [tz('TorsoZ'), [ rz('TorsoPsi'), [ry('TorsoTheta'), [rx('TorsoPhi',name='Torso'), [ tz(-1.5, mass=50), tx(-1.011), [tz(0.658, name='Right Torso Hook')], tx( 1.011), [tz(0.658, name= 'Left Torso Hook')], tz(0.9, name='Head'), [tz(0.5, mass=(10,1,1,1))], # Define the left arm tx(1.3), [tz(0.4), [ rz('LShoulderPsi'), [ry('LShoulderTheta'), [rx('LShoulderPhi', name='Left Shoulder'), [ tz(-0.95, name='Left Humerus', mass=(5,1,1,1)), tz(-1.9), [ rx('LElbowTheta', name='Left Elbow'), [ tz(-1, name='Left Radius', mass=(4,1,1,1)), tz(-2.001), [tx(0.14), [ty(-0.173, name='Left Finger')]]]]]]]]], # Define the right arm tx(-1.3), [tz(0.4), [ rz('RShoulderPsi'), [ry('RShoulderTheta'), [rx('RShoulderPhi', name='Right Shoulder'), [
# transform from center of geometry of the cover out to the string hooks. The # base frame has x forward, y out the left side of the robot, and z out the top # (all in the robot's pov). cover_to_left_string = [-0.010, 0.039, 0.041] cover_to_right_string = [-0.010, -0.039, 0.041] ################################################################################ # Now we are ready to define the system ################################################################################ system = trep.System() frames = [ ###### PUPPET ###### tx('TorsoX'), [ty('TorsoY'), [tz('TorsoZ'), [ rz('TorsoPsi'), [ry('TorsoTheta'), [rx('TorsoPhi',name='Torso'), [ tz(-torso_height_2/2, mass=torso_mass), tx(-torso_width_1/2), [tz(torso_height_3, name='RightShoulderHook')], tx( torso_width_1/2), [tz(torso_height_3, name= 'LeftShoulderHook')], tz(torso_height_4, name='Head'), [tz(head_length/2, mass=head_mass)], # Define the left arm tx(torso_width/2), [tz(torso_height_1), [ rz('LShoulderPsi'), [ry('LShoulderTheta'), [rx('LShoulderPhi', name='LeftShoulder'), [ tz(-humerus_length/2, name='LeftHumerus', mass=humerus_mass), tz(-humerus_length), [ rx('LElbowPhi', name='LeftElbow'), [ tz(-radius_length/2, name='LeftRadius', mass=radius_mass), tz(-radius_length), [ tz(-hand_length/2, mass=hand_mass), tz(-hand_length, name='LeftFinger')]]]]]]]], # Define the right arm
def h_dq(self, q_i): if q_i == self.angle_config: return self.pitch elif q_i == self.offset_config: return -1.0 else: return 0.0 def h_dqdq(self, q_i, q_j): return 0.0 system = trep.System() frames = [ tz('hel-x', kinematic=True, name='hel-mid', mass=(1,1,1,1)), [ rz('hel-angle', name='hel-part', mass=(1,1,1,1)), [tx(1)]] ] # Add the frames to the system. system.import_frames(frames) # Add gravity trep.potentials.Gravity(system, (0, 0, -9.8)) Screw(system, "hel-angle", "hel-x", 1.0/5.0) # Define a function that we'll use to drive the kinematic variable. def calc_x(t): return 0.75 + 0.75*sin(t) # Calculate an initial condition that is consistent with the constraints. system.q = 0.0 system.get_config('hel-x').q = calc_x(dt)
import trep.visual as visual # Define the length of the simulation and the time step. tf = 10.0 dt = 0.01 system = trep.System() frames = [ tx('TorsoX'), [ ty('TorsoY'), [ tz('TorsoZ'), [ rz('TorsoPsi'), [ ry('TorsoTheta'), [ rx('TorsoPhi', name='Torso'), [ tz(-1.5, mass=50), tx(-1.011), [tz(0.658, name='Right Torso Hook')], tx(1.011), [tz(0.658, name='Left Torso Hook')], tz(0.9, name='Head'), [tz(0.5, mass=(10, 1, 1, 1))], # Define the left arm tx(1.3), [