def reset(self): self.isdone = False self.ant_sys.Clear() self.body_abdomen = chrono.ChBody() self.body_abdomen.SetPos(chrono.ChVectorD(0, self.abdomen_y0, 0 )) self.body_abdomen.SetMass(self.abdomen_mass) self.body_abdomen.SetInertiaXX(self.abdomen_inertia) # set collision surface properties self.body_abdomen.SetMaterialSurface(self.ant_material) abdomen_ellipsoid = chrono.ChEllipsoid(chrono.ChVectorD(0, 0, 0 ), chrono.ChVectorD(self.abdomen_x, self.abdomen_y, self.abdomen_z )) self.abdomen_shape = chrono.ChEllipsoidShape(abdomen_ellipsoid) self.body_abdomen.AddAsset(self.abdomen_shape) self.body_abdomen.SetMaterialSurface(self.ant_material) self.body_abdomen.SetCollide(True) self.body_abdomen.GetCollisionModel().ClearModel() self.body_abdomen.GetCollisionModel().AddEllipsoid(self.abdomen_x, self.abdomen_y, self.abdomen_z, chrono.ChVectorD(0, 0, 0 ) ) self.body_abdomen.GetCollisionModel().BuildModel() self.ant_sys.Add(self.body_abdomen) leg_ang = (1/4)*math.pi+(1/2)*math.pi*np.array([0,1,2,3]) Leg_quat = [chrono.ChQuaternionD() for i in range(len(leg_ang))] self.leg_body = [chrono.ChBody() for i in range(len(leg_ang))] self.leg_pos= [chrono.ChVectorD() for i in range(len(leg_ang))] leg_cyl = chrono.ChCylinder(-chrono.ChVectorD( self.leg_length/2, 0 ,0),chrono.ChVectorD( self.leg_length/2, 0 ,0), self.leg_radius) self.leg_shape = chrono.ChCylinderShape(leg_cyl) ankle_cyl = chrono.ChCylinder(-chrono.ChVectorD( self.ankle_length/2, 0 ,0),chrono.ChVectorD( self.ankle_length/2, 0 ,0), self.ankle_radius) self.ankle_shape = chrono.ChCylinderShape(ankle_cyl) foot_sphere = chrono.ChSphere(chrono.ChVectorD(self.ankle_length/2, 0, 0 ), self.ankle_radius ) self.foot_shape = chrono.ChSphereShape(foot_sphere) Leg_qa = [ chrono.ChQuaternionD() for i in range(len(leg_ang))] Leg_q = [ chrono.ChQuaternionD() for i in range(len(leg_ang))] z2x_leg = [ chrono.ChQuaternionD() for i in range(len(leg_ang))] Leg_rev_pos=[] Leg_chordsys = [] self.legjoint_frame = [] x_rel = [] z_rel = [] self.Leg_rev = [chrono.ChLinkLockRevolute() for i in range(len(leg_ang))] self.leg_motor = [chrono.ChLinkMotorRotationTorque() for i in range(len(leg_ang)) ] #ankle lists anklejoint_chordsys = [] self.anklejoint_frame = [] self.ankleCOG_frame = [] q_ankle_zrot = [ chrono.ChQuaternionD() for i in range(len(leg_ang))] self.ankle_body = [chrono.ChBody() for i in range(len(leg_ang))] self.Ankle_rev = [chrono.ChLinkLockRevolute() for i in range(len(leg_ang))] self.ankle_motor = [chrono.ChLinkMotorRotationTorque() for i in range(len(leg_ang)) ] for i in range(len(leg_ang)): # Legs Leg_quat[i].Q_from_AngAxis(-leg_ang[i] , chrono.ChVectorD(0, 1, 0)) self.leg_pos[i] = chrono.ChVectorD( (0.5*self.leg_length+self.abdomen_x)*math.cos(leg_ang[i]) ,self.abdomen_y0, (0.5*self.leg_length+self.abdomen_z)*math.sin(leg_ang[i])) self.leg_body[i].SetPos(self.leg_pos[i]) self.leg_body[i].SetRot(Leg_quat[i]) self.leg_body[i].AddAsset(self.leg_shape) self.leg_body[i].SetMass(self.leg_mass) self.leg_body[i].SetInertiaXX(self.leg_inertia) self.ant_sys.Add(self.leg_body[i]) x_rel.append( Leg_quat[i].Rotate(chrono.ChVectorD(1, 0, 0))) z_rel.append( Leg_quat[i].Rotate(chrono.ChVectorD(0, 0, 1))) Leg_qa[i].Q_from_AngAxis(-leg_ang[i] , chrono.ChVectorD(0, 1, 0)) z2x_leg[i].Q_from_AngAxis(chrono.CH_C_PI / 2 , x_rel[i]) Leg_q[i] = z2x_leg[i] * Leg_qa[i] Leg_rev_pos.append(chrono.ChVectorD(self.leg_pos[i]-chrono.ChVectorD(math.cos(leg_ang[i])*self.leg_length/2,0,math.sin(leg_ang[i])*self.leg_length/2))) Leg_chordsys.append(chrono.ChCoordsysD(Leg_rev_pos[i], Leg_q[i])) self.legjoint_frame.append(chrono.ChFrameD(Leg_chordsys[i])) self.Leg_rev[i].Initialize(self.body_abdomen, self.leg_body[i],Leg_chordsys[i]) self.ant_sys.Add(self.Leg_rev[i]) self.leg_motor[i].Initialize(self.body_abdomen, self.leg_body[i],self.legjoint_frame[i]) self.ant_sys.Add(self.leg_motor[i]) # Ankles q_ankle_zrot[i].Q_from_AngAxis(-self.ankle_angle , z_rel[i]) anklejoint_chordsys.append(chrono.ChCoordsysD(self.leg_body[i].GetPos()+ self.leg_body[i].GetRot().Rotate(chrono.ChVectorD(self.leg_length/2, 0, 0)) , q_ankle_zrot[i] * self.leg_body[i].GetRot() )) self.anklejoint_frame.append(chrono.ChFrameD(anklejoint_chordsys[i])) self.ankle_body[i].SetPos(self.anklejoint_frame[i].GetPos() + self.anklejoint_frame[i].GetRot().Rotate(chrono.ChVectorD(self.ankle_length/2, 0, 0))) self.ankle_body[i].SetRot( self.anklejoint_frame[i].GetRot() ) self.ankle_body[i].AddAsset(self.ankle_shape) self.ankle_body[i].SetMass(self.ankle_mass) self.ankle_body[i].SetInertiaXX(self.ankle_inertia) self.ant_sys.Add(self.ankle_body[i]) self.Ankle_rev[i].Initialize(self.leg_body[i], self.ankle_body[i], anklejoint_chordsys[i]) self.ant_sys.Add(self.Ankle_rev[i]) self.ankle_motor[i].Initialize(self.leg_body[i], self.ankle_body[i],self.anklejoint_frame[i]) self.ant_sys.Add(self.ankle_motor[i]) # Feet collisions self.ankle_body[i].SetMaterialSurface(self.ant_material) self.ankle_body[i].SetCollide(True) self.ankle_body[i].GetCollisionModel().ClearModel() self.ankle_body[i].GetCollisionModel().AddSphere(self.ankle_radius, chrono.ChVectorD(self.ankle_length/2, 0, 0 ) ) self.ankle_body[i].GetCollisionModel().BuildModel() self.ankle_body[i].AddAsset(self.ankle_shape) self.ankle_body[i].AddAsset(self.foot_shape) self.Leg_rev[i].GetLimit_Rz().SetActive(True) self.Leg_rev[i].GetLimit_Rz().SetMin(-math.pi/3) self.Leg_rev[i].GetLimit_Rz().SetMax(math.pi/3) self.Ankle_rev[i].GetLimit_Rz().SetActive(True) self.Ankle_rev[i].GetLimit_Rz().SetMin(-math.pi/2) self.Ankle_rev[i].GetLimit_Rz().SetMax(math.pi/4) # Create the room floor: a simple fixed rigid body with a collision shape # and a visualization shape self.body_floor = chrono.ChBody() self.body_floor.SetBodyFixed(True) self.body_floor.SetPos(chrono.ChVectorD(0, -1, 0 )) self.body_floor.SetMaterialSurface(self.ant_material) # Floor Collision. self.body_floor.SetMaterialSurface(self.ant_material) self.body_floor.GetCollisionModel().ClearModel() self.body_floor.GetCollisionModel().AddBox(50, 1, 50, chrono.ChVectorD(0, 0, 0 )) self.body_floor.GetCollisionModel().BuildModel() self.body_floor.SetCollide(True) # Visualization shape body_floor_shape = chrono.ChBoxShape() body_floor_shape.GetBoxGeometry().Size = chrono.ChVectorD(5, 1, 5) body_floor_shape.SetColor(chrono.ChColor(0.4,0.4,0.5)) self.body_floor.GetAssets().push_back(body_floor_shape) body_floor_texture = chrono.ChTexture() body_floor_texture.SetTextureFilename(chrono.GetChronoDataFile('vehicle/terrain/textures/grass.jpg')) self.body_floor.GetAssets().push_back(body_floor_texture) self.ant_sys.Add(self.body_floor) #self.body_abdomen.SetBodyFixed(True) if (self.animate): self.myapplication.AssetBindAll() self.myapplication.AssetUpdateAll() self.numsteps= 0 self.step(np.zeros(8)) return self.get_ob()
def reset(self): self.isdone = False self.hexapod_sys.Clear() self.exported_items = chrono.ImportSolidWorksSystem(self.fpath) self.csys = [] self.frames = [] self.revs = [] self.motors = [] self.limits = [] for con, coi in zip(self.con_link, self.coi_link): indices = [] for i in range(len(self.exported_items)): if con == self.exported_items[i].GetName( ) or coi == self.exported_items[i].GetName(): indices.append(i) rev = self.exported_items[indices[0]] af0 = rev.GetAssetsFrame() # Revolute joints and ChLinkMotorRotation are z oriented, while parallel is x oriented. # Event though this Frame won't be used anymore is good practice to create a copy before editing its value. af = chrono.ChFrameD(af0) af.SetRot(af0.GetRot() % chrono.Q_ROTATE_X_TO_Z) self.frames.append(af) for i in reversed(indices): del self.exported_items[i] # ADD IMPORTED ITEMS TO THE SYSTEM for my_item in self.exported_items: self.hexapod_sys.Add(my_item) """ $$$$$$$$ FIND THE SW DEFINED CONSTRAINTS, GET THEIR MARKERS AND GET RID OF EM $$$$$$$$ """ self.hips = [ self.hexapod_sys.SearchBody(name) for name in self.hip_names ] self.femurs = [ self.hexapod_sys.SearchBody(name) for name in self.femur_names ] self.tibias = [ self.hexapod_sys.SearchBody(name) for name in self.tibia_names ] self.feet = [ self.hexapod_sys.SearchBody(name) for name in self.feet_names ] self.centralbody = self.hexapod_sys.SearchBody('Body-1') # Bodies are used to replace constraints and detect unwanted collision, so feet are excluded self.bodies = [self.centralbody ] + self.hips + self.femurs + self.tibias self.centralbody.SetBodyFixed(False) self.y0 = self.centralbody.GetPos().y """ # SNIPPET FOR COLOR orange = chrono.ChColorAsset() orange.SetColor(chrono.ChColor(255/255,77/255,6/255)) black = chrono.ChColorAsset() black.SetColor(chrono.ChColor(0,0,0)) for body in self.bodies[:-1]: assets = body.GetAssets() for ast in assets: ass_lev = chrono.CastToChAssetLevel(ast) ass_lev.GetAssets().push_back(orange) assets = self.hand.GetAssets() for ast in assets: ass_lev = chrono.CastToChAssetLevel(ast) ass_lev.GetAssets().push_back(black) """ for i in range(len(self.con_link)): revolute = chrono.ChLinkLockRevolute() cs = chrono.ChCoordsysD(self.frames[i].GetPos(), self.frames[i].GetRot()) self.csys.append(cs) if i < 6: j = 0 else: j = i - 5 revolute.Initialize(self.bodies[j], self.bodies[i + 1], self.csys[i]) self.revs.append(revolute) self.hexapod_sys.Add(self.revs[i]) lim = self.revs[i].GetLimit_Rz() self.limits.append(lim) self.limits[i].SetActive(True) self.limits[i].SetMin(self.minRot[i] * (math.pi / 180)) self.limits[i].SetMax(self.maxRot[i] * (math.pi / 180)) m = chrono.ChLinkMotorRotationTorque() m.SetSpindleConstraint(False, False, False, False, False) m.Initialize(self.bodies[j], self.bodies[i + 1], self.frames[i]) self.motors.append(m) self.hexapod_sys.Add(self.motors[i]) self.body_floor = chrono.ChBody() self.body_floor.SetBodyFixed(True) self.body_floor.SetPos(chrono.ChVectorD(0, -1 - 0.128 - 0.0045, 10)) # Floor Collision. self.body_floor.GetCollisionModel().ClearModel() self.body_floor.GetCollisionModel().AddBox(self.my_material, 50, 1, 50, chrono.ChVectorD(0, 0, 0)) self.body_floor.GetCollisionModel().BuildModel() self.body_floor.SetCollide(True) # Visualization shape body_floor_shape = chrono.ChBoxShape() body_floor_shape.GetBoxGeometry().Size = chrono.ChVectorD(4, 1, 15) body_floor_shape.SetColor(chrono.ChColor(0.4, 0.4, 0.5)) self.body_floor.GetAssets().push_back(body_floor_shape) body_floor_texture = chrono.ChTexture() texpath = os.path.join(chrono.GetChronoDataPath(), 'concrete.jpg') body_floor_texture.SetTextureFilename(texpath) self.body_floor.GetAssets().push_back(body_floor_texture) self.hexapod_sys.Add(self.body_floor) self.numsteps = 0 if (self.render_setup): self.myapplication.AssetBindAll() self.myapplication.AssetUpdateAll() return self.get_ob()
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 reset(self): self.isdone = False self.robosystem.Clear() #action = (np.random.rand(6,)-0.5)*2 #torques = np.multiply(action, self.maxT) self.exported_items = chrono.ImportSolidWorksSystem(self.fpath) self.csys = [] self.frames = [] self.revs = [] self.motors = [] self.limits = [] for con, coi in zip(self.con_link, self.coi_link): indices = [] for i in range(len(self.exported_items)): if con == self.exported_items[i].GetName( ) or coi == self.exported_items[i].GetName(): indices.append(i) rev = self.exported_items[indices[0]] af0 = rev.GetAssetsFrame() # Revolute joints and ChLinkMotorRotation are z oriented, while parallel is x oriented. # Event though this Frame won't be used anymore is good practice to create a copy before editing its value. af = chrono.ChFrameD(af0) af.SetRot(af0.GetRot() % chrono.Q_ROTATE_X_TO_Z) self.frames.append(af) for i in indices: del self.exported_items[i] # ADD IMPORTED ITEMS TO THE SYSTEM for my_item in self.exported_items: self.robosystem.Add(my_item) """ $$$$$$$$ FIND THE SW DEFINED CONSTRAINTS, GET THEIR MARKERS AND GET RID OF EM $$$$$$$$ """ self.bodies = [ self.robosystem.SearchBody(name) for name in self.bodiesNames ] self.hand = self.robosystem.SearchBody('Hand_base_and_p07-2') self.base = self.robosystem.SearchBody('Racer3_p01-3') self.biceps = self.robosystem.SearchBody('Racer3_p03-1') self.forearm = self.robosystem.SearchBody('Racer3_p05-1') self.finger1 = self.robosystem.SearchBody('HAND_e_finger-1') self.finger2 = self.robosystem.SearchBody('HAND_e_finger-2') for i in range(len(self.con_link)): revolute = chrono.ChLinkLockRevolute() cs = chrono.ChCoordsysD(self.frames[i].GetPos(), self.frames[i].GetRot()) self.csys.append(cs) revolute.Initialize(self.bodies[i], self.bodies[i + 1], self.csys[i]) self.revs.append(revolute) self.robosystem.Add(self.revs[i]) lim = self.revs[i].GetLimit_Rz() self.limits.append(lim) self.limits[i].SetActive(True) self.limits[i].SetMin(self.minRot[i] * (math.pi / 180)) self.limits[i].SetMax(self.maxRot[i] * (math.pi / 180)) m = chrono.ChLinkMotorRotationTorque() m.Initialize(self.bodies[i], self.bodies[i + 1], self.frames[i]) #self.robosystem.Add(m2) #m2.SetTorqueFunction(chrono.ChFunction_Const(5)) self.motors.append(m) #self.motors[i].SetTorqueFunction(chrono.ChFunction_Const(float(torques[i]))) self.robosystem.Add(self.motors[i]) self.body_floor = chrono.ChBody() self.body_floor.SetBodyFixed(True) self.body_floor.SetPos(chrono.ChVectorD(0, -1, 0)) # Floor Collision. self.body_floor.GetCollisionModel().ClearModel() self.body_floor.GetCollisionModel().AddBox(self.my_material, 5, 1, 5, chrono.ChVectorD(0, 0, 0)) self.body_floor.GetCollisionModel().BuildModel() self.body_floor.SetCollide(True) # Visualization shape body_floor_shape = chrono.ChBoxShape() body_floor_shape.GetBoxGeometry().Size = chrono.ChVectorD(5, 1, 5) body_floor_shape.SetColor(chrono.ChColor(0.4, 0.4, 0.5)) self.body_floor.GetAssets().push_back(body_floor_shape) body_floor_texture = chrono.ChTexture() texpath = os.path.join(chrono.GetChronoDataPath(), 'concrete.jpg') body_floor_texture.SetTextureFilename(texpath) self.body_floor.GetAssets().push_back(body_floor_texture) self.robosystem.Add(self.body_floor) r = np.random.rand(2, ) - np.asarray([0.5, 0.5]) self.targ_init_pos = [ -0.52 + 2 * (r[0] * 0.05), 0.015, 2 * r[1] * 0.05 ] self.targ_box = chrono.ChBody() # UNset to grasp self.targ_box.SetBodyFixed(True) self.targ_box.SetPos( chrono.ChVectorD(self.targ_init_pos[0], self.targ_init_pos[1], self.targ_init_pos[2])) # Floor Collision. self.targ_box.GetCollisionModel().ClearModel() self.targ_box.GetCollisionModel().AddBox(self.my_material, 0.015, 0.015, 0.015, chrono.ChVectorD(0, 0, 0)) self.targ_box.GetCollisionModel().BuildModel() self.targ_box.SetCollide(True) # Visualization shape targ_box_shape = chrono.ChBoxShape() targ_box_shape.GetBoxGeometry().Size = chrono.ChVectorD( 0.015, 0.015, 0.015) col = chrono.ChColorAsset() col.SetColor(chrono.ChColor(1.0, 0, 0)) self.targ_box.GetAssets().push_back(targ_box_shape) self.targ_box.GetAssets().push_back(col) self.robosystem.Add(self.targ_box) self.numsteps = 0 if (self.render_setup): self.myapplication.AssetBindAll() self.myapplication.AssetUpdateAll() return self.get_ob()
def reset(self): self.isdone = False self.ant_sys.Clear() self.body_abdomen = chrono.ChBody() self.body_abdomen.SetPos(chrono.ChVectorD(0, self.abdomen_y0, 0)) self.body_abdomen.SetMass(self.abdomen_mass) self.body_abdomen.SetInertiaXX(self.abdomen_inertia) # set collision surface properties self.body_abdomen.SetMaterialSurface(self.ant_material) abdomen_ellipsoid = chrono.ChEllipsoid( chrono.ChVectorD(0, 0, 0), chrono.ChVectorD(self.abdomen_x, self.abdomen_y, self.abdomen_z)) self.abdomen_shape = chrono.ChEllipsoidShape(abdomen_ellipsoid) self.body_abdomen.AddAsset(self.abdomen_shape) self.body_abdomen.SetMaterialSurface(self.ant_material) self.body_abdomen.SetCollide(True) self.body_abdomen.GetCollisionModel().ClearModel() self.body_abdomen.GetCollisionModel().AddEllipsoid( self.abdomen_x, self.abdomen_y, self.abdomen_z, chrono.ChVectorD(0, 0, 0)) self.body_abdomen.GetCollisionModel().BuildModel() self.ant_sys.Add(self.body_abdomen) tilt_factor = 1.1 # no tilt; higher number tilts body toward positive x leg_ang = np.array([ tilt_factor * math.pi / 2, tilt_factor * math.pi / 2 ]) #(1/4)*math.pi+(1/2)*math.pi*np.array([0,1]) Leg_quat = [chrono.ChQuaternionD() for i in range(len(leg_ang))] self.leg_body = [chrono.ChBody() for i in range(len(leg_ang))] self.leg_pos = [chrono.ChVectorD() for i in range(len(leg_ang))] leg_cyl = chrono.ChCylinder( -chrono.ChVectorD(self.leg_length / 2, 0, 0), chrono.ChVectorD(self.leg_length / 2, 0, 0), self.leg_radius) self.leg_shape = chrono.ChCylinderShape(leg_cyl) ankle_cyl = chrono.ChCylinder( -chrono.ChVectorD(self.ankle_length / 2, 0, 0), chrono.ChVectorD(self.ankle_length / 2, 0, 0), self.ankle_radius) self.ankle_shape = chrono.ChCylinderShape(ankle_cyl) foot_sphere = chrono.ChSphere( chrono.ChVectorD(self.ankle_length / 2, 0, 0), self.ankle_radius) self.foot_shape = chrono.ChSphereShape(foot_sphere) Leg_qa = [chrono.ChQuaternionD() for i in range(len(leg_ang))] Leg_q = [chrono.ChQuaternionD() for i in range(len(leg_ang))] z2x_leg = [chrono.ChQuaternionD() for i in range(len(leg_ang))] Leg_rev_pos = [] Leg_chordsys = [] self.legjoint_frame = [] x_rel = [] z_rel = [] self.Leg_rev = [ chrono.ChLinkLockRevolute() for i in range(len(leg_ang)) ] self.leg_motor = [ chrono.ChLinkMotorRotationTorque() for i in range(len(leg_ang)) ] #ankle lists anklejoint_chordsys = [] self.anklejoint_frame = [] self.ankleCOG_frame = [] q_ankle_zrot = [chrono.ChQuaternionD() for i in range(len(leg_ang))] self.ankle_body = [chrono.ChBody() for i in range(len(leg_ang))] self.Ankle_rev = [ chrono.ChLinkLockRevolute() for i in range(len(leg_ang)) ] self.ankle_motor = [ chrono.ChLinkMotorRotationTorque() for i in range(len(leg_ang)) ] for i in range(len(leg_ang)): # Legs Leg_quat[i].Q_from_AngAxis(-leg_ang[i], chrono.ChVectorD(0, 0, 1)) # postion of leg CM from abdomen self.leg_pos[i] = chrono.ChVectorD( (0.5 * self.leg_length * math.cos(leg_ang[i])), (self.abdomen_y0 - self.leg_length / 2 * math.sin(leg_ang[i]) - self.abdomen_y * math.cos(self.CMtohip_angle)), (-1)**i * self.abdomen_z * math.sin(self.CMtohip_angle)) self.leg_body[i].SetPos(self.leg_pos[i]) self.leg_body[i].SetRot(Leg_quat[i]) self.leg_body[i].AddAsset(self.leg_shape) self.leg_body[i].SetMass(self.leg_mass) self.leg_body[i].SetInertiaXX(self.leg_inertia) self.ant_sys.Add(self.leg_body[i]) x_rel.append(Leg_quat[i].Rotate(chrono.ChVectorD(1, 0, 0))) z_rel.append(Leg_quat[i].Rotate(chrono.ChVectorD(0, 0, 1))) Leg_qa[i].Q_from_AngAxis(-leg_ang[i], chrono.ChVectorD(0, 1, 0)) z2x_leg[i].Q_from_AngAxis(chrono.CH_C_PI / 2, x_rel[i]) Leg_q[i] = z2x_leg[i] * Leg_qa[i] Leg_rev_pos.append(chrono.ChVectorD(0, self.abdomen_y0, 0)) # connection to abdomen Leg_chordsys.append(chrono.ChCoordsysD(Leg_rev_pos[i], Leg_q[i])) self.legjoint_frame.append(chrono.ChFrameD(Leg_chordsys[i])) self.Leg_rev[i].Initialize(self.body_abdomen, self.leg_body[i], Leg_chordsys[i]) self.ant_sys.Add(self.Leg_rev[i]) self.leg_motor[i].Initialize(self.body_abdomen, self.leg_body[i], self.legjoint_frame[i]) self.ant_sys.Add(self.leg_motor[i]) # Ankles q_ankle_zrot[i].Q_from_AngAxis(-self.ankle_angle, z_rel[i]) anklejoint_chordsys.append( chrono.ChCoordsysD( self.leg_body[i].GetPos() + self.leg_body[i].GetRot().Rotate( chrono.ChVectorD(self.leg_length / 2, 0, 0)), q_ankle_zrot[i] * self.leg_body[i].GetRot())) self.anklejoint_frame.append( chrono.ChFrameD(anklejoint_chordsys[i])) self.ankle_body[i].SetPos( self.anklejoint_frame[i].GetPos() + self.anklejoint_frame[i].GetRot().Rotate( chrono.ChVectorD(self.ankle_length / 2, 0, 0))) self.ankle_body[i].SetRot(self.anklejoint_frame[i].GetRot()) self.ankle_body[i].AddAsset(self.ankle_shape) self.ankle_body[i].SetMass(self.ankle_mass) self.ankle_body[i].SetInertiaXX(self.ankle_inertia) self.ant_sys.Add(self.ankle_body[i]) self.Ankle_rev[i].Initialize(self.leg_body[i], self.ankle_body[i], anklejoint_chordsys[i]) self.ant_sys.Add(self.Ankle_rev[i]) self.ankle_motor[i].Initialize(self.leg_body[i], self.ankle_body[i], self.anklejoint_frame[i]) self.ant_sys.Add(self.ankle_motor[i]) # Feet collisions self.ankle_body[i].SetMaterialSurface(self.ant_material) self.ankle_body[i].SetCollide(True) self.ankle_body[i].GetCollisionModel().ClearModel() self.ankle_body[i].GetCollisionModel().AddSphere( self.ankle_radius, chrono.ChVectorD(self.ankle_length / 2, 0, 0)) self.ankle_body[i].GetCollisionModel().BuildModel() self.ankle_body[i].AddAsset(self.ankle_shape) self.ankle_body[i].AddAsset(self.foot_shape) # Limits need to turn back to true self.Leg_rev[i].GetLimit_Rz().SetActive(True) self.Leg_rev[i].GetLimit_Rz().SetMin(-math.pi / 3) self.Leg_rev[i].GetLimit_Rz().SetMax(math.pi / 3) self.Ankle_rev[i].GetLimit_Rz().SetActive(True) self.Ankle_rev[i].GetLimit_Rz().SetMin(-math.pi / 3) self.Ankle_rev[i].GetLimit_Rz().SetMax(math.pi / 3) # Create the room floor: a simple fixed rigid body with a collision shape # and a visualization shape self.body_floor = chrono.ChBody() self.body_floor.SetBodyFixed(True) self.body_floor.SetPos( chrono.ChVectorD( 0, -self.abdomen_y0 - self.abdomen_y * math.cos(self.CMtohip_angle) - self.ankle_radius, 0)) self.body_floor.SetMaterialSurface(self.ant_material) # Floor Collision. self.body_floor.SetMaterialSurface(self.ant_material) self.body_floor.GetCollisionModel().ClearModel() self.body_floor.GetCollisionModel().AddBox(50, 1, 50, chrono.ChVectorD(0, 0, 0)) self.body_floor.GetCollisionModel().BuildModel() self.body_floor.SetCollide(True) # Visualization shape body_floor_shape = chrono.ChBoxShape() body_floor_shape.GetBoxGeometry().Size = chrono.ChVectorD(5, 1, 5) body_floor_shape.SetColor(chrono.ChColor(0.4, 0.4, 0.5)) self.body_floor.GetAssets().push_back(body_floor_shape) body_floor_texture = chrono.ChTexture() body_floor_texture.SetTextureFilename('../data/grass.jpg') self.body_floor.GetAssets().push_back(body_floor_texture) self.ant_sys.Add(self.body_floor) # this was commented out to not fix the segments # this is helpful to check geometry of humanoid #self.body_abdomen.SetBodyFixed(True) #self.leg_body[0].SetBodyFixed(True) #self.leg_body[1].SetBodyFixed(True) #self.ankle_body[0].SetBodyFixed(True) #self.ankle_body[1].SetBodyFixed(True) if (self.animate): self.myapplication.AssetBindAll() self.myapplication.AssetUpdateAll() self.numsteps = 0 self.step(np.zeros(4)) return self.get_ob()