def __init__(self, *args):
if len(args) == 1:
if type(args[0]) is not HexapodLeg:
raise TypeError(
"Copy constructor expects a single HexapodLeg argument.")
o = args[0]
self.id = o.id
self.body = o.body
self.direction = o.direction
self.ref = o.ref
self.world_ref = o.world_ref
self.segments = []
prevSeg = self
for seg in o.segments:
nextSeg = seg.clone(prevSeg)
self.segments.append(nextSeg)
prevSeg = nextSeg
elif len(args) == 4:
config, body, world_ref, legid = args
self.id = legid
self.body = body
disp = config.getLegDisplacement(self)
self.ref = ReferenceFrame(disp, Quaternion.from_z_rotation(0.),
body.ref)
self.world_ref = ReferenceFrame(disp,
Quaternion.from_z_rotation(0.),
world_ref)
self.segments = body.getLegSegments(config, self)
self.direction = body.getLegDirection(self)
else:
raise TypeError("Unexpected")
def __init__(self, config):
# Prepare the root reference frame.
self.ref = ReferenceFrame(Vector3([0., 0., 0.]),
Quaternion.from_z_rotation(0.))
self.body = HexapodBody(self.ref, config)
self.legs = [
HexapodLeg(config, self.body, self.ref, legid)
for legid in config.getLegs()
]
self.mp = HexapodMotionPlanner(config, self.body, self.legs)
self.stepmp = HexapodStepMotionPlanner(config, self, self.mp,
self.legs,
config.getLegPhases()[2])
self.stepmp.start()
delta = target.dist(ee)
if (delta | delta) < self.options["delta_sq"]:
# We have converged!
# Return the translations to reach the target.
return [s.getRotation() for s in leg.getSegments()]
update = (jacob_inv * np.array([[delta.x], [delta.y], [delta.z]]) *
alpha).tolist()
for u, s in zip(update, leg.getSegments()):
s.setRotation(s.getRotation() + u[0])
i += 1
worldRefFrame = ReferenceFrame()
class HexapodMotionPlanner(object):
def __init__(self, config, body, legs):
self.body_ref = body.ref
self.legs = legs
self.leg_dict = dict(
(l.getId(),
(l, LegMotionPlanner(l, **config.getLegMotionPlannerParams())))
for l in legs)
self.options = config.getHexapodMotionPlannerParams()
if "delta" not in self.options:
self.options["delta"] = 0.1
def __init__(self, parent_ref, config):
irot, itrans = config.getInitialPose()
self.ref = ReferenceFrame(Vector3(itrans),
Quaternion.from_eulers(irot), parent_ref)
self.legs = []
