def fromDict(data): outputs = {} queryout = data['outputs'] if 'poses' in queryout : poses = [] for pose in queryout['poses']: tgt = gr.Frame(pose['target']) ref = gr.Frame(pose['reference']) poses.append( gr.Pose(target=tgt, reference=ref) ) outputs['poses'] = poses else : outputs['poses'] = [] if 'velocities' in queryout : outputs['velocities'] = [VelSpecs(**vel) for vel in queryout['velocities']] # we expect the dictionary data to match the fields of the # named tuples thus we can use the **notation else : outputs['velocities'] = [] if 'jacs' in queryout : outputs['jacobians'] = [JacSpecs(**jac) for jac in queryout['jacs']] else : outputs['jacobians'] = [] return _FKSolver(data['name'], data['kind'], outputs)
def __init__(self, specs): self._robot = specs.rmodels['robot'] self._frames = specs.rmodels['frames'] self.name = specs.name self.level = specs.level self.cfgSpace = specs.cfgSpace self.targetFrame = specs.targetFrame self.referenceFrame = specs.referenceFrame fkSolverName = "fk__" + self.name pose = gr.Pose(target=specs.targetFrame, reference=specs.referenceFrame) fkouts = {} fkouts['pose'] = [pose] tgtL = specs.targetFrame.body refL = specs.referenceFrame.body velSpecs = query.VelSpecs(target=tgtL.name, reference=refL.name, kind="6D", cframe="NA") vel = query.checkVelocitySpecs(specs.rmodels['frames'], None, velSpecs) fkouts['jacobian'] = [JacobianSpecs(velocity=vel)] self.requiredFK = FKSolverSpecs(name=fkSolverName, kind="sweeping", rmodels=specs.rmodels, requests=fkouts)
def posePath(self, givenpose): ''' The sequence of distance-1 poses equivalent to the given pose. Distance-1 poses involve adjacent frames, i.e. their value is a "primitive" value of the robot model. The returned sequence is constructed from the shortest path connecting the target frame and the reference frame of the original given pose. The returned sequence is in fact an optcompose.Path object. While building the sequence, this method also populates the sets of constant-poses and joint-poses of the robot model. ''' composablesList = [] framesGraph = self.rmodels['frames'] graphPath = framesGraph.path(givenpose.target, givenpose.reference) tgt = givenpose.target for ref in graphPath[1:]: pose = _ComposablePose(gr.Pose(tgt, ref)) if framesGraph.kind(tgt, ref) is FrameRelationKind.acrossJoint: pose.joint = framesGraph.joint(tgt, ref) self.jointPoses.add(pose) else: self.constPoses.add(pose) #if not framesGraph.relativePoseIsIdentity(tgt, ref) : composablesList.append(pose) tgt = ref return optcompose.Path(composablesList)
def validatePoses(self, posesRequest) : poses = [] for pose in posesRequest : target = self.frames.getAttachedFrame(pose.target) reference = self.frames.getAttachedFrame(pose.reference) if target==None or reference==None : raise RuntimeError("Could not find attached frame " + pose.target.name + " or " + pose.reference.name) poses.append( gr.Pose(target, reference) ) return poses
def singleLine(jvel): tpl = "" poseid = "" if jvel.polarity == -1: refF = self.solverModel.robotFrames.framesByName[ jvel.vel.target.name] tgtF = self.solverModel.robotFrames.framesByName[ jvel.joint.name] pose = gr.Pose(target=tgtF, reference=refF) poseid = poseIdentifier(pose) tpl = "${velid} = { joint='${joint.name}', polarity=-1, ctransform='${pose}' }" else: tpl = "${velid} = { joint='${joint.name}', polarity=1 }" context = { 'velid': velocityIdentifier(jvel.vel), 'joint': jvel.joint, 'jnum': self.solverModel.robot.jointNum(jvel.joint), 'pose': poseid } return Template(tpl).render(**context)
def __init__(self, solverSpec): self.name = solverSpec.name self.rmodels = solverSpec.rmodels self.constPoses = set() self.jointPoses = set() self.output = solverSpec.requests framesModel = self.rmodels['frames'] allPoses = set(self.output['pose']) # shallow copy of the list # Geometric Jacobians require specific relative poses to be computed, so # we must add those to the poses explicitly requested in the query. self.geometricJacobians = [] for J in self.output['jacobian']: jac = jacobians.GeometricJacobian(framesModel, J.velocity) allPoses.update(jac.jointPoses) allPoses.add(jac.targetPose) self.geometricJacobians.append(jac) self.output['jacobian'] = self.geometricJacobians self.jointVelocities = {} velComposePaths = [ self.velocityPath(v) for v in self.output['velocity'] ] self.velComposes = optcompose.allComposes(velComposePaths) self.velBinaryComposes = [] # The composition of velocities, on the other hand, requires certain # coordinate transforms. We can again add some relative poses to achieve # the desired effect, relying on the assumption that relative poses # will be concretely represented by usable coordinate transforms, e.g. # homogeneous transforms. This is a bit of hack, as the two concepts # (relative pose and coordinate transform) should be probably kept # separate. for vcomp in self.velComposes: for vcomp in vcomp.asSequenceOfBinaryCompositions(): #print(vcomp.arg1.target.name + " " + vcomp.arg1.reference.name) #print(vcomp.arg2.target.name + " " + vcomp.arg2.reference.name) #print("require transform from {0} to {1}".format(vcomp.arg2.target.name,vcomp.arg1.target.name)) # Now the hack gets uglier, because we are also assuming that # this specific pose (see code) is the one encoding the appropriate # coordinate transform... tgtF = framesModel.framesByName[vcomp.arg2.target.name] refF = framesModel.framesByName[vcomp.arg1.target.name] pose = gr.Pose(target=tgtF, reference=refF) allPoses.add(pose) vcomp.pose = pose self.velBinaryComposes.append(vcomp) # The second argument of any binary velocity composition, is the one that # gets coordinate-transformed, thus it must have been computed explicitly # before the composition. If it is a velocity across a joint, then we # have to mark it for explicit computation, because joint velocities are # in general not computed explicitly for optimization purposes. self.jointVelocitiesExplicit = set() for vbc in self.velBinaryComposes: if vbc.arg2.v in self.jointVelocities.keys(): self.jointVelocitiesExplicit.add(vbc.arg2.v) # Consider the corner case in which the desired output velocity is a # joint velocity for v in self.output['velocity']: if v in self.jointVelocities.keys(): self.jointVelocitiesExplicit.add(v) # Joint velocities with opposite polarity (that is, velocity of predecessor # relative to successor), require the coordinate transform from joint # frame to predecessor frame. We add here the corresponding pose to make # sure the solver will include it for v in self.jointVelocities.values(): if v.polarity == -1: refF = self.robotFrames.framesByName[v.vel.target.name] tgtF = self.robotFrames.framesByName[v.joint.name] pose = gr.Pose(target=tgtF, reference=refF) allPoses.add(pose) poseComposePaths = [self.posePath(pose) for pose in allPoses] self.poseComposes = optcompose.allComposes(poseComposePaths)