def step(self, ac): xposbefore = self.body_abdomen.GetPos().x self.numsteps += 1 if (self.animate): self.myapplication.GetDevice().run() self.myapplication.BeginScene() self.myapplication.DrawAll() self.ac = ac.reshape((-1,)) for i in range(len(self.leg_motor)): action_a = chrono.ChFunction_Const(self.gain*float(self.ac[i])) action_b = chrono.ChFunction_Const(self.gain*float(self.ac[i+4])) self.leg_motor[i].SetTorqueFunction(action_a) self.ankle_motor[i].SetTorqueFunction(action_b) if (self.animate): self.myapplication.DoStep() self.myapplication.EndScene() else: self.ant_sys.DoStepDynamics(self.timestep) obs= self.get_ob() rew = self.calc_rew(xposbefore) self.is_done() return obs, rew, self.isdone, self.info
def step(self, ac): #posbefore = self.body_abdomen.GetPos().x self.numsteps += 1 self.ac = ac.reshape((-1, )) #TODO: do not control each timestep. Frequency not reachable by real world contorllers torques = np.multiply(self.ac, self.maxT) for i, t in enumerate(torques): if self.revs[i].GetRelWvel().z > self.maxSpeed[i] * ( math.pi / 180) and torques[i] > 0: torques[i] = 0 if self.revs[i].GetRelWvel().z < -self.maxSpeed[i] * ( math.pi / 180) and torques[i] < 0: torques[i] = 0 for m, t in zip(self.motors, torques): m.SetTorqueFunction(chrono.ChFunction_Const(float(t))) if (self.animate): self.myapplication.GetDevice().run() self.myapplication.BeginScene() self.myapplication.DrawAll() self.myapplication.DoStep() self.myapplication.EndScene() else: self.robosystem.DoStepDynamics(self.timestep) obs = self.get_ob() rew = self.calc_rew() self.is_done() return obs, rew, self.isdone, self.info
def step(self, ac): xposbefore = self.body_abdomen.GetPos().x self.numsteps += 1 self.ac = ac.reshape((-1,)) for i in range(len(self.leg_motor)): action_a = chrono.ChFunction_Const(self.gain*float(self.ac[i])) action_b = chrono.ChFunction_Const(self.gain*float(self.ac[i+4])) self.leg_motor[i].SetTorqueFunction(action_a) self.ankle_motor[i].SetTorqueFunction(action_b) self.ant_sys.DoStepDynamics(self.timestep) obs= self.get_ob() rew = self.calc_rew(xposbefore) self.is_done() return obs, rew, self.isdone, self.info
def step(self, ac): action = float(ac[0]) self.steps += 1 self.ac = chrono.ChFunction_Const(action) self.actuator.SetForceFunction(self.ac) self.omega = self.pin_joint.GetRelWvel().Length() self.rev_pend_sys.DoStepDynamics(self.timestep) self.rew = 1.0 self.obs = self.get_ob() self.is_done() return self.obs, self.rew, self.isdone, self.info
def step(self, ac): #posbefore = self.body_abdomen.GetPos().x self.numsteps += 1 self.ac = ac.reshape((-1, )) torques = np.multiply(self.ac, self.maxT) for i, t in enumerate(torques): if self.revs[i].GetRelWvel().z > self.maxSpeed[i] * ( math.pi / 180) and torques[i] > 0: torques[i] = 0 if self.revs[i].GetRelWvel().z < -self.maxSpeed[i] * ( math.pi / 180) and torques[i] < 0: torques[i] = 0 for m, t in zip(self.motors, torques): m.SetTorqueFunction(chrono.ChFunction_Const(float(t))) self.hexapod_sys.DoStepDynamics(self.timestep) obs = self.get_ob() rew = self.calc_rew() self.is_done() return obs, rew, self.isdone, self.info
def step(self, ac): action = float(ac[0]) self.steps += 1 self.ac = chrono.ChFunction_Const(action) self.actuator.SetForceFunction(self.ac) self.omega = self.pin_joint.GetRelWvel().Length() if self.render: self.myapplication.GetDevice().run() self.myapplication.BeginScene() self.myapplication.DrawAll() self.myapplication.DoStep() else: self.rev_pend_sys.DoStepDynamics(self.timestep) self.rew = 1.0 self.obs = self.get_ob() if self.render: self.myapplication.EndScene() self.is_done() return self.obs, self.rew, self.isdone, self.info
my_ground = my_system.SearchBody('ground') if not my_ground: sys.exit('Error: cannot find ground from its name in the C::E system!') # ***TRICK*** # Create an engine along the Z direction of the coordsystem specified by # the marker, and acting between shaft and ground revolute_frame = my_marker.GetAbsFrame() link_motor = chrono.ChLinkMotorRotationSpeed() link_motor.Initialize(my_shaft, my_ground, revolute_frame) link_motor.SetSpindleConstraint( chrono.ChLinkMotorRotationSpeed.SpindleConstraint_CYLINDRICAL ) # Set_shaft_mode(chrono.ChLinkEngine.ENG_SHAFT_PRISM) link_motor.SetMotorFunction(chrono.ChFunction_Const( 1.0 * chrono.CH_C_2PI)) # 1.0 Hz to rad/s my_system.Add(link_motor) if m_visualization == "pov": # --------------------------------------------------------------------- # # Render a short animation by generating scripts # to be used with POV-Ray # pov_exporter = postprocess.ChPovRay(my_system) # Sets some file names for in-out processes. pov_exporter.SetTemplateFile("_template_POV.pov") pov_exporter.SetOutputScriptFile("rendering_frames.pov")
mbody_gearA.SetPos(chrono.ChVectorD(0, 0, -1)) mbody_gearA.SetRot(chrono.Q_from_AngX(m.pi / 2)) mbody_gearA.AddAsset(cylinder_texture) # for aesthetic reasons, also add a thin cylinder only as a visualization mshaft_shape = chrono.ChCylinderShape() mshaft_shape.GetCylinderGeometry().p1 = chrono.ChVectorD(0, -3, 0) mshaft_shape.GetCylinderGeometry().p2 = chrono.ChVectorD(0, 10, 0) mshaft_shape.GetCylinderGeometry().rad = radA * 0.4 mbody_gearA.AddAsset(mshaft_shape) # ...impose rotation speed between the first gear and the fixed truss link_motor = chrono.ChLinkMotorRotationSpeed() link_motor.Initialize(mbody_gearA, mbody_truss, chrono.ChFrameD(chrono.ChVectorD(0, 0, 0), chrono.QUNIT)) link_motor.SetSpeedFunction(chrono.ChFunction_Const(6)) mphysicalSystem.AddLink(link_motor) # ...the second gear interaxis12 = radA + radB mbody_gearB = chrono.ChBodyEasyCylinder(radB, 0.4, 1000, True, False, mat) mphysicalSystem.Add(mbody_gearB) mbody_gearB.SetPos(chrono.ChVectorD(interaxis12, 0, -1)) mbody_gearB.SetRot(chrono.Q_from_AngX(m.pi / 2)) mbody_gearB.AddAsset(cylinder_texture) # ...the second gear is fixed to the rotating bar link_revolute = chrono.ChLinkLockRevolute() link_revolute.Initialize( mbody_gearB, mbody_train, chrono.ChCoordsysD(chrono.ChVectorD(interaxis12, 0, 0), chrono.QUNIT))
## TO DO ## 3. Create joint constraints. ## All joint frames are specified in the global frame. ## Define two quaternions representing: ## - a rotation of -90 degrees around x (z2y) ## - a rotation of +90 degrees around y (z2x) z2y = chrono.ChQuaternionD() z2x = chrono.ChQuaternionD() z2y.Q_from_AngAxis(-chrono.CH_C_PI / 2, chrono.ChVectorD(1, 0, 0)) z2x.Q_from_AngAxis(chrono.CH_C_PI / 2, chrono.ChVectorD(0, 1, 0)) ## Create a ChFunction object that always returns the constant value PI/2. fun = chrono.ChFunction_Const() fun.Set_yconst(chrono.CH_C_PI) ## Motor between ground and crank. ## Note that this also acts as a revolute joint (i.e. it enforces the same ## kinematic constraints as a revolute joint). As before, we apply the 'z2y' ## rotation to align the rotation axis with the Y axis of the global frame. engine_ground_crank = chrono.ChLinkMotorRotationSpeed() engine_ground_crank.SetName("engine_ground_crank") engine_ground_crank.Initialize(ground, crank, chrono.ChFrameD(chrono.ChVectorD(0, 0, 0), z2y)) engine_ground_crank.SetSpeedFunction(fun) system.AddLink(engine_ground_crank) ## Prismatic joint between ground and slider. ## The translational axis of a prismatic joint is along the Z axis of the
yd = f_test.Get_y_dx(x) ydd = f_test.Get_y_dxdx(x) test_file.write("%f %f %f %f\n" % (x, y, yd, ydd)) test_file.close() # Function sequence # ----------------- f_seq = chrono.ChFunction_Sequence() f_const_acc1 = chrono.ChFunction_ConstAcc() f_const_acc1.Set_end(0.5) # ramp length f_const_acc1.Set_h(0.3) # ramp height f_seq.InsertFunct(f_const_acc1, 0.5, 1, False, False, False, 0) f_const = chrono.ChFunction_Const() f_seq.InsertFunct(f_const, 0.4, 1, True, False, False, -1) f_const_acc2 = chrono.ChFunction_ConstAcc() f_const_acc2.Set_end(0.6) # ramp length f_const_acc2.Set_av(0.3) # acceleration ends after 30% length f_const_acc2.Set_aw(0.7) # deceleration starts after 70% length f_const_acc2.Set_h(-0.2) # ramp height f_seq.InsertFunct(f_const_acc2, 0.6, 1, True, False, False, -1) f_seq.Setup(); # Evaluate the function and its derivatives at 101 points in [0,2] and write to file seq_file = open(out_dir + "/f_seq.out", "w+") for i in range(101): x = i / 50.0
def __init__(self, sys, gravity, material, width, height, position_base, position_tip, damping, elements, torque=10, origin=True, stator_constraint=None): self.mesh = fea.ChMesh() self.mesh.SetAutomaticGravity(gravity) self.section = fea.ChBeamSectionAdvanced() self.section.SetAsRectangularSection(width, height) self.section.SetYoungModulus(material.modulus) self.section.SetGshearModulus(material.shear) self.section.SetDensity(material.density) self.section.SetBeamRaleyghDamping(damping) self.position_base = chrono.ChVectorD(*position_base) self.position_tip = chrono.ChVectorD(*position_tip) self.builder = fea.ChBuilderBeamEuler( ) #Use the beam builder assembly method to assembly each beam self.builder.BuildBeam( self.mesh, self.section, elements, self.position_base, self.position_tip, chrono.ChVectorD(0, 1, 0), ) self.stator = chrono.ChBodyEasyCylinder(0.01, 0.01, 1000) self.stator.SetPos(self.position_base) self.stator.SetBodyFixed(origin) self.frame = chrono.ChFrameD(self.stator) sys.Add(self.stator) if (origin == False) and ( stator_constraint is not None ): #If the beam is not located at the origin, it must need to be constrained to another beam self.constraint = chrono.ChLinkMateGeneric() self.constraint.Initialize(self.stator, stator_constraint, False, chrono.ChFrameD(self.stator), chrono.ChFrameD(stator_constraint)) self.constraint.SetConstrainedCoords(True, True, True, True, True, True) sys.Add(self.constraint) self.frame = chrono.ChFrameD(self.stator) self.rotor = chrono.ChBodyEasyCylinder(0.011, 0.011, 1000) self.rotor.SetPos(self.position_base) sys.Add(self.rotor) self.frame.SetRot( chrono.Q_from_AngAxis(chrono.CH_C_PI_2, chrono.VECT_X) ) #Rotate the direction of rotation to be planar (x-z plane) self.motor = chrono.ChLinkMotorRotationTorque() self.motor.Initialize(self.rotor, self.stator, self.frame) sys.Add(self.motor) self.motor.SetTorqueFunction(chrono.ChFunction_Const(torque)) self.arm_base = (self.builder.GetLastBeamNodes().front()) self.arm_tip = (self.builder.GetLastBeamNodes().back()) self.mate = chrono.ChLinkMateGeneric() self.mate.Initialize(self.builder.GetLastBeamNodes().front(), self.rotor, chrono.ChFrameD(self.builder.GetLastBeamNodes( ).front().GetPos())) #constrain beam to rotor self.mate.SetConstrainedCoords( True, True, True, True, True, True ) #constraints must be in format: (True,True,True,True,True,True) to constrain x,y,z,rotx,roty,rotz coordinates sys.Add(self.mate) self.visual = fea.ChVisualizationFEAmesh(self.mesh) self.visual.SetFEMdataType( fea.ChVisualizationFEAmesh.E_PLOT_ELEM_BEAM_MZ) self.visual.SetColorscaleMinMax(-1.4, 1.4) self.visual.SetSmoothFaces(True) self.visual.SetWireframe(False) self.mesh.AddAsset(self.visual) sys.Add(self.mesh)
def motor_torque(arm): #define function which passes torque value to motors) arm.motor.SetTorqueFunction( chrono.ChFunction_Const(random.randrange(-10, 10, 1) * 5) ) #Insert function which calls value from agent here for motor torques
def set_torque(self, torque): self.motor.SetTorqueFunction(chrono.ChFunction_Const(torque))
def make_model(system): # write_xml() # Make ground body ground = make_box([1, 1, 10], [0, 0, -10], fixed=True, color=(1., 0.5, 0)) system.Add(ground) # Make main body main_body = make_box(geometry.body_size, geometry.body_center, fixed=False) system.Add(main_body) # Make right links right_recip = make_box(geometry.recip_link_size, geometry.right_recip_link_center, fixed=False, euler_angles=geometry.recip_link_euler) system.Add(right_recip) right_connector = make_box(geometry.connector_link_size, geometry.right_connector_link_center, fixed=False, euler_angles=geometry.connector_link_euler) system.Add(right_connector) right_thigh = make_box(geometry.thigh_link_size, geometry.right_thigh_link_center, fixed=False, euler_angles=geometry.thigh_link_euler) system.Add(right_thigh) right_shin = make_box(geometry.shin_link_size, geometry.right_shin_link_center, fixed=False, euler_angles=geometry.shin_link_euler) system.Add(right_shin) right_4bar = make_box(geometry.four_bar_link_size, geometry.right_4bar_link_center, fixed=False, euler_angles=geometry.four_bar_link_euler) system.Add(right_4bar) right_ankle1 = make_box(geometry.ankle_link_size1, geometry.right_ankle_link_center1, fixed=False, collide=False, euler_angles=geometry.ankle_link_euler1) system.Add(right_ankle1) right_ankle2 = make_box(geometry.ankle_link_size2, geometry.right_ankle_link_center2, fixed=False, euler_angles=geometry.ankle_link_euler2) system.Add(right_ankle2) # right_foot = make_mesh('foot.stl', [0, 0, 0], fixed=True, collide=False) # system.Add(right_foot) # Setting up right joints right_recip_connector_joint = make_joint(right_recip, right_connector, geometry.right_recip_link_pos2) system.Add(right_recip_connector_joint) right_thigh_mount = make_joint(main_body, right_thigh, geometry.right_thigh_link_pos1) system.Add(right_thigh_mount) right_thigh_connector_joint = make_joint( right_connector, right_thigh, geometry.right_connector_link_pos2) system.Add(right_thigh_connector_joint) right_thigh_4bar_joint = make_joint(right_thigh, right_4bar, geometry.right_4bar_link_pos1) system.Add(right_thigh_4bar_joint) right_shin_ankle_joint1 = make_joint(right_shin, right_ankle1, geometry.right_shin_link_pos2) system.Add(right_shin_ankle_joint1) right_4bar_ankle_joint1 = make_joint(right_4bar, right_ankle1, geometry.right_4bar_link_pos2) system.Add(right_4bar_ankle_joint1) right_shin_ankle_joint2 = make_joint(right_shin, right_ankle2, geometry.right_shin_link_pos2) system.Add(right_shin_ankle_joint2) right_4bar_ankle_joint2 = make_joint(right_4bar, right_ankle2, geometry.right_4bar_link_pos2) system.Add(right_4bar_ankle_joint2) right_recip_joint = make_joint(right_recip, main_body, geometry.right_recip_link_pos1, joint_type=chrono.ChLinkMotorRotationTorque) recip_func = chrono.ChFunction_Sequence() recip_func.Set_start(3) recip_func.InsertFunct(chrono.ChFunction_Const(-10), 100) recip_func.Setup() right_recip_joint.SetTorqueFunction(recip_func) system.Add(right_recip_joint) right_thigh_shin_joint = make_joint(right_thigh, right_shin, geometry.right_shin_link_pos1) system.Add(right_thigh_shin_joint) ''' Now do it again, but for the left side! ''' left_recip = make_box(geometry.recip_link_size, geometry.left_recip_link_center, fixed=False, euler_angles=geometry.recip_link_euler) system.Add(left_recip) left_connector = make_box(geometry.connector_link_size, geometry.left_connector_link_center, fixed=False, euler_angles=geometry.connector_link_euler) system.Add(left_connector) left_thigh = make_box(geometry.thigh_link_size, geometry.left_thigh_link_center, fixed=False, euler_angles=geometry.thigh_link_euler) system.Add(left_thigh) left_shin = make_box(geometry.shin_link_size, geometry.left_shin_link_center, fixed=False, euler_angles=geometry.shin_link_euler) system.Add(left_shin) left_4bar = make_box(geometry.four_bar_link_size, geometry.left_4bar_link_center, fixed=False, euler_angles=geometry.four_bar_link_euler) system.Add(left_4bar) left_ankle1 = make_box(geometry.ankle_link_size1, geometry.left_ankle_link_center1, fixed=False, collide=False, euler_angles=geometry.ankle_link_euler1) system.Add(left_ankle1) left_ankle2 = make_box(geometry.ankle_link_size2, geometry.left_ankle_link_center2, fixed=False, euler_angles=geometry.ankle_link_euler2) system.Add(left_ankle2) # Setting up left joints left_recip_connector_joint = make_joint(left_recip, left_connector, geometry.left_recip_link_pos2) system.Add(left_recip_connector_joint) left_thigh_mount = make_joint(main_body, left_thigh, geometry.left_thigh_link_pos1) system.Add(left_thigh_mount) left_thigh_connector_joint = make_joint(left_connector, left_thigh, geometry.left_connector_link_pos2) system.Add(left_thigh_connector_joint) left_thigh_4bar_joint = make_joint(left_thigh, left_4bar, geometry.left_4bar_link_pos1) system.Add(left_thigh_4bar_joint) left_shin_ankle_joint1 = make_joint(left_shin, left_ankle1, geometry.left_shin_link_pos2) system.Add(left_shin_ankle_joint1) left_4bar_ankle_joint1 = make_joint(left_4bar, left_ankle1, geometry.left_4bar_link_pos2) system.Add(left_4bar_ankle_joint1) left_shin_ankle_joint2 = make_joint(left_shin, left_ankle2, geometry.left_shin_link_pos2) system.Add(left_shin_ankle_joint2) left_4bar_ankle_joint2 = make_joint(left_4bar, left_ankle2, geometry.left_4bar_link_pos2) system.Add(left_4bar_ankle_joint2) left_recip_joint = make_joint(main_body, left_recip, geometry.left_recip_link_pos1, joint_type=chrono.ChLinkMotorRotationTorque) left_recip_joint.SetTorqueFunction(chrono.ChFunction_Const(10)) system.Add(left_recip_joint) left_thigh_shin_joint = make_joint(left_thigh, left_shin, geometry.left_shin_link_pos1) system.Add(left_thigh_shin_joint) left_shin_pulley = chrono.ChLinkPulley() left_shin_pulley.Initialize(left_connector, left_shin, chrono.ChCoordsysD(chrono.ChVectorD(0, 0, 0))) system.Add(left_shin_pulley) right_shin_pulley = chrono.ChLinkPulley() right_shin_pulley.Initialize(right_connector, right_shin, chrono.ChCoordsysD(chrono.ChVectorD(0, 0, 0))) system.Add(right_shin_pulley)