def __init__(self):
super().__init__()
self.inertia = inertia(1, numpy.diag([1, 1, 1]))
self.impulse_screw = screw()
self.add_shape(
#zencad.sphere(self.r)
model)
self.make_drivers()
def apply_acceleration_to_rigid_bodies(self):
for r in self.rigid_bodies:
r.acceleration = screw(lin=(self.accelerations[r.dynno * 6 + 0],
self.accelerations[r.dynno * 6 + 1],
self.accelerations[r.dynno * 6 + 2]),
ang=(self.accelerations[r.dynno * 6 + 3],
self.accelerations[r.dynno * 6 + 4],
self.accelerations[r.dynno * 6 + 5]))
def _force_evaluation(self, unit):
output_force = screw()
for c in unit.childs:
loc = unit.global_location.inverse() * c.global_location
output_force += self._force_evaluation(c).transform(loc) #inverse?
unit.force_model.output_force = output_force
#unit.force_model.evaluate_local_force()
return unit.force_model.input_force()
def eval_body_control(self, delta, t):
#self.body_control = screw.from_trans(self.body.global_location.inverse() * self.body_target)
z = self.body.global_location.translation().z
zdiff = math.sin(t / 3) * 5 + 7 - z
self.zdiff_integral += zdiff * delta
zsig = zdiff * 1 + self.zdiff_integral * 0.3
self.body_control = screw(lin=(0, 12, zsig), ang=(0, 0, 0.1))
def inertia_force(self):
aspd = self.speed.ang
mass = self.reference_inertia.mass
rho = self.reference_inertia.radius
imat = self.reference_inertia.matrix
ret = screw(lin=-mass * aspd.cross(aspd.cross(rho)),
ang=-aspd.cross(imat * aspd))
return ret
#def inertia_force_in_body_frame(self):
# arm = -self.reference_inertia.radius
# return self.inertia_force().force_carry(arm)
def eval_body_position(self, delta):
#transes = [ self.legs1[i].output.global_location.translation() for i in range(6) ]
#self.lastposes = [ o.global_location for o in self.outs ]
moved_speed = []
moved_speed_arms = []
for i in range(6):
if self.connected[i]:
arm = (self.body.global_location.inverse() *
self.outs[i].global_location).translation()
#last = self.body.global_location.inverse() * self.lastposes[i]
#cur = self.body.global_location.inverse() * self.outs[i].global_location
#speed = screw.from_trans(last.inverse() * cur)
speed = self.ttargets[i] * delta
#speed = speed.lin
speed.z = 0
moved_speed.append(speed)
arm.z = 0
moved_speed_arms.append(arm)
body_linear_speed = vector3(0, 0, 0)
body_rotor_speed = 0
for m in moved_speed:
body_linear_speed -= m
if len(moved_speed) > 0:
body_linear_speed = body_linear_speed * (1 / len(moved_speed))
for m, arm in zip(moved_speed, moved_speed_arms):
diff = (m - body_linear_speed)
a = diff.cross(arm.normalize()).z
l = arm.length()
#print(a)
body_rotor_speed += a / l
#body_rotor_speed = 0
if len(moved_speed) > 0:
body_rotor_speed = body_rotor_speed * (1 / len(moved_speed))
if len(moved_speed) != 0:
body_delta = screw(lin=body_linear_speed,
ang=vector3(0, 0, body_rotor_speed)).to_trans()
self.body.relocate(self.body.global_location * body_delta)
for i in range(6):
self.lastposes[i] = self.outs[i].global_location
def __init__(self): self.body = body() self.legs0 = [] self.legs1 = [] self.body_position_error_integral = screw() self.body_control_target = translate(0,0,10) self.errspd = screw() self.last_corrected_group = -1 self.last_on_ground = False for r in self.body.rots2: p0 = leg0() p1 = leg1() r.output.link(p0) p0.rot.link(p1) self.legs0.append(p0) self.legs1.append(p1) self.rots0 = self.body.rots self.rots1 = self.body.rots2 self.rots2 = [ l.rot for l in self.legs0 ] self.outs = [ l.output for l in self.legs1 ] for r in self.rots1[:3]: r.set_coord(deg(-50)) for r in self.rots1[3:]: r.set_coord(deg(50)) for r in self.rots2[:3]: r.set_coord(deg(-50)) for r in self.rots2[3:]: r.set_coord(deg(50)) self.rots0_servos = [ zencad.bullet.servo_controller3(simulation=simulation, kunit=self.rots0[i]) for i in range(len(self.rots0)) ] self.rots1_servos = [ zencad.bullet.servo_controller3(simulation=simulation, kunit=self.rots1[i]) for i in range(len(self.rots1)) ] self.rots2_servos = [ zencad.bullet.servo_controller3(simulation=simulation, kunit=self.rots2[i]) for i in range(len(self.rots2)) ] self.rots_all = self.rots0_servos + self.rots1_servos + self.rots2_servos #for r in self.rots_all: r.set_koeffs(ki=100, kp=30) self.init_leg_controllers()
def active_forces(self):
NR = len(self.rigid_bodies)
N = NR * 6
S = numpy.zeros((N, 1))
gravs = []
if self.gravity != pyservoce.vector3(0, 0, 0):
for i, r in enumerate(self.rigid_bodies):
arm = -r.reference_inertia.radius
scr = screw(lin=self.gravity, ang=(
0, 0, 0)).force_carry(arm) * r.reference_inertia.mass
gravs.append(scr.npvec_lin_first())
P = np.concatenate(gravs).reshape((N, 1))
return S + P
def body_trajectory_control(self, delta):
#pass
#if time.time() - self.grounded_time > 1.5:
self.body_control_target = self.body_control_target * screw(
lin=(0, 4, 0), ang=(0, 0, 0)).scale(delta).to_trans()
def body_control(self):
K = 1.3
KI = 0
KD = 0 #10
STRESS_TRIGGER = 0
CORRECTION_TIME = 3
target = self.body_control_target
current = self.body.global_location
vel = self.simulation_controller.velocity()
velscr = screw(ang=vel[1], lin=vel[0])
#print(velscr)
error = current.inverse() * target
errsig = screw.from_trans(error)
alpha = 0.1
self.errspd = self.errspd * (1 - alpha) + (-velscr +
self.errspd) * alpha
self.body_position_error_integral += errsig
errsig = errsig * K + self.body_position_error_integral * KI + self.errspd * KD
group1_correction_mode = False
group2_correction_mode = False
for s in self.group1_ctrs:
if s.bcmode == False:
group1_correction_mode = True
for s in self.group2_ctrs:
if s.bcmode == False:
group2_correction_mode = True
if group1_correction_mode is False and group2_correction_mode is False:
if self.last_corrected_group != 1:
self.last_corrected_group = 1
print("activete 1 correction")
for s in self.group1_ctrs:
stress = s.stress()
if stress > STRESS_TRIGGER:
self.group1_activate_correction()
elif self.last_corrected_group != 2:
self.last_corrected_group = 2
print("activete 2 correction")
for s in self.group2_ctrs:
stress = s.stress()
if stress > STRESS_TRIGGER:
self.group2_activate_correction()
if group1_correction_mode is True and time.time(
) - self.corrected_start > CORRECTION_TIME:
print("group1_correction_mode is True")
leave = True
for s in self.group1_ctrs:
if not s.is_grounded():
leave = False
else:
print("GROUNDED")
s.bcmode = True
if leave:
group1_correction_mode = False
if group2_correction_mode is True and time.time(
) - self.corrected_start > CORRECTION_TIME:
print("group2_correction_mode is True")
leave = True
for s in self.group2_ctrs:
if not s.is_grounded():
leave = False
else:
s.bcmode = True
if leave:
group2_correction_mode = False
print(self.last_corrected_group)
for i, s in enumerate(self.leg_controllers):
if s.bcmode:
s.set_body_mirror_control(errsig, koeff=1, z=None, rkoeff=0)
else:
if time.time() - self.corrected_start < CORRECTION_TIME:
s.set_body_mirror_control(errsig, koeff=1, rkoeff=1, z=0)
else:
s.set_body_mirror_control(errsig,
koeff=-1,
rkoeff=0,
z=None)
#!/usr/bin/env python3
from zencad import *
import zencad.malgo
import zencad.libs.kinematic as kinematic
import zencad.libs.rigidity
import zencad.geom.curve3
from zencad.libs.screw import screw
import time
import numpy
gravity_mass = screw(ang=(0, 0, 0), lin=(0, 0, -10))
class finite_element(zencad.assemble.unit):
def __init__(self, h, last, ax):
super().__init__()
#E = 200000 * 10**6
#G = 82 * 10**9
E = 40 * 10**6
G = 3 * 10**7
F = 1
Jx = 1
Jy = 1
Jz = 1
r = 5
def __init__(self, unit):
self.parent = unit
self.output_force = screw()
def __init__(self):
self.body = body()
self.legs0 = []
self.legs1 = []
for r in self.body.rots2:
p0 = leg0()
p1 = leg1()
r.output.link(p0)
p0.rot.link(p1)
self.legs0.append(p0)
self.legs1.append(p1)
self.rots0 = self.body.rots
self.rots1 = self.body.rots2
self.rots2 = [l.rot for l in self.legs0]
self.outs = [l.output for l in self.legs1]
for r in self.rots1[:3]:
r.set_coord(deg(-80))
for r in self.rots1[3:]:
r.set_coord(deg(80))
for r in self.rots2[:3]:
r.set_coord(deg(-80))
for r in self.rots2[3:]:
r.set_coord(deg(80))
self.chains = []
for i in range(6):
self.chains.append(
zencad.libs.kinematic.kinematic_chain(
finallink=self.legs1[i].output))
self.typical_targets = [nulltrans()] * 6
self.typical_targets[0] = translate(-25, 0, 0)
self.typical_targets[1] = translate(-20, -20, 0)
self.typical_targets[2] = translate(-20, 20, 0)
self.typical_targets[3] = translate(20, -20, 0)
self.typical_targets[4] = translate(20, 20, 0)
self.typical_targets[5] = translate(25, 0, 0)
self.speeds = [None] * 6
self.group1 = [0, 3, 4]
self.group2 = [1, 2, 5]
self.lastposes = [o.global_location for o in self.outs]
self.connected = [False] * 6
self.leg_stress = [0] * 6
self.position_stress = [0] * 6
self.leg_stimul = [0] * 6
self.body_control = screw()
self.legs_control = [vector3(0, 0, 0)] * 6
self.body_target = up(10)
self.zdiff_integral = 0
self.ttargets = [vector3(0, 0, 0)] * 6
self.in_reposition = [True] * 6
self.need_reposition = [True] * 6
self.on_ground = [False] * 6
self.rots0_servos = [
zencad.bullet.speed_controller(simulation=simulation,
kunit=self.rots0[i])
for i in range(len(self.rots0))
]
self.rots1_servos = [
zencad.bullet.speed_controller(simulation=simulation,
kunit=self.rots1[i])
for i in range(len(self.rots1))
]
self.rots2_servos = [
zencad.bullet.speed_controller(simulation=simulation,
kunit=self.rots2[i])
for i in range(len(self.rots2))
]
self.rots_all = self.rots0_servos + self.rots1_servos + self.rots2_servos
for r in self.rots_all:
r.set_koeffs(ki=100, kp=30)
self.arms = [
#zencad.libs.kinematic.kinematic_chain(self.outs[i], startlink=self.body) for i in range(6)
zencad.libs.kinematic.kinematic_chain(self.outs[i])
for i in range(6)
]
self.lastgroup = None
def sensivity(self):
return screw(ang=self.marshal, lin=(0, 0, 0))
def get_force_screw(self):
return screw(ang=self.marshal * self.signal, lin=(0, 0, 0))
