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).
