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;