def __init__(self, driver_board, front_or_back: FrontOrBack,
left_hip_knee_indices: HipKneeIndices,
right_hip_knee_indices: HipKneeIndices, **kwargs):
# Here, pair_index N will correspond to Leg's 2*N and 2*N+1.
# Since a Leg has 2 Servo obj's, and a Pair has 2 Leg obj's,
# it has 4 Servo obj's, and therefore there are only 4 Pair's
# to a driver_board.
self.leg_left = Leg(driver_board,
LeftOrRight.LEFT,
front_or_back=front_or_back,
hip_knee_indices=left_hip_knee_indices,
**kwargs)
self.leg_right = Leg(driver_board,
LeftOrRight.RIGHT,
front_or_back=front_or_back,
hip_knee_indices=right_hip_knee_indices,
**kwargs)
self.legs = [self.leg_left, self.leg_right]
self.phase_offset = 0
self.phase_incr = 0.05 * 2 * pi
self.phase = 0. if front_or_back == FrontOrBack.FRONT else pi
self.leg_left_phase_offset = 0.
self.leg_right_phase_offset = pi / 1.
def raiseLegs(self, Legs, offset=[0, 95]):
steps = 30
pulses = self.calculatePulse(self._Height, self._Length)
startpulseAnkle = Legs[0].getAnkle()
startpulseKnee = Legs[0].getKnee()
for step in range(1, steps, 1 * self._Multiplier):
#raise leg
for Leg in Legs:
Leg.moveAnkle(self.calculateVerticalPulse(startpulseAnkle, pulses[1] - offset[0], step, steps))
Leg.moveKnee(self.calculateVerticalPulse(startpulseKnee, pulses[0] - offset[1], step, steps))
time.sleep(self.SleepTime)
def LowerLegs(self, Legs):
steps = 30
startpulseAnkle = Legs[0].getAnkle() #klopt die array? twee keer 0 voor ankle en knee.
startpulseKnee = Legs[0].getKnee()
pulses = self.calculatePulse(self._Height, self._Length)
for step in range(1, steps, 1 * self._Multiplier):
#lower leg
for Leg in Legs:
Leg.moveAnkle(self.calculateVerticalPulse(startpulseAnkle, pulses[1], step, steps))
Leg.moveKnee(self.calculateVerticalPulse(startpulseKnee, pulses[0], step, steps)) #startpulseKnee in plaats van Ankle
time.sleep(self.SleepTime)
class Drawer:
def __init__(self):
self.root = tk.Tk()
self.root.title("Reverse Kinematic")
self.can = tk.Canvas(self.root,
width=1200,
height=1000,
bg="black",
highlightthickness=0)
self.can.pack()
self.root.bind_all("<Escape>", self.quit)
self.can.bind('<Motion>', self.handle_mouse_motion)
self.leg = Leg()
self.draw_leg()
def run(self):
self.root.mainloop()
def redraw(self):
self.can.delete("all")
self.draw_leg()
def handle_mouse_motion(self, event):
x = event.x
y = event.y
self.leg.head_to(x, y)
self.redraw()
def quit(self, event=None):
self.root.quit()
self.root.destroy()
def draw_leg(self):
colors = ['red', 'blue', 'yellow']
segments = self.leg.get_segments()
for i, segment in enumerate(segments):
self.can.create_line(segment.p1.x,
segment.p1.y,
segment.p2.x,
segment.p2.y,
fil="green",
width=15)
# A little circle to show the base of the segment
self.can.create_oval(segment.p1.x - 10,
segment.p1.y - 10,
segment.p1.x + 10,
segment.p1.y + 10,
fill=colors[i])
def __init__(self, start_addr, stop_addr):
self.angle = 0
self.start_addr = start_addr[0]
self.stop_addr = stop_addr[0]
self.point_zero = 280
self.min = -64
self.max = 64
self.mode = 1
self.modes = []
self.last = 0
self.lleg = Leg('left', start_addr[1], stop_addr[1])
self.rleg = Leg('right', start_addr[2], stop_addr[2])
def __init__(self):
self.root = tk.Tk()
self.root.title("Reverse Kinematic")
self.can = tk.Canvas(self.root,
width=1200,
height=1000,
bg="black",
highlightthickness=0)
self.can.pack()
self.root.bind_all("<Escape>", self.quit)
self.can.bind('<Motion>', self.handle_mouse_motion)
self.leg = Leg()
self.draw_leg()
def step
rightLeg = Leg((1,2,3),(0,0,0))
rightLeg.addMove((5,5,5,1))
rightLeg.addMove((-5,-5,-5,2))
startTime = time.time()
while (rightLeg.pendingMoves()):
rightLeg.callback(time.time() - startTime)
time.sleep(0.05)
def MoveLegsBackward(self, Legs, Raised, Turn, offset=[90,90]):
if Turn[0] < Turn[1]:
self.MoveLegsForward(Legs, Raised, Turn)
return
startpoint = Turn[0]
steps = Turn[1]
pulses = self.calculatePulse(self._Height, self._Length)
if Raised:
pulses[0] -= offset[0]
pulses[1] -= offset[1]
for step in range(startpoint, steps, -1 * self._Multiplier):
for Leg in Legs:
Leg.moveHip(self.calculateHipPulse(step))
time.sleep(self.SleepTime)
class LegPair:
def __init__(self, driver_board, front_or_back: FrontOrBack,
left_hip_knee_indices: HipKneeIndices,
right_hip_knee_indices: HipKneeIndices, **kwargs):
# Here, pair_index N will correspond to Leg's 2*N and 2*N+1.
# Since a Leg has 2 Servo obj's, and a Pair has 2 Leg obj's,
# it has 4 Servo obj's, and therefore there are only 4 Pair's
# to a driver_board.
self.leg_left = Leg(driver_board,
LeftOrRight.LEFT,
front_or_back=front_or_back,
hip_knee_indices=left_hip_knee_indices,
**kwargs)
self.leg_right = Leg(driver_board,
LeftOrRight.RIGHT,
front_or_back=front_or_back,
hip_knee_indices=right_hip_knee_indices,
**kwargs)
self.legs = [self.leg_left, self.leg_right]
self.phase_offset = 0
self.phase_incr = 0.05 * 2 * pi
self.phase = 0. if front_or_back == FrontOrBack.FRONT else pi
self.leg_left_phase_offset = 0.
self.leg_right_phase_offset = pi / 1.
def increment_phase_and_move(self):
self.phase = (self.phase + self.phase_incr) % (2 * pi)
self.move_to_phase(self.phase)
# sleep(self.incr_pause)
def move_to_phase(self, phase):
self.phase = phase
self.leg_left.move_to_phase(self.phase + self.leg_left_phase_offset)
self.leg_right.move_to_phase(self.phase + self.leg_right_phase_offset)
def reset_pair(self):
for l in self.legs:
l.reset_leg()
def test_fk_ik():
length = {
'coxaLength': 26,
'femurLength': 42,
'tibiaLength': 63
}
channels = [0, 1, 2]
leg = Leg(length, channels)
angles = [0, -70, -90]
pts = leg.fk(*angles)
angles2 = leg.ik(*pts)
pts2 = leg.fk(*angles2)
# angles2 = [r2d(a), r2d(b), r2d(c)]
print('angles (orig):', angles)
print('pts from fk(orig): {:.2f} {:.2f} {:.2f}'.format(*pts))
print('angles2 from ik(pts): {:.2f} {:.2f} {:.2f}'.format(*angles2))
print('pts2 from fk(angle2): {:.2f} {:.2f} {:.2f}'.format(*pts2))
# print('diff:', np.linalg.norm(np.array(angles) - np.array(angles2)))
print('diff [mm]: {:.2f}'.format(np.linalg.norm(pts - pts2)))
time.sleep(1)
def MoveLegsForward(self, Legs, Raised, Turn, offset=[0,95]):
if Turn[0] > Turn[1]:
self.MoveLegsBackward(Legs, Raised, Turn)
return
startpoint = Turn[0]
steps = Turn[1]
pulses = self.calculatePulse(self._Height, self._Length)
if Raised:
pulses[0] -= offset[0]
pulses[1] -= offset[1]
for step in range(startpoint, steps, 1 * self._Multiplier):
#move leg forward
#pulses = self._MInterface.calculatePulse(self._MInterface._Height, self._MInterface.calculateLengthLeg(self._MInterface._Length, step))
#if Raised:
#pulses[0] -= 90
#pulses[1] -= 90
for Leg in Legs:
Leg.moveHip(self.calculateHipPulse(step))
time.sleep(self.SleepTime)
def Calibreren(self, number, pulse):
for Leg in self._Legs:
if number == 1:
Leg.moveHip(pulse)
if number == 2:
Leg.moveAnkle(pulse)
if number == 3:
Leg.moveKnee(pulse)
def run():
# sport = '/dev/serial0'
sport = None
if sport:
ser = ServoSerial(sport)
else:
ser = DummySerial('test')
Servo.ser = ser # set static servo serial comm
leg = Leg([1, 2, 3])
foot0 = leg.foot0
print('foot0', foot0)
angles = [0, 0, 0]
pts = leg.fk(*angles) # allows angles out of range
print('pts', pts)
leg.moveFoot(*pts)
sit = Pose(leg.foot0)
sit.run()
time.sleep(2)
print('Bye')
def run():
# sport = '/dev/serial0'
sport = None
if sport:
ser = ServoSerial(sport)
else:
ser = DummySerial('test')
Servo.ser = ser # set static servo serial comm
leg = Leg([1, 2, 3])
foot0 = leg.foot0
print('foot0', foot0)
angles = [0, 0, 0]
pts = leg.fk(*angles) # allows angles out of range
print('pts', pts)
leg.moveFoot(*pts)
sit = Pose(leg.foot0)
sit.run()
time.sleep(2)
print('Bye')
def __init__(self):
self.legs = [Leg() for i in range(6)]
for i in range(3):
self.legs[i].setSide(Leg.RIGHT)
self.legs[i + 3].setSide(Leg.LEFT)
for i in range(6):
self.legs[i].setPort(i * 4)
for i in [0, 2, 4]:
self.legs[i].setPhase(0)
for i in [1, 3, 5]:
self.legs[i].setPhase(1)
def add_leg(self, legData, constants):
leg_name = legData["name"]
body_pin = legData["motors"]["body"]["pinValue"]
body_offset = legData["motors"]["body"]["offset"]
body_center = constants["bodyCenterValue"] + body_offset
body_min = constants["bodyRange"]["min"]
body_max = constants["bodyRange"]["max"]
mid_horiz_value = legData["motors"]["middle"]["horizValue"]
middle_pin = legData["motors"]["middle"]["pinValue"]
middle_min = constants["middleRange"]["min"]
middle_max = constants["middleRange"]["max"]
middle_offset_to_center = constants["midOffsetFromHoriz"]
leg_horiz_value = legData["motors"]["leg"]["horizValue"]
leg_pin = legData["motors"]["leg"]["pinValue"]
leg_min = constants["legRange"]["min"]
leg_max = constants["legRange"]["max"]
leg_offset_to_center = constants["legOffsetFromHoriz"]
leg = Leg(self.pwm, leg_name, body_pin, body_min, body_max,
body_center, mid_horiz_value, middle_pin, middle_min,
middle_max, middle_offset_to_center, leg_horiz_value,
leg_pin, leg_min, leg_max, leg_offset_to_center)
if leg_name == "FR":
self.front_right = leg
elif leg_name == "FL":
self.front_left = leg
elif leg_name == "BL":
self.back_left = leg
elif leg_name == "BR":
self.back_right = leg
else:
print "ERROR: LEG CANNOT BE IDENTIFIED"
def test_full_fk_ik(c=[0, 1, 2]):
length = {
'coxaLength': 26,
'femurLength': 42,
'tibiaLength': 63
}
channels = c
leg = Leg(length, channels)
servorange = [[-90, 90], [-90, 90], [-180, 0]]
for s in range(0, 3):
leg.servos[s].setServoRangeAngle(*servorange[s])
for i in range(1, 3):
for a in range(-70, 70, 10):
angles = [0, 0, -10]
if i == 2: a -= 90
angles[i] = a
pts = leg.fk(*angles)
angles2 = leg.ik(*pts)
pts2 = leg.fk(*angles2)
angle_error = np.linalg.norm(np.array(angles) - np.array(angles2))
pos_error = np.linalg.norm(pts - pts2)
# print(angle_error, pos_error)
if angle_error > 0.0001:
print('Angle Error')
printError(pts, pts2, angles, angles2)
exit()
elif pos_error > 0.0001:
print('Position Error')
printError(pts, pts2, angles, angles2)
exit()
else:
print('Good: {} {} {} error(deg,mm): {} {}'.format(angles[0], angles[1], angles[2], angle_error, pos_error))
leg.move(*pts)
time.sleep(0.1)
Servo.all_stop()
import numpy
from Leg import Leg
class Sample(object):
pass
__doc__ = """This script computes the forward kinematics for a table with three three vertical legs."""
# define the degrees of freedom of the legs
# --> leg 1 can only move in z direction
#L1.dof = 'z';
leg1 = Leg("z")
# --> leg 2 can move in x and z direction
#L2.dof = 'xz';
leg2 = Leg("xz")
# --> leg 3 can move in x, y and z direction
#L3.dof = 'xyz';
leg3 = Leg("xyz");
# define the position of the three legs in the world coordinates
# --> position of leg 1 (=> L1) in the world frame (w)
leg1.w = numpy.asarray([0.1, -0.2, 0], dtype=numpy.float64)
# --> position of leg 2 (=> L2) in the world frame (w)
leg2.w = numpy.asarray([-0.1, -0.1, 0], dtype=numpy.float64)
# --> position of leg 3 (=> L3) in the world frame (w)
leg3.w = numpy.asarray([0, 0.2, 0], dtype=numpy.float64)
class Body(BodyPart):
def __init__(self, bodyCentre):
self.bodyCentreVec = bodyCentre
self.bodyHeight = self.bodyCentreVec.y
self.worldTransform = Transformation()
self.angleX = 0.0
self.angleXOrig = vector(0, 0, 0)
self.angleY = 0.0
self.angleYOrig = vector(0, 0, 0)
self.angleZ = 0.0
self.angleZOrig = vector(0, 0, 0)
self.URLCoxaAngle = 0.0
self.URLUpperLegAngle = 0.0
self.URLLowerLegAngle = 0.0
self.ULLCoxaAngle = 0.0
self.ULLUpperLegAngle = 0.0
self.ULLLowerLegAngle = 0.0
self.MRLCoxaAngle = 0.0
self.MRLUpperLegAngle = 0.0
self.MRLLowerLegAngle = 0.0
#self.bodyCentreVis = sphere(pos = self.bodyCentreVec, radius = JOINT_RADIUS, color = JOINT_COLOR)
##draw body
self.bodyLE = cylinder(pos=self.bodyCentreVec -
vector(BODY_LENGTH / 2, 0, BODY_WIDTH / 2),
length=BODY_LENGTH,
radius=BODY_RADIUS,
axis=vector(1, 0, 0),
color=color.white)
self.bodyRI = cylinder(pos=self.bodyCentreVec -
vector(BODY_LENGTH / 2, 0, -BODY_WIDTH / 2),
length=BODY_LENGTH,
radius=BODY_RADIUS,
axis=vector(1, 0, 0),
color=color.white)
self.bodyUP = cylinder(pos=self.bodyCentreVec -
vector(BODY_LENGTH / 2, 0, BODY_WIDTH / 2),
length=BODY_WIDTH,
radius=BODY_RADIUS,
axis=vector(0, 0, 1),
color=color.white)
self.bodyLO = cylinder(pos=self.bodyCentreVec -
vector(-(BODY_LENGTH / 2), 0, BODY_WIDTH / 2),
length=BODY_WIDTH,
radius=BODY_RADIUS,
axis=vector(0, 0, 1),
color=color.white)
self.worldTransform.AddElement(self.bodyLE)
self.worldTransform.AddElement(self.bodyRI)
self.worldTransform.AddElement(self.bodyUP)
self.worldTransform.AddElement(self.bodyLO)
self.upperRightLeg = Leg(
self, self.bodyCentreVec + vector(
(BODY_LENGTH / 2) - 2, 0, BODY_WIDTH / 2), False)
self.worldTransform.AddElement(self.upperRightLeg)
self.middleRightLeg = Leg(
self,
self.upperRightLeg.GetPosition() - vector(LEG_SPACING, 0, 0),
False)
self.worldTransform.AddElement(self.middleRightLeg)
self.lowerRightLeg = Leg(
self,
self.middleRightLeg.GetPosition() - vector(LEG_SPACING, 0, 0),
False)
self.worldTransform.AddElement(self.lowerRightLeg)
self.upperLeftLeg = Leg(
self, self.bodyCentreVec + vector(
(BODY_LENGTH / 2) - 2, 0, -(BODY_WIDTH / 2)), True)
self.upperLeftLeg.RotateY(180, self.upperLeftLeg.GetPosition())
self.worldTransform.AddElement(self.upperLeftLeg)
self.middleLeftLeg = Leg(
self,
self.upperLeftLeg.GetPosition() - vector(LEG_SPACING, 0, 0), True)
self.middleLeftLeg.RotateY(180, self.middleLeftLeg.GetPosition())
self.worldTransform.AddElement(self.middleLeftLeg)
self.lowerLeftLeg = Leg(
self,
self.middleLeftLeg.GetPosition() - vector(LEG_SPACING, 0, 0), True)
self.lowerLeftLeg.RotateY(180, self.lowerLeftLeg.GetPosition())
self.worldTransform.AddElement(self.lowerLeftLeg)
def SetBodyHeight(self, height):
self.bodyHeight = height
def GetBodyHeight(self):
return self.bodyHeight
def RotateX(self, angle, orig):
self.angleX = angle
self.angleXOrig = orig
def RotateY(self, angle, orig):
self.angleY = angle
self.angleYOrig = orig
def RotateZ(self, angle, orig):
self.angleZ = angle
self.angleZOrig = orig
def SetURLCoxaAngle(self, angle):
self.URLCoxaAngle = angle
def SetURLUpperLegAngle(self, angle):
self.URLUpperLegAngle = angle
def SetURLLowerLegAngle(self, angle):
self.URLLowerLegAngle = angle
def SetULLCoxaAngle(self, angle):
self.ULLCoxaAngle = angle
def SetULLUpperLegAngle(self, angle):
self.ULLUpperLegAngle = angle
def SetULLLowerLegAngle(self, angle):
self.ULLLowerLegAngle = angle
def SetMRLCoxaAngle(self, angle):
self.MRLCoxaAngle = angle
def SetMRLUpperLegAngle(self, angle):
self.MRLUpperLegAngle = angle
def SetMRLLowerLegAngle(self, angle):
self.MRLLowerLegAngle = angle
def ApplyTransformations(self):
self.lowerLeftLeg.ResetLowerLegAngle()
self.lowerLeftLeg.ResetUpperLegAngle()
self.lowerLeftLeg.ResetCoxaAngle()
self.lowerRightLeg.ResetLowerLegAngle()
self.lowerRightLeg.ResetUpperLegAngle()
self.lowerRightLeg.ResetCoxaAngle()
self.middleLeftLeg.ResetLowerLegAngle()
self.middleLeftLeg.ResetUpperLegAngle()
self.middleLeftLeg.ResetCoxaAngle()
self.middleRightLeg.ResetLowerLegAngle()
self.middleRightLeg.ResetUpperLegAngle()
self.middleRightLeg.ResetCoxaAngle()
self.upperLeftLeg.ResetLowerLegAngle()
self.upperLeftLeg.ResetUpperLegAngle()
self.upperLeftLeg.ResetCoxaAngle()
self.upperRightLeg.ResetLowerLegAngle()
self.upperRightLeg.ResetUpperLegAngle()
self.upperRightLeg.ResetCoxaAngle()
self.worldTransform.ResetTrans()
self.worldTransform.RotateX(self.angleX, self.angleXOrig)
self.worldTransform.RotateY(self.angleY, self.angleYOrig)
self.worldTransform.RotateZ(self.angleZ, self.angleZOrig)
self.upperRightLeg.moveToTargetPos()
self.middleRightLeg.moveToTargetPos()
self.lowerRightLeg.moveToTargetPos()
self.upperLeftLeg.moveToTargetPos()
self.middleLeftLeg.moveToTargetPos()
self.lowerLeftLeg.moveToTargetPos()
from Arm import Arm
from ForeArm import ForeArm
from UpLeg import UpLeg
from Wrist import Wrist
from Spine import Spine
#-------------------ESTRUCTURAS DE DATOS Y OBJETOS-------------------------
#Creación de la jerarquía de Bones del MoCap
#cada objeto se inicializa con un ID (nombre identificador) y las coordenadas
#dentro de la sección MOTION
#miembros inferiores
LeftUpLeg = UpLeg('Izquierda', 132, 133, 134)
RightUpLeg = UpLeg('Derecha', 144, 145, 146)
LeftLeg = Leg('Izquierda', 135, 136, 137)
RightLeg = Leg('Derecha', 147, 148, 149)
Leftfoot = Foot('Izquierdo', 138, 139, 140)
Rightfoot = Foot('Derecho', 150, 151, 152)
#tronco superior
LeftArm = Arm('Izquierdo', 21, 22, 23)
RightArm = Arm('Derecho', 78, 79, 80)
LeftForeArm = ForeArm('Izquierdo', 24, 25, 26)
RightForeArm = ForeArm('Derecho', 81, 82, 83)
LeftHand = Wrist('Izquierda', 27, 28, 29)
RightHand = Wrist('Derecha', 84, 85, 86)
Spine1 = Spine('lumbar-toracica', 9, 10, 11)
Neck = Spine('Cervical', 12, 13, 14)
__author__ = 'Ivar'
from Leg import Leg
import time
Leg1 = Leg(["0x40", "0x40", "0x40"], [0, 1, 3], [375, 375, 375])
while (True):
# Change speed of continuous servo on channel O
#angle = int(raw_input("Angle 150 to 600: "))
#Leg1.moveHip(angle)
Leg1.moveKnee(475)
Leg1.moveAnkle(475)
time.sleep(1)
Leg1.moveHip(475)
time.sleep(1)
Leg1.moveKnee(275)
Leg1.moveAnkle(275)
time.sleep(1)
Leg1.moveHip(275)
time.sleep(1)
pwm.setDuty(ch, fduty)
else:
print "femur nan!"
if not isnan(tibia):
tduty = a / 180 * tibia + b
pwm.setDuty(ch + 1, tduty)
else:
print "tibia nan!"
setup()
zeroBot()
standheight = 3.5
frLeg = Leg(side=1, zeroz=-.0254 * standheight, zerox=0)
flLeg = Leg(side=2, zeroz=-.0254 * standheight, zerox=0)
lrLeg = Leg(side=2, zeroz=-.0254 * (standheight + .2), zerox=0)
rrLeg = Leg(side=1, zeroz=-.0254 * standheight, zerox=0)
# frLeg = Leg(side=1)
# flLeg = Leg(side=2)
# lrLeg = Leg(side=2)
# rrLeg = Leg(side=1)
walker = Walk(stride_length=0.0, stride_height=0.00)
p = 0
while True:
setHips(90)
def __init__(self, bodyCentre):
self.bodyCentreVec = bodyCentre
self.bodyHeight = self.bodyCentreVec.y
self.worldTransform = Transformation()
self.angleX = 0.0
self.angleXOrig = vector(0, 0, 0)
self.angleY = 0.0
self.angleYOrig = vector(0, 0, 0)
self.angleZ = 0.0
self.angleZOrig = vector(0, 0, 0)
self.URLCoxaAngle = 0.0
self.URLUpperLegAngle = 0.0
self.URLLowerLegAngle = 0.0
self.ULLCoxaAngle = 0.0
self.ULLUpperLegAngle = 0.0
self.ULLLowerLegAngle = 0.0
self.MRLCoxaAngle = 0.0
self.MRLUpperLegAngle = 0.0
self.MRLLowerLegAngle = 0.0
#self.bodyCentreVis = sphere(pos = self.bodyCentreVec, radius = JOINT_RADIUS, color = JOINT_COLOR)
##draw body
self.bodyLE = cylinder(pos=self.bodyCentreVec -
vector(BODY_LENGTH / 2, 0, BODY_WIDTH / 2),
length=BODY_LENGTH,
radius=BODY_RADIUS,
axis=vector(1, 0, 0),
color=color.white)
self.bodyRI = cylinder(pos=self.bodyCentreVec -
vector(BODY_LENGTH / 2, 0, -BODY_WIDTH / 2),
length=BODY_LENGTH,
radius=BODY_RADIUS,
axis=vector(1, 0, 0),
color=color.white)
self.bodyUP = cylinder(pos=self.bodyCentreVec -
vector(BODY_LENGTH / 2, 0, BODY_WIDTH / 2),
length=BODY_WIDTH,
radius=BODY_RADIUS,
axis=vector(0, 0, 1),
color=color.white)
self.bodyLO = cylinder(pos=self.bodyCentreVec -
vector(-(BODY_LENGTH / 2), 0, BODY_WIDTH / 2),
length=BODY_WIDTH,
radius=BODY_RADIUS,
axis=vector(0, 0, 1),
color=color.white)
self.worldTransform.AddElement(self.bodyLE)
self.worldTransform.AddElement(self.bodyRI)
self.worldTransform.AddElement(self.bodyUP)
self.worldTransform.AddElement(self.bodyLO)
self.upperRightLeg = Leg(
self, self.bodyCentreVec + vector(
(BODY_LENGTH / 2) - 2, 0, BODY_WIDTH / 2), False)
self.worldTransform.AddElement(self.upperRightLeg)
self.middleRightLeg = Leg(
self,
self.upperRightLeg.GetPosition() - vector(LEG_SPACING, 0, 0),
False)
self.worldTransform.AddElement(self.middleRightLeg)
self.lowerRightLeg = Leg(
self,
self.middleRightLeg.GetPosition() - vector(LEG_SPACING, 0, 0),
False)
self.worldTransform.AddElement(self.lowerRightLeg)
self.upperLeftLeg = Leg(
self, self.bodyCentreVec + vector(
(BODY_LENGTH / 2) - 2, 0, -(BODY_WIDTH / 2)), True)
self.upperLeftLeg.RotateY(180, self.upperLeftLeg.GetPosition())
self.worldTransform.AddElement(self.upperLeftLeg)
self.middleLeftLeg = Leg(
self,
self.upperLeftLeg.GetPosition() - vector(LEG_SPACING, 0, 0), True)
self.middleLeftLeg.RotateY(180, self.middleLeftLeg.GetPosition())
self.worldTransform.AddElement(self.middleLeftLeg)
self.lowerLeftLeg = Leg(
self,
self.middleLeftLeg.GetPosition() - vector(LEG_SPACING, 0, 0), True)
self.lowerLeftLeg.RotateY(180, self.lowerLeftLeg.GetPosition())
self.worldTransform.AddElement(self.lowerLeftLeg)
pwm.setDuty(ch + 1, tduty)
pwm.setDuty(hipch, hduty)
def setLeg(femur, tibia, ch):
fduty = a / 180 * femur + b
tduty = a / 180 * tibia + b
pwm.setDuty(ch, fduty)
pwm.setDuty(ch + 1, tduty)
setup()
zeroBot()
frLeg = Leg(side=1)
flLeg = Leg(side=2)
lrLeg = Leg(side=2)
rrLeg = Leg(side=1)
walker = LegPattern()
p = 0
while True:
frLeg = Leg3d(side=1)
fr = walker.getPos(p)
frfem, frtib, frhip = frLeg.servoAngles(fr[0], fr[1], fr[2])
# print frfem,frtib,frhip
def legs(self):
# Take a list of legs, iterate of over it and output methods
for i in range(len(self.leglist)):
self.llegs.append(
Leg(self.leglist[i], self.leglist[i + 1], self.winddir))
import numpy
from Leg import Leg
class Sample(object):
pass
__doc__ = """This script computes the forward kinematics for a table with three three vertical legs."""
# define the degrees of freedom of the legs
# --> leg 1 can only move in z direction
#L1.dof = 'z';
leg1 = Leg("z")
# --> leg 2 can move in x and z direction
#L2.dof = 'xz';
leg2 = Leg("xz")
# --> leg 3 can move in x, y and z direction
#L3.dof = 'xyz';
leg3 = Leg("xyz")
# define the position of the three legs in the world coordinates
# --> position of leg 1 (=> L1) in the world frame (w)
leg1.w = numpy.asarray([0.1, -0.2, 0], dtype=numpy.float64)
# --> position of leg 2 (=> L2) in the world frame (w)
leg2.w = numpy.asarray([-0.1, -0.1, 0], dtype=numpy.float64)
# --> position of leg 3 (=> L3) in the world frame (w)
#now walk state stuff.
state = 1
fPWM = 50
i2c_address = 0x40 # (standard) adapt to your module
channel = 0 # adapt to your wiring
a = 8.5 # adapt to your servo
b = 2 # adapt to your servo
setup()
zeroBot()
standheight = 3.5
frLeg = Leg(side=1)
flLeg = Leg(side=2)
lrLeg = Leg(side=2)
rrLeg = Leg(side=1)
# frLeg = Leg(side=1)
# flLeg = Leg(side=2)
# lrLeg = Leg(side=2)
# rrLeg = Leg(side=1)
walker = Walk(stride_length=0.02, stride_height=-0.01)
p = 0
while key != ord('q'):
setHips(90)
key = stdscr.getch()
pwm.setDuty(ch, fduty)
else:
print "femur nan!"
if not isnan(tibia):
tduty = a / 180 * tibia + b
pwm.setDuty(ch + 1, tduty)
else:
print "tibia nan!"
setup()
zeroBot()
standheight = 3.5
frLeg = Leg(side=1)
flLeg = Leg(side=2)
lrLeg = Leg(side=2)
rrLeg = Leg(side=1)
# frLeg = Leg(side=1)
# flLeg = Leg(side=2)
# lrLeg = Leg(side=2)
# rrLeg = Leg(side=1)
walker = Walk(stride_length=0.02, stride_height=-0.01)
p = 0
while True:
setHips(90)
duty = a / 180 * angle + b
for ch in range(8,12):
pwm.setDuty(ch,duty)
def setLeg(femur,tibia,ch):
fduty = a / 180 * femur + b
tduty = a/180*tibia+b
pwm.setDuty(ch,fduty)
pwm.setDuty(ch+1,tduty)
setup()
zeroBot()
frLeg = Leg(side=1)
flLeg = Leg(side=2)
lrLeg = Leg(side=2)
rrLeg = Leg(side=1)
walker = Walk(stride_height=-0.01,stride_length =.02)
p = 0
while True:
setHips(90)
#if keyboard.is_pressed('1'):
# state=1
# print("standing")
#if keyboard.is_pressed('2'):
r = Mark('R', 'Meudon Hotel', 50.11, -5.07)
s = Mark('S', 'Sunbeam', 50.166, -5.045)
sn = Mark('Sn', 'Sailtech', 50.155, -5.03)
t = Mark('T', 'Falmouth Boat Construction', 50.161, -5.05)
w = Mark('W', 'Rustler Yachts', 50.16, -5.032)
wt = Mark('Wt', 'Ancasta', 50.169, -5.032)
z = Mark('Z', 'Penrose Sails', 50.136, -5.018)
br = Mark('Br', 'Black Rock', 50.145, -5.029)
ca = Mark('Ca', 'Castle', 50.15, -5.027)
sm = Mark('Sm', 'St Mawes', 50.152, -5.024)
g = Mark('G', 'The Governor', 50.153, -5.04)
wn = Mark('Wn', 'West Narrows', 50.157, -5.035)
en = Mark('En', 'East Narrows', 50.157, -5.032)
v = Mark('V', 'The Vilt', 50.166, -5.038)
nb = Mark('Nb', 'Northbank', 50.172, -5.038)
j = Mark('J', 'St Just', 50.174, -5.029)
me = Mark('Me', 'Messack', 50.188, -5.037)
ck = Mark('Ck', 'Carrick', 50.193, -5.046)
pill = Mark('Pill', 'Pill', 50.201, -5.04)
re = Mark('Re', 'Restronguet', 50.197, -5.047)
tu = Mark('Tu', 'Turnaware', 50.195, -5.032)
gr = Mark('Gr', 'Greatwood', 50.186, -5.044)
jb = Mark('Jb', 'JB', 50.187, -5.032)
we = Mark('We', 'Windward', 50.182, -5.042)
outlst = list()
leglist = [Leg(j, jb, 360), Leg(jb, gr, 360), Leg(gr, j, 360)]
for leg in leglist:
#outlst.append((round(leg.distance(), 2), leg.twatest()))
print(leg.angle)
print(outlst)
from control_msgs.msg import JointControllerState
from Leg import Leg
class LegController:
def __init__(self, leg):
rospy.init_node('leg_controller', anonymous=True)
self.leg = leg
self.time_previous_step = np.array([rospy.get_time()], dtype='float64')
# publish the joint
self.c1_pub = rospy.Publisher('/phantomx/j_c1_' + self.name +
'_position_controller/command',
Float64,
queue_size=10)
self.thigh_pub = rospy.Publisher('/phantomx/j_thigh_' + self.name +
'_position_controller/command',
Float64,
queue_size=10)
self.tibia_pub = rospy.Publisher('/phantomx/j_tibia_' + self.name +
'_position_controller/command',
Float64,
queue_size=10)
def move_to(self, target_position):
pass
if __name__ == '__main__':
rm_LC = LegController(leg=Leg('rm'))
class PairOfLegs():
def __init__(self, start_addr, stop_addr):
self.angle = 0
self.start_addr = start_addr[0]
self.stop_addr = stop_addr[0]
self.point_zero = 280
self.min = -64
self.max = 64
self.mode = 1
self.modes = []
self.last = 0
self.lleg = Leg('left', start_addr[1], stop_addr[1])
self.rleg = Leg('right', start_addr[2], stop_addr[2])
def calibrate(self):
# if(self.lleg.angle <= abs(self.angle)):
# self.lleg.lock = False
# if(self.rleg.angle <= abs(self.angle)):
# self.rleg.lock = False
while ((abs(self.angle) > self.lleg.angle and self.lleg.angle > 0)
or (abs(self.angle) > self.rleg.angle and self.rleg.angle > 0)):
if (self.angle != 0):
self.angle = self.angle + (0 - self.angle) / abs(self.angle)
move_angle(self.point_zero, self.angle, self.stop_addr)
time.sleep(.005)
# if(self.lleg.angle + self.rleg.angle == 0):
# self.rleg.lock = True
# self.lleg.lock = True
while (self.angle != 0 or self.lleg.angle != 0
or self.rleg.angle != 0):
#while((self.angle > self.lleg.angle and self.lleg.angle > 0) or (self.angle > self.rleg.angle and self.rleg.angle > 0)):
if (self.angle != 0):
self.angle = self.angle + (0 - self.angle) / abs(self.angle)
if (self.lleg.angle + self.rleg.angle == 0):
self.rleg.lock = True
self.lleg.lock = True
if (self.lleg.angle != 0 and self.lleg.lock):
self.lleg.calibrate()
if (self.rleg.angle != 0 and self.rleg.lock):
self.rleg.calibrate()
time.sleep(.005)
#move_angle(self.addr)
#print('p: {}, l: {}, r: {}'.format(self.angle, self.lleg.angle, self.rleg.angle))
self.last = 0
def move(self, mode, way):
if (self.lleg.lock):
self.lleg.move(self.last, mode)
if (self.rleg.lock):
self.rleg.move(self.last, mode)
self.angle = self.angle + 2 * self.modes[self.last]
move_angle(self.point_zero, way * self.angle, self.stop_addr)
if (self.angle >= self.max):
self.last = 1
elif (self.angle <= self.min):
self.last = 3
elif (self.angle == 0 and self.last == 1):
self.last = 2
elif (self.angle == 0 and self.last == 3):
self.last = 4
elif (abs(self.angle) == int(self.max / 2) and self.last == 0):
self.last = 5
if (self.last == 2 or self.last == 4 or self.last == 5):
if (self.lleg.angle + self.rleg.angle == 0):
self.rleg.lock = True
self.lleg.lock = True
def move_up(self, mode):
self.lleg.move(self.last, self.lleg.mode)
move_angle(self.lleg.point_zero, self.lleg.angle, self.lleg.stop_addr)
self.rleg.move(self.last, self.rleg.mode)
move_angle(self.rleg.point_zero, self.rleg.angle, self.rleg.stop_addr)
self.angle = self.angle + self.modes[self.last]
move_angle(self.point_zero, self.angle, self.stop_addr)
time.sleep(.3)
if (abs(self.angle) == 22 and self.last == 0):
self.last = 5