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)