class HexapodBody(object):
def __init__(self, parent_ref, config):
irot, itrans = config.getInitialPose()
self.ref = ReferenceFrame(Vector3(itrans),
Quaternion.from_eulers(irot), parent_ref)
self.legs = []
def getLegSegments(self, config, lobj):
legargs = config.getLegSegmentConfig(lobj.getId())
segs = []
prevSeg = lobj
for args in legargs:
prevSeg = HexapodLegSegment(prevSeg, *args)
segs.append(prevSeg)
return segs
# Returns -1 if left side, 1 otherwise.
def getLegDirection(self, leg):
if "left" in leg.getId():
return -1
else:
return 1
def setRotation(self, rot):
self.ref.setRotation(rot)
def setTranslation(self, trans):
self.ref.setTranslation(trans)
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")
class HexapodBody(object):
def __init__(self, parent_ref, config):
irot, itrans = config.getInitialPose();
self.ref = ReferenceFrame(Vector3(itrans), Quaternion.from_eulers(irot), parent_ref);
self.legs = [];
def getLegSegments(self, config, lobj):
legargs = config.getLegSegmentConfig(lobj.getId())
segs = [];
prevSeg = lobj;
for args in legargs:
prevSeg = HexapodLegSegment(prevSeg, *args);
segs.append(prevSeg);
return segs;
# Returns -1 if left side, 1 otherwise.
def getLegDirection(self, leg):
if "left" in leg.getId():
return -1;
else:
return 1;
def setRotation(self, rot):
self.ref.setRotation(rot);
def setTranslation(self, trans):
self.ref.setTranslation(trans);
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 = []
class HexapodLeg(object):
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 getDirection(self):
return self.direction
def getId(self):
return self.id
def getSegments(self):
return self.segments
def getEndEffector(self):
return self.segments[-1]
def getEndEffectorPosition(self):
s_pos, s_rot, joint_axis = self.getEndEffector(
).computeForwardKinematics()
return s_pos[-1]
def update(self, angles):
for s, a in zip(self.segments, angles):
s.setRotation(a)
# Get a position fixed to the world, so that it can be used to plan steps.
def getWorldPositionAnchor(self):
return self.world_ref.getTranslation()
def computeForwardKinematics(self):
return ([self.ref.getTranslation()], [self.ref.getRotation()], [])
def clone(self):
return HexapodLeg(self)
def computeInverseKinematicsPass(self):
s_pos, s_rot, joint_axis = self.getEndEffector(
).computeForwardKinematics()
# Get the end effector position:
ee = s_pos[-1]
# For each joint, calculate the Jacobian:
jacob = [(c_rot ^ (ee - p_pos)).xyz
for p_pos, c_rot in zip(s_pos[:-1], joint_axis)]
jacob = np.matrix(jacob)
jacob_t = np.linalg.pinv(jacob.T)
return s_pos, jacob_t
def __init__(self, parent_ref, config):
irot, itrans = config.getInitialPose();
self.ref = ReferenceFrame(Vector3(itrans), Quaternion.from_eulers(irot), parent_ref);
self.legs = [];
class HexapodLeg(object):
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 getDirection(self):
return self.direction;
def getId(self):
return self.id;
def getSegments(self):
return self.segments;
def getEndEffector(self):
return self.segments[-1];
def getEndEffectorPosition(self):
s_pos, s_rot, joint_axis = self.getEndEffector().computeForwardKinematics();
return s_pos[-1];
def update(self, angles):
for s,a in zip(self.segments, angles):
s.setRotation(a);
# Get a position fixed to the world, so that it can be used to plan steps.
def getWorldPositionAnchor(self):
return self.world_ref.getTranslation();
def computeForwardKinematics(self):
return ([self.ref.getTranslation()],[self.ref.getRotation()], []);
def clone(self):
return HexapodLeg(self);
def computeInverseKinematicsPass(self):
s_pos, s_rot, joint_axis = self.getEndEffector().computeForwardKinematics();
# Get the end effector position:
ee = s_pos[-1];
# For each joint, calculate the Jacobian:
jacob = [(c_rot ^ (ee - p_pos)).xyz for p_pos, c_rot in zip(s_pos[:-1], joint_axis)];
jacob = np.matrix(jacob);
jacob_t = np.linalg.pinv(jacob.T);
return s_pos, jacob_t;
