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