mithi

• Modifications
• The Many cases of Stability

mithi

from __future__ import print_function  # Make print work with python 2 & 3
print('Please wait while modules load...')
import time
import pigpio
import smbus
import subprocess

#Setup I2C
try:
    bus = smbus.SMBus(1)

mithi

if TEST == False:
  import pigpio
  import smbus
  import subprocess
  try:
    bus = smbus.SMBus(1)
    address = 0x48
  except FileNotFoundError:
    print('')
    print('')

mithi

if TEST == False:
  import pigpio
  import smbus
  import subprocess
  try:
    bus = smbus.SMBus(1)
    address = 0x48
  except FileNotFoundError:
    print('')
    print('')

mithi

#----------------------------
# CONSTANTS FOR CALIBRATION
#----------------------------
TEST = False

INCREMENTS = 20
WALK_STEPS = 5
DELAY = 0.25

TWIST_HIP = 30

mithi

from .models import Point
import numpy as np 

def cross(a, b):
  x = a.y * b.z - a.z * b.y
  y = a.z * b.x - a.x * b.z
  z = a.x * b.y - a.y * b.x

  return Point(x, y, z)

mithi

https://math.stackexchange.com/questions/40164/how-do-you-rotate-a-vector-by-a-unit-quaternion
https://www.3dgep.com/understanding-quaternions/
https://math.stackexchange.com/questions/180418/calculate-rotation-matrix-to-align-vector-a-to-vector-b-in-3d/897677#897677
https://stackoverflow.com/questions/1171849/finding-quaternion-representing-the-rotation-from-one-vector-to-another
https://stackoverflow.com/questions/14812005/rotate-plane-so-its-normal-is-the-same-direction-as-another-planes-normal
https://answers.unity.com/questions/1221564/rotate-plane-by-normal-how-to-calculate-new-positi.html
https://stackoverflow.com/questions/1023948/rotate-normal-vector-onto-axis-plane
https://stackoverflow.com/questions/57694061/how-can-i-use-the-rotation-angle-and-axis-to-rotate-a-3d-plane

Rotate plane by normal, how to calculate new position?

mithi

import matplotlib.pyplot as plt
from mpl_toolkits import mplot3d
from mpl_toolkits.mplot3d.art3d import Poly3DCollection
import numpy as np

class Point:
  def __init__(self, x, y, z):
    self.x = x
    self.y = y
    self.z = z

mithi

import matplotlib.pyplot as plt
from mpl_toolkits import mplot3d
from mpl_toolkits.mplot3d.art3d import Poly3DCollection
import numpy as np

class Point:
  def __init__(self, x, y, z):
    self.x = x
    self.y = y
    self.z = z

mithi

import matplotlib.pyplot as plt
from mpl_toolkits import mplot3d
from mpl_toolkits.mplot3d.art3d import Poly3DCollection
import numpy as np

FRONT_LENGTH = 20
SIDE_LENGTH= 30
MID_LENGTH = 40
HIP_LENGTH =10
KNEE_LENGTH = 40