def step(self, ac):
xposbefore = self.body_abdomen.GetPos().x
self.numsteps += 1
if (self.animate):
self.myapplication.GetDevice().run()
self.myapplication.BeginScene()
self.myapplication.DrawAll()
self.ac = ac.reshape((-1,))
for i in range(len(self.leg_motor)):
action_a = chrono.ChFunction_Const(self.gain*float(self.ac[i]))
action_b = chrono.ChFunction_Const(self.gain*float(self.ac[i+4]))
self.leg_motor[i].SetTorqueFunction(action_a)
self.ankle_motor[i].SetTorqueFunction(action_b)
if (self.animate):
self.myapplication.DoStep()
self.myapplication.EndScene()
else:
self.ant_sys.DoStepDynamics(self.timestep)
obs= self.get_ob()
rew = self.calc_rew(xposbefore)
self.is_done()
return obs, rew, self.isdone, self.info
def step(self, ac):
#posbefore = self.body_abdomen.GetPos().x
self.numsteps += 1
self.ac = ac.reshape((-1, ))
#TODO: do not control each timestep. Frequency not reachable by real world contorllers
torques = np.multiply(self.ac, self.maxT)
for i, t in enumerate(torques):
if self.revs[i].GetRelWvel().z > self.maxSpeed[i] * (
math.pi / 180) and torques[i] > 0:
torques[i] = 0
if self.revs[i].GetRelWvel().z < -self.maxSpeed[i] * (
math.pi / 180) and torques[i] < 0:
torques[i] = 0
for m, t in zip(self.motors, torques):
m.SetTorqueFunction(chrono.ChFunction_Const(float(t)))
if (self.animate):
self.myapplication.GetDevice().run()
self.myapplication.BeginScene()
self.myapplication.DrawAll()
self.myapplication.DoStep()
self.myapplication.EndScene()
else:
self.robosystem.DoStepDynamics(self.timestep)
obs = self.get_ob()
rew = self.calc_rew()
self.is_done()
return obs, rew, self.isdone, self.info
def step(self, ac):
xposbefore = self.body_abdomen.GetPos().x
self.numsteps += 1
self.ac = ac.reshape((-1,))
for i in range(len(self.leg_motor)):
action_a = chrono.ChFunction_Const(self.gain*float(self.ac[i]))
action_b = chrono.ChFunction_Const(self.gain*float(self.ac[i+4]))
self.leg_motor[i].SetTorqueFunction(action_a)
self.ankle_motor[i].SetTorqueFunction(action_b)
self.ant_sys.DoStepDynamics(self.timestep)
obs= self.get_ob()
rew = self.calc_rew(xposbefore)
self.is_done()
return obs, rew, self.isdone, self.info
def step(self, ac):
action = float(ac[0])
self.steps += 1
self.ac = chrono.ChFunction_Const(action)
self.actuator.SetForceFunction(self.ac)
self.omega = self.pin_joint.GetRelWvel().Length()
self.rev_pend_sys.DoStepDynamics(self.timestep)
self.rew = 1.0
self.obs = self.get_ob()
self.is_done()
return self.obs, self.rew, self.isdone, self.info
def step(self, ac):
#posbefore = self.body_abdomen.GetPos().x
self.numsteps += 1
self.ac = ac.reshape((-1, ))
torques = np.multiply(self.ac, self.maxT)
for i, t in enumerate(torques):
if self.revs[i].GetRelWvel().z > self.maxSpeed[i] * (
math.pi / 180) and torques[i] > 0:
torques[i] = 0
if self.revs[i].GetRelWvel().z < -self.maxSpeed[i] * (
math.pi / 180) and torques[i] < 0:
torques[i] = 0
for m, t in zip(self.motors, torques):
m.SetTorqueFunction(chrono.ChFunction_Const(float(t)))
self.hexapod_sys.DoStepDynamics(self.timestep)
obs = self.get_ob()
rew = self.calc_rew()
self.is_done()
return obs, rew, self.isdone, self.info
def step(self, ac):
action = float(ac[0])
self.steps += 1
self.ac = chrono.ChFunction_Const(action)
self.actuator.SetForceFunction(self.ac)
self.omega = self.pin_joint.GetRelWvel().Length()
if self.render:
self.myapplication.GetDevice().run()
self.myapplication.BeginScene()
self.myapplication.DrawAll()
self.myapplication.DoStep()
else:
self.rev_pend_sys.DoStepDynamics(self.timestep)
self.rew = 1.0
self.obs = self.get_ob()
if self.render:
self.myapplication.EndScene()
self.is_done()
return self.obs, self.rew, self.isdone, self.info
my_ground = my_system.SearchBody('ground')
if not my_ground:
sys.exit('Error: cannot find ground from its name in the C::E system!')
# ***TRICK***
# Create an engine along the Z direction of the coordsystem specified by
# the marker, and acting between shaft and ground
revolute_frame = my_marker.GetAbsFrame()
link_motor = chrono.ChLinkMotorRotationSpeed()
link_motor.Initialize(my_shaft, my_ground, revolute_frame)
link_motor.SetSpindleConstraint(
chrono.ChLinkMotorRotationSpeed.SpindleConstraint_CYLINDRICAL
) # Set_shaft_mode(chrono.ChLinkEngine.ENG_SHAFT_PRISM)
link_motor.SetMotorFunction(chrono.ChFunction_Const(
1.0 * chrono.CH_C_2PI)) # 1.0 Hz to rad/s
my_system.Add(link_motor)
if m_visualization == "pov":
# ---------------------------------------------------------------------
#
# Render a short animation by generating scripts
# to be used with POV-Ray
#
pov_exporter = postprocess.ChPovRay(my_system)
# Sets some file names for in-out processes.
pov_exporter.SetTemplateFile("_template_POV.pov")
pov_exporter.SetOutputScriptFile("rendering_frames.pov")
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)
mshaft_shape.GetCylinderGeometry().p2 = chrono.ChVectorD(0, 10, 0)
mshaft_shape.GetCylinderGeometry().rad = radA * 0.4
mbody_gearA.AddAsset(mshaft_shape)
# ...impose rotation speed between the first gear and the fixed truss
link_motor = chrono.ChLinkMotorRotationSpeed()
link_motor.Initialize(mbody_gearA, mbody_truss,
chrono.ChFrameD(chrono.ChVectorD(0, 0, 0), chrono.QUNIT))
link_motor.SetSpeedFunction(chrono.ChFunction_Const(6))
mphysicalSystem.AddLink(link_motor)
# ...the second gear
interaxis12 = radA + radB
mbody_gearB = chrono.ChBodyEasyCylinder(radB, 0.4, 1000, True, False, mat)
mphysicalSystem.Add(mbody_gearB)
mbody_gearB.SetPos(chrono.ChVectorD(interaxis12, 0, -1))
mbody_gearB.SetRot(chrono.Q_from_AngX(m.pi / 2))
mbody_gearB.AddAsset(cylinder_texture)
# ...the second gear is fixed to the rotating bar
link_revolute = chrono.ChLinkLockRevolute()
link_revolute.Initialize(
mbody_gearB, mbody_train,
chrono.ChCoordsysD(chrono.ChVectorD(interaxis12, 0, 0), chrono.QUNIT))
## TO DO
## 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))
## Create a ChFunction object that always returns the constant value PI/2.
fun = chrono.ChFunction_Const()
fun.Set_yconst(chrono.CH_C_PI)
## Motor between ground and crank.
## Note that this also acts as a revolute joint (i.e. it enforces the same
## kinematic constraints as a revolute joint). As before, we apply the 'z2y'
## rotation to align the rotation axis with the Y axis of the global frame.
engine_ground_crank = chrono.ChLinkMotorRotationSpeed()
engine_ground_crank.SetName("engine_ground_crank")
engine_ground_crank.Initialize(ground, crank,
chrono.ChFrameD(chrono.ChVectorD(0, 0, 0), z2y))
engine_ground_crank.SetSpeedFunction(fun)
system.AddLink(engine_ground_crank)
## Prismatic joint between ground and slider.
## The translational axis of a prismatic joint is along the Z axis of the
yd = f_test.Get_y_dx(x)
ydd = f_test.Get_y_dxdx(x)
test_file.write("%f %f %f %f\n" % (x, y, yd, ydd))
test_file.close()
# Function sequence
# -----------------
f_seq = chrono.ChFunction_Sequence()
f_const_acc1 = chrono.ChFunction_ConstAcc()
f_const_acc1.Set_end(0.5) # ramp length
f_const_acc1.Set_h(0.3) # ramp height
f_seq.InsertFunct(f_const_acc1, 0.5, 1, False, False, False, 0)
f_const = chrono.ChFunction_Const()
f_seq.InsertFunct(f_const, 0.4, 1, True, False, False, -1)
f_const_acc2 = chrono.ChFunction_ConstAcc()
f_const_acc2.Set_end(0.6) # ramp length
f_const_acc2.Set_av(0.3) # acceleration ends after 30% length
f_const_acc2.Set_aw(0.7) # deceleration starts after 70% length
f_const_acc2.Set_h(-0.2) # ramp height
f_seq.InsertFunct(f_const_acc2, 0.6, 1, True, False, False, -1)
f_seq.Setup();
# Evaluate the function and its derivatives at 101 points in [0,2] and write to file
seq_file = open(out_dir + "/f_seq.out", "w+")
for i in range(101):
x = i / 50.0
def __init__(self,
sys,
gravity,
material,
width,
height,
position_base,
position_tip,
damping,
elements,
torque=10,
origin=True,
stator_constraint=None):
self.mesh = fea.ChMesh()
self.mesh.SetAutomaticGravity(gravity)
self.section = fea.ChBeamSectionAdvanced()
self.section.SetAsRectangularSection(width, height)
self.section.SetYoungModulus(material.modulus)
self.section.SetGshearModulus(material.shear)
self.section.SetDensity(material.density)
self.section.SetBeamRaleyghDamping(damping)
self.position_base = chrono.ChVectorD(*position_base)
self.position_tip = chrono.ChVectorD(*position_tip)
self.builder = fea.ChBuilderBeamEuler(
) #Use the beam builder assembly method to assembly each beam
self.builder.BuildBeam(
self.mesh,
self.section,
elements,
self.position_base,
self.position_tip,
chrono.ChVectorD(0, 1, 0),
)
self.stator = chrono.ChBodyEasyCylinder(0.01, 0.01, 1000)
self.stator.SetPos(self.position_base)
self.stator.SetBodyFixed(origin)
self.frame = chrono.ChFrameD(self.stator)
sys.Add(self.stator)
if (origin == False) and (
stator_constraint is not None
): #If the beam is not located at the origin, it must need to be constrained to another beam
self.constraint = chrono.ChLinkMateGeneric()
self.constraint.Initialize(self.stator, stator_constraint, False,
chrono.ChFrameD(self.stator),
chrono.ChFrameD(stator_constraint))
self.constraint.SetConstrainedCoords(True, True, True, True, True,
True)
sys.Add(self.constraint)
self.frame = chrono.ChFrameD(self.stator)
self.rotor = chrono.ChBodyEasyCylinder(0.011, 0.011, 1000)
self.rotor.SetPos(self.position_base)
sys.Add(self.rotor)
self.frame.SetRot(
chrono.Q_from_AngAxis(chrono.CH_C_PI_2, chrono.VECT_X)
) #Rotate the direction of rotation to be planar (x-z plane)
self.motor = chrono.ChLinkMotorRotationTorque()
self.motor.Initialize(self.rotor, self.stator, self.frame)
sys.Add(self.motor)
self.motor.SetTorqueFunction(chrono.ChFunction_Const(torque))
self.arm_base = (self.builder.GetLastBeamNodes().front())
self.arm_tip = (self.builder.GetLastBeamNodes().back())
self.mate = chrono.ChLinkMateGeneric()
self.mate.Initialize(self.builder.GetLastBeamNodes().front(),
self.rotor,
chrono.ChFrameD(self.builder.GetLastBeamNodes(
).front().GetPos())) #constrain beam to rotor
self.mate.SetConstrainedCoords(
True, True, True, True, True, True
) #constraints must be in format: (True,True,True,True,True,True) to constrain x,y,z,rotx,roty,rotz coordinates
sys.Add(self.mate)
self.visual = fea.ChVisualizationFEAmesh(self.mesh)
self.visual.SetFEMdataType(
fea.ChVisualizationFEAmesh.E_PLOT_ELEM_BEAM_MZ)
self.visual.SetColorscaleMinMax(-1.4, 1.4)
self.visual.SetSmoothFaces(True)
self.visual.SetWireframe(False)
self.mesh.AddAsset(self.visual)
sys.Add(self.mesh)
def motor_torque(arm): #define function which passes torque value to motors)
arm.motor.SetTorqueFunction(
chrono.ChFunction_Const(random.randrange(-10, 10, 1) * 5)
) #Insert function which calls value from agent here for motor torques
def set_torque(self, torque):
self.motor.SetTorqueFunction(chrono.ChFunction_Const(torque))
def make_model(system):
# write_xml()
# Make ground body
ground = make_box([1, 1, 10], [0, 0, -10], fixed=True, color=(1., 0.5, 0))
system.Add(ground)
# Make main body
main_body = make_box(geometry.body_size, geometry.body_center, fixed=False)
system.Add(main_body)
# Make right links
right_recip = make_box(geometry.recip_link_size,
geometry.right_recip_link_center,
fixed=False,
euler_angles=geometry.recip_link_euler)
system.Add(right_recip)
right_connector = make_box(geometry.connector_link_size,
geometry.right_connector_link_center,
fixed=False,
euler_angles=geometry.connector_link_euler)
system.Add(right_connector)
right_thigh = make_box(geometry.thigh_link_size,
geometry.right_thigh_link_center,
fixed=False,
euler_angles=geometry.thigh_link_euler)
system.Add(right_thigh)
right_shin = make_box(geometry.shin_link_size,
geometry.right_shin_link_center,
fixed=False,
euler_angles=geometry.shin_link_euler)
system.Add(right_shin)
right_4bar = make_box(geometry.four_bar_link_size,
geometry.right_4bar_link_center,
fixed=False,
euler_angles=geometry.four_bar_link_euler)
system.Add(right_4bar)
right_ankle1 = make_box(geometry.ankle_link_size1,
geometry.right_ankle_link_center1,
fixed=False,
collide=False,
euler_angles=geometry.ankle_link_euler1)
system.Add(right_ankle1)
right_ankle2 = make_box(geometry.ankle_link_size2,
geometry.right_ankle_link_center2,
fixed=False,
euler_angles=geometry.ankle_link_euler2)
system.Add(right_ankle2)
# right_foot = make_mesh('foot.stl', [0, 0, 0], fixed=True, collide=False)
# system.Add(right_foot)
# Setting up right joints
right_recip_connector_joint = make_joint(right_recip, right_connector,
geometry.right_recip_link_pos2)
system.Add(right_recip_connector_joint)
right_thigh_mount = make_joint(main_body, right_thigh,
geometry.right_thigh_link_pos1)
system.Add(right_thigh_mount)
right_thigh_connector_joint = make_joint(
right_connector, right_thigh, geometry.right_connector_link_pos2)
system.Add(right_thigh_connector_joint)
right_thigh_4bar_joint = make_joint(right_thigh, right_4bar,
geometry.right_4bar_link_pos1)
system.Add(right_thigh_4bar_joint)
right_shin_ankle_joint1 = make_joint(right_shin, right_ankle1,
geometry.right_shin_link_pos2)
system.Add(right_shin_ankle_joint1)
right_4bar_ankle_joint1 = make_joint(right_4bar, right_ankle1,
geometry.right_4bar_link_pos2)
system.Add(right_4bar_ankle_joint1)
right_shin_ankle_joint2 = make_joint(right_shin, right_ankle2,
geometry.right_shin_link_pos2)
system.Add(right_shin_ankle_joint2)
right_4bar_ankle_joint2 = make_joint(right_4bar, right_ankle2,
geometry.right_4bar_link_pos2)
system.Add(right_4bar_ankle_joint2)
right_recip_joint = make_joint(right_recip,
main_body,
geometry.right_recip_link_pos1,
joint_type=chrono.ChLinkMotorRotationTorque)
recip_func = chrono.ChFunction_Sequence()
recip_func.Set_start(3)
recip_func.InsertFunct(chrono.ChFunction_Const(-10), 100)
recip_func.Setup()
right_recip_joint.SetTorqueFunction(recip_func)
system.Add(right_recip_joint)
right_thigh_shin_joint = make_joint(right_thigh, right_shin,
geometry.right_shin_link_pos1)
system.Add(right_thigh_shin_joint)
'''
Now do it again, but for the left side!
'''
left_recip = make_box(geometry.recip_link_size,
geometry.left_recip_link_center,
fixed=False,
euler_angles=geometry.recip_link_euler)
system.Add(left_recip)
left_connector = make_box(geometry.connector_link_size,
geometry.left_connector_link_center,
fixed=False,
euler_angles=geometry.connector_link_euler)
system.Add(left_connector)
left_thigh = make_box(geometry.thigh_link_size,
geometry.left_thigh_link_center,
fixed=False,
euler_angles=geometry.thigh_link_euler)
system.Add(left_thigh)
left_shin = make_box(geometry.shin_link_size,
geometry.left_shin_link_center,
fixed=False,
euler_angles=geometry.shin_link_euler)
system.Add(left_shin)
left_4bar = make_box(geometry.four_bar_link_size,
geometry.left_4bar_link_center,
fixed=False,
euler_angles=geometry.four_bar_link_euler)
system.Add(left_4bar)
left_ankle1 = make_box(geometry.ankle_link_size1,
geometry.left_ankle_link_center1,
fixed=False,
collide=False,
euler_angles=geometry.ankle_link_euler1)
system.Add(left_ankle1)
left_ankle2 = make_box(geometry.ankle_link_size2,
geometry.left_ankle_link_center2,
fixed=False,
euler_angles=geometry.ankle_link_euler2)
system.Add(left_ankle2)
# Setting up left joints
left_recip_connector_joint = make_joint(left_recip, left_connector,
geometry.left_recip_link_pos2)
system.Add(left_recip_connector_joint)
left_thigh_mount = make_joint(main_body, left_thigh,
geometry.left_thigh_link_pos1)
system.Add(left_thigh_mount)
left_thigh_connector_joint = make_joint(left_connector, left_thigh,
geometry.left_connector_link_pos2)
system.Add(left_thigh_connector_joint)
left_thigh_4bar_joint = make_joint(left_thigh, left_4bar,
geometry.left_4bar_link_pos1)
system.Add(left_thigh_4bar_joint)
left_shin_ankle_joint1 = make_joint(left_shin, left_ankle1,
geometry.left_shin_link_pos2)
system.Add(left_shin_ankle_joint1)
left_4bar_ankle_joint1 = make_joint(left_4bar, left_ankle1,
geometry.left_4bar_link_pos2)
system.Add(left_4bar_ankle_joint1)
left_shin_ankle_joint2 = make_joint(left_shin, left_ankle2,
geometry.left_shin_link_pos2)
system.Add(left_shin_ankle_joint2)
left_4bar_ankle_joint2 = make_joint(left_4bar, left_ankle2,
geometry.left_4bar_link_pos2)
system.Add(left_4bar_ankle_joint2)
left_recip_joint = make_joint(main_body,
left_recip,
geometry.left_recip_link_pos1,
joint_type=chrono.ChLinkMotorRotationTorque)
left_recip_joint.SetTorqueFunction(chrono.ChFunction_Const(10))
system.Add(left_recip_joint)
left_thigh_shin_joint = make_joint(left_thigh, left_shin,
geometry.left_shin_link_pos1)
system.Add(left_thigh_shin_joint)
left_shin_pulley = chrono.ChLinkPulley()
left_shin_pulley.Initialize(left_connector, left_shin,
chrono.ChCoordsysD(chrono.ChVectorD(0, 0, 0)))
system.Add(left_shin_pulley)
right_shin_pulley = chrono.ChLinkPulley()
right_shin_pulley.Initialize(right_connector, right_shin,
chrono.ChCoordsysD(chrono.ChVectorD(0, 0, 0)))
system.Add(right_shin_pulley)
