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