def DrawCones(self, points, color, z=.3, n=10): for p in points[::n]: p.z += z cmesh = chrono.ChTriangleMeshConnected() if color == 'red': cmesh.LoadWavefrontMesh( chrono.GetChronoDataFile("sensor/cones/red_cone.obj"), False, True) elif color == 'green': cmesh.LoadWavefrontMesh( chrono.GetChronoDataFile("sensor/cones/green_cone.obj"), False, True) cshape = chrono.ChTriangleMeshShape() cshape.SetMesh(cmesh) cshape.SetName("Cone") cshape.SetStatic(True) cbody = chrono.ChBody() cbody.SetPos(p) cbody.AddAsset(cshape) cbody.SetBodyFixed(True) cbody.SetCollide(False) if color == 'red': cbody.AddAsset(chrono.ChColorAsset(1, 0, 0)) elif color == 'green': cbody.AddAsset(chrono.ChColorAsset(0, 1, 0)) self.system.Add(cbody)
def DrawBarriers(self, points, n=5, height=1, width=1): points = points[::n] if points[-1] != points[0]: points.append(points[-1]) for i in range(len(points) - 1): p1 = points[i] p2 = points[i + 1] box = chrono.ChBodyEasyBox((p2 - p1).Length(), height, width, 1000, True, True) box.SetPos(p1) q = chrono.ChQuaternionD() v1 = p2 - p1 v2 = chrono.ChVectorD(1, 0, 0) ang = math.atan2((v1 % v2).Length(), v1 ^ v2) if chrono.ChVectorD(0, 0, 1) ^ (v1 % v2) > 0.0: ang *= -1 q.Q_from_AngZ(ang) box.SetRot(q) box.SetBodyFixed(True) color = chrono.ChColorAsset() if i % 2 == 0: color.SetColor(chrono.ChColor(1, 0, 0)) else: color.SetColor(chrono.ChColor(1, 1, 1)) box.AddAsset(color) self.system.Add(box)
def add_asset(self, asset: 'Any'): if isinstance(asset, chrono.ChBody): self._system._system.AddBody(asset) return if isinstance(asset, WABody): if not hasattr(asset, 'size') or not hasattr(asset, 'position'): raise AttributeError( "Body must have 'size', and 'position' fields") position = asset.position yaw = 0 if not hasattr(asset, 'yaw') else asset.yaw size = asset.size body_type = 'box' if hasattr(asset, 'body_type'): body_type = asset.body_type if body_type == 'sphere': body = chrono.ChBodyEasySphere(size.length, 1000, True, False) body.SetBodyFixed(True) elif body_type == 'box': body = chrono.ChBodyEasyBox( size.x, size.y, size.z, 1000, True, False) body.SetBodyFixed(True) else: raise ValueError( f"'{asset.body_type}' not a supported body type.") body.SetPos(WAVector_to_ChVector(position)) body.SetRot(chrono.Q_from_AngZ(-yaw + WA_PI / 2)) if hasattr(asset, 'color'): color = asset.color body.AddAsset(chrono.ChColorAsset( chrono.ChColor(color.x, color.y, color.z))) texture = chrono.ChVisualMaterial() texture.SetDiffuseColor( chrono.ChVectorF(color.x, color.y, color.z)) chrono.CastToChVisualization( body.GetAssets()[0]).material_list.append(texture) if hasattr(asset, 'texture'): texture = chrono.ChVisualMaterial() texture.SetKdTexture(get_wa_data_file(asset.texture)) chrono.CastToChVisualization( body.GetAssets()[0]).material_list.append(texture) self._system._system.AddBody(body) asset.chrono_body = body super().add_asset(asset)
def DrawBarriers(self, points, n=5, height=1, width=1): points = points[::n] if points[-1] != points[0]: points.append(points[-1]) for i in range(len(points) - 1): p1 = points[i] p2 = points[i + 1] box = chrono.ChBodyEasyBox((p2 - p1).Length(), height, width, 1000, True, True) box.SetPos(p1) q = chrono.ChQuaternionD() v1 = p2 - p1 v2 = chrono.ChVectorD(1, 0, 0) ang = math.atan2((v1 % v2).Length(), v1 ^ v2) if chrono.ChVectorD(0, 0, 1) ^ (v1 % v2) > 0.0: ang *= -1 q.Q_from_AngZ(ang) box.SetRot(q) box.SetBodyFixed(True) box_asset = box.GetAssets()[0] visual_asset = chrono.CastToChVisualization(box_asset) vis_mat = chrono.ChVisualMaterial() vis_mat.SetAmbientColor(chrono.ChVectorF(0, 0, 0)) if i % 2 == 0: vis_mat.SetDiffuseColor(chrono.ChVectorF(1.0, 0, 0)) else: vis_mat.SetDiffuseColor(chrono.ChVectorF(1.0, 1.0, 1.0)) vis_mat.SetSpecularColor(chrono.ChVectorF(0.9, 0.9, 0.9)) vis_mat.SetFresnelMin(0) vis_mat.SetFresnelMax(0.1) visual_asset.material_list.append(vis_mat) color = chrono.ChColorAsset() if i % 2 == 0: color.SetColor(chrono.ChColor(1, 0, 0)) else: color.SetColor(chrono.ChColor(1, 1, 1)) box.AddAsset(color) self.system.Add(box) self.barriers.append(box)
def create_sphere_in_chrono( system: 'WAChronoSystem', rgb=(1, 0, 0)) -> chrono.ChBodyEasySphere: """Create and add a sphere to the chrono world Args: system (WASystem): the system that manages the simulation rgb (tuple, optional): (red, green, blue). Defaults to (1, 0, 0) or red. Returns: chrono.ChBodyEasySphere: the sphere that was created """ sphere = chrono.ChBodyEasySphere(0.25, 1000, True, False) sphere.SetBodyFixed(True) sphere.AddAsset(chrono.ChColorAsset(*rgb)) texture = chrono.ChVisualMaterial() texture.SetDiffuseColor(chrono.ChVectorF(*rgb)) chrono.CastToChVisualization( sphere.GetAssets()[0]).material_list.append(texture) system._system.Add(sphere) return sphere
def CreateTerrain(sys, length, width, height, offset): ground_mat = chrono.ChMaterialSurface.DefaultMaterial( sys.GetContactMethod()) ground_mat.SetFriction(0.8) ground_mat.SetRestitution(0) if sys.GetContactMethod() == chrono.ChContactMethod_SMC: matSMC = chrono.CastToChMaterialSurfaceSMC(ground_mat) matSMC.SetYoungModulus(1e7) ground = robot.GetSystem().NewBody() ground.SetBodyFixed(True) ground.SetCollide(True) ground.GetCollisionModel().ClearModel() ground.GetCollisionModel().AddBox( ground_mat, length / 2, width / 2, 0.1, chrono.ChVectorD(offset, 0, height - 0.1)) ground.GetCollisionModel().BuildModel() box = chrono.ChBoxShape() box.GetBoxGeometry().Size = chrono.ChVectorD(length / 2, width / 2, 0.1) box.GetBoxGeometry().Pos = chrono.ChVectorD(offset, 0, height - 0.1) ground.AddAsset(box) texture = chrono.ChTexture() texture.SetTextureFilename( chrono.GetChronoDataFile("textures/pinkwhite.png")) texture.SetTextureScale(10 * length, 10 * width) ground.AddAsset(texture) ground.AddAsset(chrono.ChColorAsset(0.8, 0.8, 0.8)) sys.AddBody(ground) return ground
## - visualization assets (defined with respect to the body frame) ## Ground ground = chrono.ChBody() system.AddBody(ground) ground.SetIdentifier(-1) ground.SetName("ground") ground.SetBodyFixed(True) cyl_g = chrono.ChCylinderShape() cyl_g.GetCylinderGeometry().p1 = chrono.ChVectorD(0, 0.2, 0) cyl_g.GetCylinderGeometry().p2 = chrono.ChVectorD(0, -0.2, 0) cyl_g.GetCylinderGeometry().rad = 0.03 ground.AddAsset(cyl_g) col_g = chrono.ChColorAsset() col_g.SetColor(chrono.ChColor(0.6, 0.6, 0.2)) ground.AddAsset(col_g) ## Crank crank = chrono.ChBody() system.AddBody(crank) crank.SetIdentifier(1) crank.SetName("crank") crank.SetMass(1.0) crank.SetInertiaXX(chrono.ChVectorD(0.005, 0.1, 0.1)) crank.SetPos(chrono.ChVectorD(-1, 0, 0)) crank.SetRot(chrono.ChQuaternionD(1, 0, 0, 0)) box_c = chrono.ChBoxShape() box_c.GetBoxGeometry().Size = chrono.ChVectorD(0.95, 0.05, 0.05)
def __init__(self, step_size, sys, controller, irrlicht=False, vehicle_type='json', initLoc=chrono.ChVectorD(0,0,0), initRot=chrono.ChQuaternionD(1,0,0,0), vis_balls=False, render_step_size=1.0/60): # Chrono parameters self.step_size = step_size self.irrlicht = irrlicht self.step_number = 0 # Vehicle controller self.controller = controller # Initial vehicle position self.initLoc = initLoc # Initial vehicle orientation self.initRot = initRot # Point on chassis tracked by the camera (Irrlicht only) self.trackPoint = chrono.ChVectorD(0.0, 0.0, 1.75) if vehicle_type == 'json': # JSON file for vehicle model self.vehicle_file = veh.GetDataPath() + os.path.join('hmmwv', 'vehicle', 'HMMWV_Vehicle.json') checkFile(self.vehicle_file) # JSON file for powertrain (simple) self.simplepowertrain_file = veh.GetDataPath() + os.path.join('generic', 'powertrain', 'SimplePowertrain.json') checkFile(self.simplepowertrain_file) # JSON files tire models (rigid) self.rigidtire_file = veh.GetDataPath() + os.path.join('hmmwv', 'tire', 'HMMWV_RigidTire.json') checkFile(self.rigidtire_file) # -------------------------- # Create the various modules # -------------------------- if sys == None: self.wheeled_vehicle = veh.WheeledVehicle(self.vehicle_file) else: self.wheeled_vehicle = veh.WheeledVehicle(sys, self.vehicle_file) self.wheeled_vehicle.Initialize(chrono.ChCoordsysD(self.initLoc, self.initRot)) self.wheeled_vehicle.SetChassisVisualizationType(veh.VisualizationType_PRIMITIVES) self.wheeled_vehicle.SetSuspensionVisualizationType(veh.VisualizationType_PRIMITIVES) self.wheeled_vehicle.SetSteeringVisualizationType(veh.VisualizationType_PRIMITIVES) self.wheeled_vehicle.SetWheelVisualizationType(veh.VisualizationType_NONE) # Create and initialize the powertrain system self.powertrain = veh.SimplePowertrain(self.simplepowertrain_file) self.wheeled_vehicle.InitializePowertrain(self.powertrain) # Create and initialize the tires for axle in self.wheeled_vehicle.GetAxles(): tireL = veh.RigidTire(self.rigidtire_file) self.wheeled_vehicle.InitializeTire(tireL, axle.m_wheels[0], veh.VisualizationType_MESH) tireR = veh.RigidTire(self.rigidtire_file) self.wheeled_vehicle.InitializeTire(tireR, axle.m_wheels[1], veh.VisualizationType_MESH) self.vehicle = self.wheeled_vehicle self.sys = self.wheeled_vehicle.GetSystem() elif vehicle_type == 'rccar': if sys == None: self.rc_vehicle = veh.RCCar() self.rc_vehicle.SetContactMethod(chrono.ChMaterialSurface.SMC) self.rc_vehicle.SetChassisCollisionType(veh.ChassisCollisionType_NONE) else: self.rc_vehicle = veh.RCCar(sys) self.rc_vehicle.SetChassisFixed(False) self.rc_vehicle.SetInitPosition(chrono.ChCoordsysD(initLoc, initRot)) self.rc_vehicle.SetTireType(veh.TireModelType_RIGID) self.rc_vehicle.SetTireStepSize(step_size) self.rc_vehicle.Initialize() self.rc_vehicle.SetChassisVisualizationType(veh.VisualizationType_PRIMITIVES) self.rc_vehicle.SetSuspensionVisualizationType(veh.VisualizationType_PRIMITIVES) self.rc_vehicle.SetSteeringVisualizationType(veh.VisualizationType_PRIMITIVES) self.rc_vehicle.SetWheelVisualizationType(veh.VisualizationType_PRIMITIVES) self.rc_vehicle.SetTireVisualizationType(veh.VisualizationType_PRIMITIVES) self.vehicle = self.rc_vehicle.GetVehicle() self.sys = self.vehicle.GetSystem() self.trackPoint = chrono.ChVectorD(4, 0.0, .15) elif vehicle_type == 'sedan': if sys == None: self.sedan = veh.Sedan() self.sedan.SetContactMethod(chrono.ChMaterialSurface.NSC) self.sedan.SetChassisCollisionType(veh.ChassisCollisionType_NONE) else: self.sedan = veh.Sedan(sys) self.sedan.SetChassisFixed(False) self.sedan.SetInitPosition(chrono.ChCoordsysD(initLoc, initRot)) self.sedan.SetTireType(veh.TireModelType_RIGID) self.sedan.SetTireStepSize(step_size) self.sedan.Initialize() self.sedan.SetChassisVisualizationType(veh.VisualizationType_PRIMITIVES) self.sedan.SetSuspensionVisualizationType(veh.VisualizationType_PRIMITIVES) self.sedan.SetSteeringVisualizationType(veh.VisualizationType_PRIMITIVES) self.sedan.SetWheelVisualizationType(veh.VisualizationType_PRIMITIVES) self.sedan.SetTireVisualizationType(veh.VisualizationType_PRIMITIVES) self.vehicle = self.sedan.GetVehicle() self.sys = self.vehicle.GetVehicle().GetSystem() # ------------- # Create driver # ------------- self.driver = Driver(self.vehicle) self.driver.SetStepSize(step_size) # Set the time response for steering and throttle inputs. # NOTE: this is not exact, since we do not render quite at the specified FPS. steering_time = 1.0 # time to go from 0 to +1 (or from 0 to -1) throttle_time = 1.0 # time to go from 0 to +1 braking_time = 0.3 # time to go from 0 to +1 self.driver.SetSteeringDelta(render_step_size / steering_time) self.driver.SetThrottleDelta(render_step_size / throttle_time) self.driver.SetBrakingDelta(render_step_size / braking_time) self.vis_balls = vis_balls if self.vis_balls: self.sentinel_sphere = chrono.ChBodyEasySphere(.25, 1000, False, True) self.sentinel_sphere.SetBodyFixed(True) self.sentinel_sphere.AddAsset(chrono.ChColorAsset(1,0,0)) self.sys.Add(self.sentinel_sphere) self.sentinel_target = chrono.ChBodyEasySphere(.25, 1000, False, True) self.sentinel_target.SetBodyFixed(True) self.sentinel_target.AddAsset(chrono.ChColorAsset(0,1,0)); self.sys.Add(self.sentinel_target) # Vehicle parameters for matplotlib self.length = self.vehicle.GetWheelbase() + 2.0 # [m] self.width = self.vehicle.GetWheeltrack(0) # [m] self.backtowheel = 1.0 # [m] self.wheel_len = self.vehicle.GetWheel(0, 1).GetWidth() * 2 # [m] self.wheel_width = self.vehicle.GetWheel(0, 1).GetWidth() # [m] self.tread = self.vehicle.GetWheeltrack(0) / 2 # [m] self.wb = self.vehicle.GetWheelbase() # [m] self.offset = [-4.0,0] # [m]
system.AddBody(ground) ground.SetIdentifier(-1) ground.SetBodyFixed(True) ground.SetCollide(False) rail1 = chrono.ChBoxShape() rail1.GetBoxGeometry().SetLengths(chrono.ChVectorD(8, 0.1, 0.1)) rail1.GetBoxGeometry().Pos = chrono.ChVectorD(0, 0, -1) ground.AddAsset(rail1) rail2 = chrono.ChBoxShape() rail2.GetBoxGeometry().SetLengths(chrono.ChVectorD(8, 0.1, 0.1)) rail2.GetBoxGeometry().Pos = chrono.ChVectorD(0, 0, 1) ground.AddAsset(rail2) col = chrono.ChColorAsset() col.SetColor(chrono.ChColor(0.6, 0.6, 0.6)) ground.AddAsset(col) # Create the slider bodies slider1 = chrono.ChBody() system.AddBody(slider1) slider1.SetIdentifier(1) slider1.SetBodyFixed(False) slider1.SetCollide(False) slider1.SetMass(1) slider1.SetInertiaXX(chrono.ChVectorD(0.1, 0.1, 0.1)) slider1.SetPos(chrono.ChVectorD(-4, 0, -1)) cyl1 = chrono.ChCylinderShape() cyl1.GetCylinderGeometry().p1 = chrono.ChVectorD(-0.2, 0, 0)
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()
# ---------- # Add bodies # ---------- container = chrono.ChBody() system.Add(container) container.SetPos(chrono.ChVectorD(0, 0, 0)) container.SetBodyFixed(True) container.SetIdentifier(-1) container.SetCollide(True) container.GetCollisionModel().ClearModel() chrono.AddBoxGeometry(container, material, chrono.ChVectorD(4, 0.5, 4), chrono.ChVectorD(0, -0.5, 0)) container.GetCollisionModel().BuildModel() container.AddAsset(chrono.ChColorAsset(chrono.ChColor(0.4, 0.4, 0.4))) box1 = chrono.ChBody() box1.SetMass(10) box1.SetInertiaXX(chrono.ChVectorD(1, 1, 1)) box1.SetPos(chrono.ChVectorD(-1, 0.21, -1)) box1.SetPos_dt(chrono.ChVectorD(5, 0, 0)) box1.SetCollide(True) box1.GetCollisionModel().ClearModel() chrono.AddBoxGeometry(box1, material, chrono.ChVectorD(0.4, 0.2, 0.1)) box1.GetCollisionModel().BuildModel() box1.AddAsset(chrono.ChColorAsset(chrono.ChColor(0.1, 0.1, 0.4))) system.AddBody(box1)
body.SetIdentifier(1) body.SetBodyFixed(False) body.SetCollide(False) body.SetMass(1) body.SetInertiaXX(chrono.ChVectorD(1, 1, 1)) # Attach visualization assets sph = chrono.ChSphereShape() sph.GetSphereGeometry().rad = 0.3 body.AddAsset(sph) cyl = chrono.ChCylinderShape() cyl.GetCylinderGeometry().p1 = chrono.ChVectorD(-1.5, 0, 0) cyl.GetCylinderGeometry().p2 = chrono.ChVectorD(0, 0, 0) cyl.GetCylinderGeometry().rad = 0.1 body.AddAsset(cyl) col = chrono.ChColorAsset() col.SetColor(chrono.ChColor(0.7, 0.8, 0.8)) body.AddAsset(col) # Create revolute joint between body and ground rev = chrono.ChLinkLockRevolute() rev.Initialize(body, ground, chrono.ChCoordsysD(rev_pos, rev_rot)) system.AddLink(rev) # Create the rotational spring between body and ground torque = MySpringTorque() spring = chrono.ChLinkRotSpringCB() spring.Initialize(body, ground, chrono.ChCoordsysD(rev_pos, rev_rot)) spring.RegisterTorqueFunctor(torque) system.AddLink(spring)
# ------------------------------------------------------------- body_1 = chrono.ChBody() system.AddBody(body_1) body_1.SetPos(chrono.ChVectorD(-1, -3, 0)) body_1.SetIdentifier(1) body_1.SetBodyFixed(False) body_1.SetCollide(False) body_1.SetMass(1) body_1.SetInertiaXX(chrono.ChVectorD(1, 1, 1)) # Attach a visualization asset. box_1 = chrono.ChBoxShape() box_1.GetBoxGeometry().SetLengths(chrono.ChVectorD(1, 1, 1)) body_1.AddAsset(box_1) col_1 = chrono.ChColorAsset() col_1.SetColor(chrono.ChColor(0.6, 0, 0)) body_1.AddAsset(col_1) # Create the spring between body_1 and ground. The spring end points are # specified in the body relative frames. spring_1 = chrono.ChLinkTSDA() spring_1.Initialize(body_1, ground, True, chrono.ChVectorD(0, 0, 0), chrono.ChVectorD(-1, 0, 0), False, rest_length) spring_1.SetSpringCoefficient(spring_coef) spring_1.SetDampingCoefficient(damping_coef) system.AddLink(spring_1) # Attach a visualization asset. spring_1.AddAsset(col_1) spring_1.AddAsset(chrono.ChPointPointSpring(0.05, 80, 15))
pend.SetBodyFixed(False) pend.SetCollide(False) pend.SetMass(1) pend.SetInertiaXX(chrono.ChVectorD(0.2, 1, 1)) # Initial position of the pendulum (horizontal, pointing towards positive X). pend.SetPos(chrono.ChVectorD(1.5, 0, 0)) # Attach visualization assets. cyl_p = chrono.ChCylinderShape() cyl_p.GetCylinderGeometry().p1 = chrono.ChVectorD(-1.46, 0, 0) cyl_p.GetCylinderGeometry().p2 = chrono.ChVectorD(1.46, 0, 0) cyl_p.GetCylinderGeometry().rad = 0.2 pend.AddAsset(cyl_p) col_p = chrono.ChColorAsset() col_p.SetColor(chrono.ChColor(0.6, 0, 0)) pend.AddAsset(col_p) # Create a revolute joint to connect pendulum to ground rev = chrono.ChLinkLockRevolute() system.AddLink(rev) # Add limits to the Z rotation of the revolute joint min_angle = 0 max_angle = 0.75 * m.pi rev.GetLimit_Rz().SetActive(True) rev.GetLimit_Rz().SetMin(min_angle) rev.GetLimit_Rz().SetMax(max_angle) # Initialize the joint specifying a coordinate system (expressed in the absolute frame).