def AddBodyForce(body, force, direction, new=True, isRelative=True):
if new:
direction = ChVecify(direction).GetNormalized()
F = chrono.ChForce()
F.SetF_x(chrono.ChFunction_Const(direction.x * force))
F.SetF_y(chrono.ChFunction_Const(direction.y * force))
F.SetF_z(chrono.ChFunction_Const(direction.z * force))
body.AddForce(F)
return F
return False
def doMove(self):
#print("z axis: " + str(self.robot.mbody1.GetRot().Q_to_Euler123().z*90 ))
try:
self.oldErrorFunc = self.errorFunc
except:
self.oldErrorFunc = 0
self.errorFunc = (
(self.robot.mbody1.GetRot().Q_to_Rotv().z) * 57.2957795) - 180
self.errorFuncDerivative = self.errorFunc - self.oldErrorFunc
try:
self.errorFuncIntegral += self.errorFunc
except:
self.errorFuncIntegral = self.errorFunc
self.output = (self.Kp * self.errorFunc) + (
self.Ki * self.errorFuncIntegral) + (self.Kd *
self.errorFuncDerivative)
print("")
print("no." + str(len(self.errorArray)) + " error: " +
str(self.errorFunc) + " errorInt: " +
str(self.errorFuncIntegral) + " errorDer: " +
str(self.errorFuncDerivative) + " out :" +
str(self.output))
print("")
if self.output < 0:
self.robot.motor_R.SetMotorFunction(
chrono.ChFunction_Const(self.maxspeed))
self.robot.motor_L.SetMotorFunction(
chrono.ChFunction_Const(self.maxspeed))
print("fall back" + str(self.robot.motor_R.GetMotorFunction()))
elif self.output > 0:
self.robot.motor_R.SetMotorFunction(
chrono.ChFunction_Const(-self.maxspeed))
self.robot.motor_L.SetMotorFunction(
chrono.ChFunction_Const(-self.maxspeed))
print("fall forward")
else:
self.robot.motor_R.SetMotorFunction(chrono.ChFunction_Const(0))
self.robot.motor_L.SetMotorFunction(chrono.ChFunction_Const(0))
self.errorArray.append(self.errorFunc)
self.errorIntArray.append(self.errorFuncIntegral)
self.errorDerArray.append(self.errorFuncDerivative)
self.outputArray.append(self.output)
def doMove(self, prediction):
self.checkIfDead()
print("pred is: ")
print(prediction)
print(" ")
if prediction[0][0] > prediction[0][1]:
speed = (prediction[0][0] * self.maxSpeed)
self.robot.motor_R.SetMotorFunction(chrono.ChFunction_Const(speed))
self.robot.motor_L.SetMotorFunction(chrono.ChFunction_Const(speed))
elif prediction[0][1] > prediction[0][0]:
speed = -(prediction[0][1] * self.maxSpeed)
self.robot.motor_R.SetMotorFunction(chrono.ChFunction_Const(speed))
self.robot.motor_L.SetMotorFunction(chrono.ChFunction_Const(speed))
else:
self.robot.motor_R.SetMotorFunction(chrono.ChFunction_Const(0))
self.robot.motor_L.SetMotorFunction(chrono.ChFunction_Const(0))
# Create a stylized piston
mpiston = chrono.ChBodyEasyCylinder(0.2, 0.3, 1000)
mpiston.SetPos(crank_center + chrono.ChVectorD(crank_rad + rod_length, 0, 0))
mpiston.SetRot(chrono.Q_ROTATE_Y_TO_X)
mysystem.Add(mpiston)
# Now create constraints and motors between the bodies.
# Create crank-truss joint: a motor that spins the crank flywheel
my_motor = chrono.ChLinkMotorRotationSpeed()
my_motor.Initialize(
mcrank, # the first connected body
mfloor, # the second connected body
chrono.ChFrameD(crank_center)) # where to create the motor in abs.space
my_angularspeed = chrono.ChFunction_Const(chrono.CH_C_PI) # ang.speed: 180°/s
my_motor.SetMotorFunction(my_angularspeed)
mysystem.Add(my_motor)
# Create crank-rod joint
mjointA = chrono.ChLinkLockRevolute()
mjointA.Initialize(
mrod, mcrank,
chrono.ChCoordsysD(crank_center + chrono.ChVectorD(crank_rad, 0, 0)))
mysystem.Add(mjointA)
# Create rod-piston joint
mjointB = chrono.ChLinkLockRevolute()
mjointB.Initialize(
mpiston, mrod,
chrono.ChCoordsysD(crank_center +
def AddContainer(sys):
# The fixed body (5 walls)
fixedBody = chrono.ChBody()
fixedBody.SetMass(1.0)
fixedBody.SetBodyFixed(True)
fixedBody.SetPos(chrono.ChVectorD())
fixedBody.SetCollide(True)
# Contact material for container
fixed_mat = chrono.ChMaterialSurfaceSMC()
fixedBody.GetCollisionModel().ClearModel()
AddContainerWall(fixedBody, fixed_mat, chrono.ChVectorD(20, 1, 20),
chrono.ChVectorD(0, -5, 0))
AddContainerWall(fixedBody, fixed_mat, chrono.ChVectorD(1, 10, 20.99),
chrono.ChVectorD(-10, 0, 0))
AddContainerWall(fixedBody, fixed_mat, chrono.ChVectorD(1, 10, 20.99),
chrono.ChVectorD(10, 0, 0))
AddContainerWall(fixedBody, fixed_mat, chrono.ChVectorD(20.99, 10, 1),
chrono.ChVectorD(0, 0, -10), False)
AddContainerWall(fixedBody, fixed_mat, chrono.ChVectorD(20.99, 10, 1),
chrono.ChVectorD(0, 0, 10))
fixedBody.GetCollisionModel().BuildModel()
texture = chrono.ChTexture()
texture.SetTextureFilename(
chrono.GetChronoDataFile("textures/concrete.jpg"))
fixedBody.AddAsset(texture)
sys.AddBody(fixedBody)
# The rotating mixer body
rotatingBody = chrono.ChBody()
rotatingBody.SetMass(10.0)
rotatingBody.SetInertiaXX(chrono.ChVectorD(50, 50, 50))
rotatingBody.SetPos(chrono.ChVectorD(0, -1.6, 0))
rotatingBody.SetCollide(True)
# Contact material for mixer body
rot_mat = chrono.ChMaterialSurfaceSMC()
hsize = chrono.ChVectorD(5, 2.75, 0.5)
rotatingBody.GetCollisionModel().ClearModel()
rotatingBody.GetCollisionModel().AddBox(rot_mat, hsize.x, hsize.y, hsize.z)
rotatingBody.GetCollisionModel().BuildModel()
box = chrono.ChBoxShape()
box.GetBoxGeometry().Size = hsize
rotatingBody.AddAsset(box)
rotatingBody.AddAsset(texture)
sys.AddBody(rotatingBody)
# A motor between the two
my_motor = chrono.ChLinkMotorRotationSpeed()
my_motor.Initialize(
rotatingBody, fixedBody,
chrono.ChFrameD(chrono.ChVectorD(0, 0, 0),
chrono.Q_from_AngAxis(chrono.CH_C_PI_2,
chrono.VECT_X)))
mfun = chrono.ChFunction_Const(chrono.CH_C_PI / 2.0) # speed w=90°/s
my_motor.SetSpeedFunction(mfun)
sys.AddLink(my_motor)
return rotatingBody
def AddContainer(sys):
# Contact material for container
ground_mat = chrono.ChMaterialSurfaceNSC()
# Create the five walls of the rectangular container, using fixed rigid bodies of 'box' type
floorBody = chrono.ChBodyEasyBox(20, 1, 20, 1000, True, True, ground_mat)
floorBody.SetPos(chrono.ChVectorD(0, -5, 0))
floorBody.SetBodyFixed(True)
sys.Add(floorBody)
wallBody1 = chrono.ChBodyEasyBox(1, 10, 20.99, 1000, True, True, ground_mat)
wallBody1.SetPos(chrono.ChVectorD(-10, 0, 0))
wallBody1.SetBodyFixed(True)
sys.Add(wallBody1)
wallBody2 = chrono.ChBodyEasyBox(1, 10, 20.99, 1000, True, True, ground_mat)
wallBody2.SetPos(chrono.ChVectorD(10, 0, 0))
wallBody2.SetBodyFixed(True)
sys.Add(wallBody2)
wallBody3 = chrono.ChBodyEasyBox(20.99, 10, 1, 1000, False, True, ground_mat)
wallBody3.SetPos(chrono.ChVectorD(0, 0, -10))
wallBody3.SetBodyFixed(True)
sys.Add(wallBody3)
wallBody4 = chrono.ChBodyEasyBox(20.99, 10, 1, 1000, True, True, ground_mat)
wallBody4.SetPos(chrono.ChVectorD(0, 0, 10))
wallBody4.SetBodyFixed(True)
sys.Add(wallBody4)
# optional, attach textures for better visualization
mtexturewall = chrono.ChTexture()
mtexturewall.SetTextureFilename(chrono.GetChronoDataFile("textures/concrete.jpg"))
wallBody1.AddAsset(mtexturewall) # note: most assets can be shared
wallBody2.AddAsset(mtexturewall)
wallBody3.AddAsset(mtexturewall)
wallBody4.AddAsset(mtexturewall)
floorBody.AddAsset(mtexturewall)
# Add the rotating mixer
mixer_mat = chrono.ChMaterialSurfaceNSC()
mixer_mat.SetFriction(0.4)
rotatingBody = chrono.ChBodyEasyBox(10, 5, 1, # x,y,z size
4000, # density
True, # visualization?
True, # collision?
mixer_mat) # contact material
rotatingBody.SetPos(chrono.ChVectorD(0, -1.6, 0))
sys.Add(rotatingBody)
# .. a motor between mixer and truss
my_motor = chrono.ChLinkMotorRotationSpeed()
my_motor.Initialize(rotatingBody,
floorBody,
chrono.ChFrameD(chrono.ChVectorD(0, 0, 0),
chrono.Q_from_AngAxis(chrono.CH_C_PI_2, chrono.VECT_X)))
mfun = chrono.ChFunction_Const(chrono.CH_C_PI / 4.0) # speed 45 deg/s
my_motor.SetSpeedFunction(mfun)
sys.AddLink(my_motor)
# NOTE: Instead of creating five separate 'box' bodies to make
# the walls of the container, you could have used a single body
# made of five box shapes, which build a single collision description,
# as in the alternative approach:
"""
# create a plain ChBody (no colliding shape nor visualization mesh is used yet)
mrigidBody = chrono.ChBody()
# set as fixed body, and turn collision ON, otherwise no collide by default
mrigidBody.SetBodyFixed(True)
mrigidBody.SetCollide(True)
# Clear model. The colliding shape description MUST be between ClearModel() .. BuildModel() pair.
mrigidBody.GetCollisionModel().ClearModel()
# Describe the (invisible) colliding shape by adding five boxes (the walls and floor)
mrigidBody.GetCollisionModel().AddBox(ground_mat, 20, 1, 20, chrono.ChVectorD(0, -10, 0))
mrigidBody.GetCollisionModel().AddBox(ground_mat, 1, 40, 20, chrono.ChVectorD(-11, 0, 0))
mrigidBody.GetCollisionModel().AddBox(ground_mat, 1, 40, 20, chrono.ChVectorD(11, 0, 0))
mrigidBody.GetCollisionModel().AddBox(ground_mat, 20, 40, 1, chrono.ChVectorD(0, 0, -11))
mrigidBody.GetCollisionModel().AddBox(ground_mat, 20, 40, 1, chrono.ChVectorD(0, 0, 11))
# Complete the description of collision shape.
mrigidBody.GetCollisionModel().BuildModel()
# Attach some visualization shapes if needed:
vshape = chrono.ChBoxShape()
vshape.GetBoxGeometry().SetLengths(chrono.ChVectorD(20, 1, 20))
vshape.GetBoxGeometry().Pos = chrono.ChVectorD(0, -5, 0)
this.AddAsset(vshape)
# etc. for other 4 box shapes..
"""
return rotatingBody
positionA1 = chrono.ChVectorD(-3, 2, -3)
stator1, rotor1 = CreateStatorRotor(material, mphysicalSystem, positionA1)
# Create the motor
rotmotor1 = chrono.ChLinkMotorRotationSpeed()
# Connect the rotor and the stator and add the motor to the system:
rotmotor1.Initialize(
rotor1, # body A (slave)
stator1, # body B (master)
chrono.ChFrameD(positionA1) # motor frame, in abs. coords
)
mphysicalSystem.Add(rotmotor1)
# Create a ChFunction to be used for the ChLinkMotorRotationSpeed
mwspeed = chrono.ChFunction_Const(
chrono.CH_C_PI_2) # constant angular speed, in [rad/s], 1PI/s =180°/s
# Let the motor use this motion function:
rotmotor1.SetSpeedFunction(mwspeed)
# The ChLinkMotorRotationSpeed contains a hidden state that performs the time integration
# of the angular speed setpoint: such angle is then imposed to the
# constraat the positional level too, thus avoiding angle error
# accumulation (angle drift). Optionally, such positional constraint
# level can be disabled as follows:
#
# rotmotor1.SetAvoidAngleDrift(False)
# EXAMPLE A.2
#
# - class: ChLinkMotorRotationAngle
# - type: rotational motor
#
# EXAMPLE 2:
#
# Create a ChBody that contains the trajectory
mwheel = chrono.ChBody()
mwheel.SetPos(chrono.ChVectorD(-3, 2, 0))
mysystem.Add(mwheel)
# Create a motor that spins the wheel
my_motor = chrono.ChLinkMotorRotationSpeed()
my_motor.Initialize(mwheel, mfloor, chrono.ChFrameD(chrono.ChVectorD(-3, 2,
0)))
my_angularspeed = chrono.ChFunction_Const(chrono.CH_C_PI / 4.0)
my_motor.SetMotorFunction(my_angularspeed)
mysystem.Add(my_motor)
# Create a ChLinePath geometry, and insert sub-paths:
mglyph = chrono.ChLinePath()
ms1 = chrono.ChLineSegment(chrono.ChVectorD(-0.5, -0.5, 0),
chrono.ChVectorD(0.5, -0.5, 0))
mglyph.AddSubLine(ms1)
ma1 = chrono.ChLineArc(chrono.ChCoordsysD(chrono.ChVectorD(0.5, 0, 0)), 0.5,
-chrono.CH_C_PI_2, chrono.CH_C_PI_2, True)
mglyph.AddSubLine(ma1)
ms2 = chrono.ChLineSegment(chrono.ChVectorD(0.5, 0.5, 0),
chrono.ChVectorD(-0.5, 0.5, 0))
mglyph.AddSubLine(ms2)
ma2 = chrono.ChLineArc(chrono.ChCoordsysD(chrono.ChVectorD(-0.5, 0, 0)), 0.5,
