## 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)) ## Revolute joint between ground and crank. ## The rotational axis of a revolute joint is along the Z axis of the ## specified joint coordinate frame. Here, we apply the 'z2y' rotation to ## align it with the Y axis of the global reference frame. revolute_ground_crank = chrono.ChLinkLockRevolute() revolute_ground_crank.SetName("revolute_ground_crank") revolute_ground_crank.Initialize( ground, crank, chrono.ChCoordsysD(chrono.ChVectorD(0, 0, 0), z2y)) system.AddLink(revolute_ground_crank) ## Prismatic joint between ground and slider. ## The translational axis of a prismatic joint is along the Z axis of the ## specified joint coordinate system. Here, we apply the 'z2x' rotation to ## align it with the X axis of the global reference frame. prismatic_ground_slider = chrono.ChLinkLockPrismatic() prismatic_ground_slider.SetName("prismatic_ground_slider") prismatic_ground_slider.Initialize( ground, slider, chrono.ChCoordsysD(chrono.ChVectorD(2, 0, 0), z2x)) system.AddLink(prismatic_ground_slider)
mbody_truss = chrono.ChBodyEasyBox(20, 10, 2, 1000, True, False, mat) mphysicalSystem.Add(mbody_truss) mbody_truss.SetBodyFixed(True) mbody_truss.SetPos(chrono.ChVectorD(0, 0, 3)) # ...a texture asset that will be shared among the four wheels cylinder_texture = chrono.ChTexture( chrono.GetChronoDataFile("textures/pinkwhite.png")) # ...the rotating bar support for the two epicycloidal wheels mbody_train = chrono.ChBodyEasyBox(8, 1.5, 1.0, 1000, True, False, mat) mphysicalSystem.Add(mbody_train) mbody_train.SetPos(chrono.ChVectorD(3, 0, 0)) # ...which must rotate respect to truss along Z axis, in 0,0,0 link_revoluteTT = chrono.ChLinkLockRevolute() link_revoluteTT.Initialize( mbody_truss, mbody_train, chrono.ChCoordsysD(chrono.ChVectorD(0, 0, 0), chrono.QUNIT)) mphysicalSystem.AddLink(link_revoluteTT) # ...the first gear mbody_gearA = chrono.ChBodyEasyCylinder(radA, 0.5, 1000, True, False, mat) mphysicalSystem.Add(mbody_gearA) 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)
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): #print("reset") self.isdone = False self.rev_pend_sys.Clear() # create it self.body_rod = chrono.ChBody() # set initial position self.body_rod.SetPos(chrono.ChVectorD(0, self.size_rod_y / 2, 0)) # set mass properties self.body_rod.SetMass(self.mass_rod) self.body_rod.SetInertiaXX( chrono.ChVectorD(self.inertia_rod_x, self.inertia_rod_y, self.inertia_rod_x)) # set collision surface properties self.body_rod.SetMaterialSurface(self.rod_material) # Visualization shape, for rendering animation self.cyl_base1 = chrono.ChVectorD(0, -self.size_rod_y / 2, 0) self.cyl_base2 = chrono.ChVectorD(0, self.size_rod_y / 2, 0) self.body_rod_shape = chrono.ChCylinderShape() self.body_rod_shape.GetCylinderGeometry().p1 = self.cyl_base1 self.body_rod_shape.GetCylinderGeometry().p2 = self.cyl_base2 self.body_rod_shape.GetCylinderGeometry().rad = self.radius_rod self.body_rod.AddAsset(self.body_rod_shape) self.rev_pend_sys.Add(self.body_rod) self.body_floor = chrono.ChBody() self.body_floor.SetBodyFixed(True) self.body_floor.SetPos(chrono.ChVectorD(0, -5, 0)) self.body_floor.SetMaterialSurface(self.rod_material) if self.render: self.body_floor_shape = chrono.ChBoxShape() self.body_floor_shape.GetBoxGeometry().Size = chrono.ChVectorD( 3, 1, 3) self.body_floor.GetAssets().push_back(self.body_floor_shape) self.body_floor_texture = chrono.ChTexture() self.body_floor_texture.SetTextureFilename( '../../../data/concrete.jpg') self.body_floor.GetAssets().push_back(self.body_floor_texture) self.rev_pend_sys.Add(self.body_floor) self.body_table = chrono.ChBody() self.body_table.SetPos(chrono.ChVectorD(0, -self.size_table_y / 2, 0)) self.body_table.SetMaterialSurface(self.rod_material) if self.render: self.body_table_shape = chrono.ChBoxShape() self.body_table_shape.GetBoxGeometry().Size = chrono.ChVectorD( self.size_table_x / 2, self.size_table_y / 2, self.size_table_z / 2) self.body_table_shape.SetColor(chrono.ChColor(0.4, 0.4, 0.5)) self.body_table.GetAssets().push_back(self.body_table_shape) self.body_table_texture = chrono.ChTexture() self.body_table_texture.SetTextureFilename( '../../../data/concrete.jpg') self.body_table.GetAssets().push_back(self.body_table_texture) self.body_table.SetMass(0.1) self.rev_pend_sys.Add(self.body_table) self.link_slider = chrono.ChLinkLockPrismatic() z2x = chrono.ChQuaternionD() z2x.Q_from_AngAxis(-chrono.CH_C_PI / 2, chrono.ChVectorD(0, 1, 0)) self.link_slider.Initialize( self.body_table, self.body_floor, chrono.ChCoordsysD(chrono.ChVectorD(0, 0, 0), z2x)) self.rev_pend_sys.Add(self.link_slider) self.act_initpos = chrono.ChVectorD(0, 0, 0) self.actuator = chrono.ChLinkMotorLinearForce() self.actuator.Initialize(self.body_table, self.body_floor, chrono.ChFrameD(self.act_initpos)) self.rev_pend_sys.Add(self.actuator) self.rod_pin = chrono.ChMarker() self.body_rod.AddMarker(self.rod_pin) self.rod_pin.Impose_Abs_Coord( chrono.ChCoordsysD(chrono.ChVectorD(0, 0, 0))) self.table_pin = chrono.ChMarker() self.body_table.AddMarker(self.table_pin) self.table_pin.Impose_Abs_Coord( chrono.ChCoordsysD(chrono.ChVectorD(0, 0, 0))) self.pin_joint = chrono.ChLinkLockRevolute() self.pin_joint.Initialize(self.rod_pin, self.table_pin) self.rev_pend_sys.Add(self.pin_joint) if self.render: # --------------------------------------------------------------------- # # Create an Irrlicht application to visualize the system # # ==IMPORTANT!== Use this function for adding a ChIrrNodeAsset to all items # in the system. These ChIrrNodeAsset assets are 'proxies' to the Irrlicht meshes. # If you need a finer control on which item really needs a visualization proxy # Irrlicht, just use application.AssetBind(myitem); on a per-item basis. self.myapplication.AssetBindAll() # ==IMPORTANT!== Use this function for 'converting' into Irrlicht meshes the assets # that you added to the bodies into 3D shapes, they can be visualized by Irrlicht! self.myapplication.AssetUpdateAll() self.isdone = False self.steps = 0 self.step(np.array([[0]])) 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 reset(self): #print("reset") self.isdone = False self.rev_pend_sys.Clear() # create it self.body_rod = chrono.ChBody() # set initial position self.body_rod.SetPos(chrono.ChVectorD(0, self.size_rod_y / 2, 0)) # set mass properties self.body_rod.SetMass(self.mass_rod) self.body_rod.SetInertiaXX( chrono.ChVectorD(self.inertia_rod_x, self.inertia_rod_y, self.inertia_rod_x)) # set collision surface properties self.body_rod.SetMaterialSurface(self.rod_material) self.cyl_base1 = chrono.ChVectorD(0, -self.size_rod_y / 2, 0) self.cyl_base2 = chrono.ChVectorD(0, self.size_rod_y / 2, 0) self.body_rod_shape = chrono.ChCylinderShape() self.body_rod_shape.GetCylinderGeometry().p1 = self.cyl_base1 self.body_rod_shape.GetCylinderGeometry().p2 = self.cyl_base2 self.body_rod_shape.GetCylinderGeometry().rad = self.radius_rod self.body_rod.AddAsset(self.body_rod_shape) self.rev_pend_sys.Add(self.body_rod) self.body_floor = chrono.ChBody() self.body_floor.SetBodyFixed(True) self.body_floor.SetPos(chrono.ChVectorD(0, -5, 0)) self.body_floor.SetMaterialSurface(self.rod_material) self.body_floor_shape = chrono.ChBoxShape() self.body_floor_shape.GetBoxGeometry().Size = chrono.ChVectorD(3, 1, 3) self.body_floor.GetAssets().push_back(self.body_floor_shape) self.body_floor_texture = chrono.ChTexture() self.body_floor_texture.SetTextureFilename(chrono.GetChronoDataPath() + '/concrete.jpg') self.body_floor.GetAssets().push_back(self.body_floor_texture) self.rev_pend_sys.Add(self.body_floor) self.body_table = chrono.ChBody() self.body_table.SetPos(chrono.ChVectorD(0, -self.size_table_y / 2, 0)) self.body_table.SetMaterialSurface(self.rod_material) self.body_table.SetMass(0.1) self.body_table_shape = chrono.ChBoxShape() self.body_table_shape.GetBoxGeometry().Size = chrono.ChVectorD( self.size_table_x / 2, self.size_table_y / 2, self.size_table_z / 2) self.body_table_shape.SetColor(chrono.ChColor(0.4, 0.4, 0.5)) self.body_table.GetAssets().push_back(self.body_table_shape) self.body_table_texture = chrono.ChTexture() self.body_table_texture.SetTextureFilename(chrono.GetChronoDataPath() + '/concrete.jpg') self.body_table.GetAssets().push_back(self.body_table_texture) self.rev_pend_sys.Add(self.body_table) self.link_slider = chrono.ChLinkLockPrismatic() z2x = chrono.ChQuaternionD() z2x.Q_from_AngAxis(-chrono.CH_C_PI / 2, chrono.ChVectorD(0, 1, 0)) self.link_slider.Initialize( self.body_table, self.body_floor, chrono.ChCoordsysD(chrono.ChVectorD(0, 0, 0), z2x)) self.rev_pend_sys.Add(self.link_slider) self.act_initpos = chrono.ChVectorD(0, 0, 0) self.actuator = chrono.ChLinkMotorLinearForce() self.actuator.Initialize(self.body_table, self.body_floor, chrono.ChFrameD(self.act_initpos)) self.rev_pend_sys.Add(self.actuator) self.rod_pin = chrono.ChMarker() self.body_rod.AddMarker(self.rod_pin) self.rod_pin.Impose_Abs_Coord( chrono.ChCoordsysD(chrono.ChVectorD(0, 0, 0))) self.table_pin = chrono.ChMarker() self.body_table.AddMarker(self.table_pin) self.table_pin.Impose_Abs_Coord( chrono.ChCoordsysD(chrono.ChVectorD(0, 0, 0))) self.pin_joint = chrono.ChLinkLockRevolute() self.pin_joint.Initialize(self.rod_pin, self.table_pin) self.rev_pend_sys.Add(self.pin_joint) if self.render_setup: self.myapplication.AssetBindAll() self.myapplication.AssetUpdateAll() self.isdone = False self.steps = 0 self.step(np.array([[0]])) return self.get_ob()
bodyC.SetRot(chrono.ChQuaternionD(1, 0, 0, 0)) bodyC.SetMass(346.17080777653) bodyC.SetInertiaXX( chrono.ChVectorD(48583.2418823358, 526927.118351673, 490689.966726565)) bodyC.SetInertiaXY( chrono.ChVectorD(1.70380722975012e-11, 1.40840344485366e-11, -2.31869065456271e-12)) bodyC.SetFrame_COG_to_REF( chrono.ChFrameD( chrono.ChVectorD(68.9923703887577, -60.1266363930238, 70.1327223302498), chrono.ChQuaternionD(1, 0, 0, 0))) myasset = chrono.ChObjShapeFile() myasset.SetFilename("shapes/test.obj") bodyC.GetAssets().push_back(myasset) # Add a revolute joint rev = chrono.ChLinkLockRevolute() rev.SetName('Revolute') rev.Initialize( bodyA, bodyC, chrono.ChCoordsysD(chrono.ChVectorD(1, 2, 3), chrono.ChQuaternionD(1, 0, 0, 0))) my_system.AddLink(rev) # Iterate over added links (Python style) print('Names of all links in the system:') for alink in my_system.Get_linklist(): print(' link: ', alink.GetName()) print('Done...')
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): #print("reset") self.isdone = False self.rev_pend_sys.Clear() # create it self.body_rod = chrono.ChBody() # set initial position self.body_rod.SetPos(chrono.ChVectorD(0, self.size_rod_y / 2, 0)) # set mass properties self.body_rod.SetMass(self.mass_rod) # una volta che le hao calcolate inserisci inerzie diverse self.body_rod.SetInertiaXX( chrono.ChVectorD(self.inertia_rod_x, self.inertia_rod_y, self.inertia_rod_x)) # set collision surface properties self.body_rod.SetMaterialSurface(self.rod_material) # Visualization shape, for rendering animation # vettori visual cilindro self.cyl_base1 = chrono.ChVectorD(0, -self.size_rod_y / 2, 0) self.cyl_base2 = chrono.ChVectorD(0, self.size_rod_y / 2, 0) #body_rod_shape = chrono.ChCylinder(cyl_base1, cyl_base2, radius_rod) self.body_rod_shape = chrono.ChCylinderShape() self.body_rod_shape.GetCylinderGeometry().p1 = self.cyl_base1 self.body_rod_shape.GetCylinderGeometry().p2 = self.cyl_base2 self.body_rod_shape.GetCylinderGeometry().rad = self.radius_rod #body_rod.GetAssets().push_back(body_rod_shape) self.body_rod.AddAsset(self.body_rod_shape) self.rev_pend_sys.Add(self.body_rod) self.body_floor = chrono.ChBody() self.body_floor.SetBodyFixed(True) self.body_floor.SetPos(chrono.ChVectorD(0, -5, 0)) self.body_floor.SetMaterialSurface(self.rod_material) # Visualization shape if self.render: self.body_floor_shape = chrono.ChBoxShape() self.body_floor_shape.GetBoxGeometry().Size = chrono.ChVectorD( 3, 1, 3) self.body_floor.GetAssets().push_back(self.body_floor_shape) self.body_floor_texture = chrono.ChTexture() self.body_floor_texture.SetTextureFilename( '../../../data/concrete.jpg') self.body_floor.GetAssets().push_back(self.body_floor_texture) self.rev_pend_sys.Add(self.body_floor) self.body_table = chrono.ChBody() self.body_table.SetPos(chrono.ChVectorD(0, -self.size_table_y / 2, 0)) self.body_table.SetMaterialSurface(self.rod_material) # Visualization shape if self.render: self.body_table_shape = chrono.ChBoxShape() self.body_table_shape.GetBoxGeometry().Size = chrono.ChVectorD( self.size_table_x / 2, self.size_table_y / 2, self.size_table_z / 2) self.body_table_shape.SetColor(chrono.ChColor(0.4, 0.4, 0.5)) self.body_table.GetAssets().push_back(self.body_table_shape) self.body_table_texture = chrono.ChTexture() self.body_table_texture.SetTextureFilename( '../../../data/concrete.jpg') self.body_table.GetAssets().push_back(self.body_table_texture) self.body_table.SetMass(0.1) self.rev_pend_sys.Add(self.body_table) # Create a constraint that blocks free 3 x y z translations and 3 rx ry rz rotations # of the table respect to the floor, and impose that the relative imposed position # depends on a specified motion law. self.link_slider = chrono.ChLinkLockPrismatic() z2x = chrono.ChQuaternionD() z2x.Q_from_AngAxis(-chrono.CH_C_PI / 2, chrono.ChVectorD(0, 1, 0)) self.link_slider.Initialize( self.body_table, self.body_floor, chrono.ChCoordsysD(chrono.ChVectorD(0, 0, 0), z2x)) self.rev_pend_sys.Add(self.link_slider) self.link_slider.SetMotion_axis(chrono.ChVectorD(1, 0, 0)) #attuatore lineare self.act_initpos = chrono.ChVectorD(0, 0, 0) self.actuator = chrono.ChLinkMotorLinearForce() self.actuator.Initialize(self.body_table, self.body_floor, chrono.ChFrameD(self.act_initpos)) self.rev_pend_sys.Add(self.actuator) # ..create the function for imposed y vertical motion, etc. # tolta: solo traslazione asse z #mfunY = chrono.ChFunction_Sine(0,1.5,0.001) # phase, frequency, amplitude #link_slider.SetMotion_Y(mfunY) # ..create the function for imposed z horizontal motion, etc. #self.mfunX = chrono.ChFunction_Sine(0,1.5,0.4) # phase, frequency, amplitude #self.link_slider.SetMotion_X(self.action) # Note that you could use other types of ChFunction_ objects, or create # your custom function by class inheritance (see demo_python.py), or also # set a function for table rotation , etc. # REVLOLUTE JOINT: # create frames for the joint self.rod_pin = chrono.ChMarker() self.body_rod.AddMarker(self.rod_pin) self.rod_pin.Impose_Abs_Coord( chrono.ChCoordsysD(chrono.ChVectorD(0, 0, 0))) self.table_pin = chrono.ChMarker() self.body_table.AddMarker(self.table_pin) self.table_pin.Impose_Abs_Coord( chrono.ChCoordsysD(chrono.ChVectorD(0, 0, 0))) self.pin_joint = chrono.ChLinkLockRevolute() self.pin_joint.Initialize(self.rod_pin, self.table_pin) self.rev_pend_sys.Add(self.pin_joint) if self.render: # --------------------------------------------------------------------- # # Create an Irrlicht application to visualize the system # # ==IMPORTANT!== Use this function for adding a ChIrrNodeAsset to all items # in the system. These ChIrrNodeAsset assets are 'proxies' to the Irrlicht meshes. # If you need a finer control on which item really needs a visualization proxy # Irrlicht, just use application.AssetBind(myitem); on a per-item basis. self.myapplication.AssetBindAll() # ==IMPORTANT!== Use this function for 'converting' into Irrlicht meshes the assets # that you added to the bodies into 3D shapes, they can be visualized by Irrlicht! self.myapplication.AssetUpdateAll() # If you want to show shadows because you used "AddLightWithShadow()' # you must remember this: self.isdone = False self.steps = 0 self.step(np.array([[0]])) 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()