def test_twoTransformsFromOnePose(self):
'''
Test the creation of a transform and its inverse from the same pose.
Given a single pose, two coordinate transforms with opposite polarity
can be constructed; they should be the inverse of each other.
'''
mot = self.backend.randomMotion()
pose = motions.PoseSpec(pose=pBA, motion=mot)
A_ct_B = toCoordinateTransform(poseSpec=pose, polarity=R_ct_T)
B_ct_A = toCoordinateTransform(poseSpec=pose, polarity=T_ct_R)
self.assertTrue(A_ct_B.leftFrame == B_ct_A.rightFrame)
self.assertTrue(A_ct_B.rightFrame == B_ct_A.leftFrame)
# now check that the API is consistent, and we can use the
# 'right_frame' argument instead of 'polarity', to achieve the same transforms
A_ct_B_ = toCoordinateTransform(poseSpec=pose, right_frame=frB)
B_ct_A_ = toCoordinateTransform(poseSpec=pose, right_frame=frA)
self.assertTrue(A_ct_B == A_ct_B_)
equals = (B_ct_A == B_ct_A_)
self.assertTrue(equals)
# Finally check that the matrix representation is consistent, one should
# be the inverse of the other
A_X_B = self.backend.asMatrix(A_ct_B)
B_X_A = self.backend.asMatrix(B_ct_A)
equals = self.backend.equal_matrix(
self.backend.mult_matrix(A_X_B, B_X_A), self.backend.identity())
self.assertTrue(equals)
def getDebugSample(numMixin, reprMixin):
class Mixin(numMixin, reprMixin):
pass
obj = Mixin()
mot = obj.randomMotion()
pose = motions.PoseSpec(pose=pBA, motion=mot)
A_ct_B = toCoordinateTransform(poseSpec=pose, polarity=R_ct_T)
A_X_B = obj.asMatrix(A_ct_B)
return mot, pose, A_ct_B, A_X_B
def test_primitiveTransformPolarity(self):
motion = self.backend.generator.randomMotion()
pose = motions.PoseSpec(pose=pBA, motion=motion)
tr1 = toCoordinateTransform(pose, primitives_polarity=R_ct_T)
tr2 = toCoordinateTransform(pose, primitives_polarity=T_ct_R)
M1 = self.backend.asMatrix(tr1)
M2 = self.backend.asMatrix(tr2)
equal = self.backend.equal_matrix(M1, M2)
self.assertTrue(equal)
def test_extrinsicIntrinsic(self):
'''Test about extrinsic and intrinsic rotations
Extrinsic rotations (ie about fixed axes) are equivalent to intrinsic
rotations (about moving axes) in the opposite order'''
rots = self.backend.randomRotations()
extrinsic = MotionSequence(rots, MotionSequence.Mode.fixedFrame)
intrinsic = MotionSequence(rots[::-1],
MotionSequence.Mode.currentFrame)
e_pose = motions.PoseSpec(pose=pBA, motion=extrinsic)
i_pose = motions.PoseSpec(pose=pCB, motion=intrinsic)
e_ct = toCoordinateTransform(e_pose)
i_ct = toCoordinateTransform(i_pose)
e_repr = self.backend.asMatrix(e_ct)
i_repr = self.backend.asMatrix(i_ct)
self.assertTrue(self.backend.equal_matrix(e_repr, i_repr))
def test_poseInference(self):
mot1 = self.backend.randomMotion()
mot2 = self.backend.randomMotion()
pose1 = motions.PoseSpec(pose=pBA, motion=mot1)
pose2 = motions.PoseSpec(pose=pCB, motion=mot2)
model = motions.PosesSpec(name='test', poses=[pose1, pose2])
inspector = motions.ConnectedFramesInspector(model)
self.assertTrue(inspector.hasRelativePose(frA, frC))
posespec = inspector.getPoseSpec(targetFrame=frC, referenceFrame=frA)
A_ct_B = toCoordinateTransform(pose1)
B_ct_C = toCoordinateTransform(pose2)
A_ct_C = toCoordinateTransform(posespec) # to be tested
A_X_B = self.backend.asMatrix(A_ct_B)
B_X_C = self.backend.asMatrix(B_ct_C)
A_X_C = self.backend.asMatrix(A_ct_C)
self.assertTrue(
self.backend.equal_matrix(self.backend.mult_matrix(A_X_B, B_X_C),
A_X_C))
def test_freeSymbols(self):
'''Check the consistency of the data structures holding the free symbols of a matrix.
'''
mot1 = self.backend.randomMotion()
pose1 = motions.PoseSpec(pose=pBA, motion=mot1)
A_ct_B = toCoordinateTransform(pose1)
A_X_B = self.backend.asMatrix(A_ct_B)
setFromCTMetadata = set()
setFromCTMetadata.update(
[argument.symbol for argument in A_X_B.variables],
[argument.symbol for argument in A_X_B.parameters],
[argument.symbol for argument in A_X_B.constants])
correct = (setFromCTMetadata == A_X_B.mx.free_symbols)
if not correct:
logger.error(
"Symbols from the CT metadata: {0}\nSymbols from the Sympy matrix: {1}"
.format(str(setFromCTMetadata), str(A_X_B.mx.free_symbols)))
self.assertTrue(correct)
def test_motionInversion(self):
'''Test the inversion of a motion model
The coordinate transform associated to the inverse of a motion M,
should be the inverse of the transform associated to M.'''
mot = self.backend.randomMotion()
pose = motions.PoseSpec(pose=pBA, motion=mot)
ipose = motions.inversePoseSpec(pose)
A_ct_B = toCoordinateTransform(pose)
B_ct_A = toCoordinateTransform(ipose)
A_X_B = self.backend.asMatrix(A_ct_B)
B_X_A = self.backend.asMatrix(B_ct_A)
equal = self.backend.equal_matrix(
self.backend.mult_matrix(A_X_B, B_X_A), self.backend.identity())
self.assertTrue(equal)
def test_duality(self):
'''Check the relation between trasforms for spatial motion/force vectors.
The transpose of a transform for spatial motion vectors is equal to the
trasform for force vectors in the opposite polarity'''
mot = self.backend.randomMotion()
pose = motions.PoseSpec(pose=pBA, motion=mot)
A_ct_B = toCoordinateTransform(poseSpec=pose,
polarity=R_ct_T) # CT from B to A
B_ct_A = toCoordinateTransform(poseSpec=pose,
polarity=T_ct_R) # CT from A to B
A_SM_B = self.backend.asMotionTransform(A_ct_B)
B_SF_A = self.backend.asForceTransform(B_ct_A)
good = self.backend.equal_matrix(self.backend.transpose(A_SM_B),
B_SF_A)
if not good:
logger.error("\n" + self.backend.prettyStr(A_SM_B))
logger.error("\n" + self.backend.prettyStr(B_SF_A))
self.assertTrue(good)
def __init__(self, connectModel, framesModel, posesModel, jointAxes=None):
'''
Construct the geometry model of a mechanism by composing the arguments.
Parameters:
- `connectModel` the connectivity model of the mechanism, with ordering
- `framesModel` the set of the Cartesian frames attached to the mechanism
- `posesModel` the constant, relative poses between the frames
- `jointAxes` the versors of the joint axes, in joint frame coordinates
The third argument is the actual geometric data; this constructor makes
sure that the three arguments are consistent and therefore can be
composed as a whole.
In fact, the poses model stored in this instance is not the given
`posesModel`. The input poses are always replaced by poses pointing to
objects of `framesModel`. This is because the frames in the given
`posesModel` might be simple placeholders. The name of the frame is used
to establish the correspondence between a placeholder frame and a
robot-attached frame.
The `jointAxes` argument defaults to `None`. In that case it is assumed
that the axis of any joint is the Z axis of its frame. Otherwise, the
argument should be a dictionary indexed by joint name, with values being
3-value tuples. Any tuple must represent the 3D joint axis versor, using
coordinates in the joint frame.
'''
if not isinstance(connectModel, ordering.Robot):
raise RuntimeError("An ordered robot model is required")
rname = connectModel.name
if framesModel.robot.name != rname:
raise RuntimeError(
"Robot name mismatch in the connectivity and frames model ('{0}' vs '{1})"
.format(rname, framesModel.robot.name))
self.byPose = {}
for poseSpec in posesModel.poses:
ignore = False
pose = poseSpec.pose
warnmsg = '''Frame '{0}' not found on the given frames-model '{1}', ignoring'''
if pose.target.name not in framesModel.framesByName:
logger.warning(warnmsg.format(pose.target.name, rname))
ignore = True
if pose.reference.name not in framesModel.framesByName:
logger.warning(warnmsg.format(pose.reference.name, rname))
ignore = True
if not ignore:
tgtF = framesModel.framesByName[pose.target.name]
refF = framesModel.framesByName[pose.reference.name]
path = framesModel.path(tgtF, refF)
for i in range(len(path) - 1):
kind = framesModel.kind(path[i], path[i + 1])
if kind == frames.FrameRelationKind.acrossJoint:
warnmsg = '''Found non-constant pose in the geometry model
('{0}' with respect to '{1}')'''.format(
tgtF.name, refF.name)
logger.warning(warnmsg)
# Rebuild the Pose instance with the frames from the frames
# model, which are attached to links. The pose instance from the
# motions model (posesModel) might only refer to named frames
realpose = Pose(target=tgtF, reference=refF)
realposespec = motions.PoseSpec(pose=realpose,
motion=poseSpec.motion)
self.byPose[realpose] = realposespec
#print( realpose )
#print( poseSpec.motion.steps, "\n\n")
# We iterate over joints and fetch the predecessor, as the joint_wrt_predecessor
# is a constant pose also for loop joints.
self.byJoint = {}
for joint in connectModel.joints.values():
predFrame = framesModel.linkFrames[connectModel.predecessor(joint)]
jointFrame = framesModel.jointFrames[joint]
pose = Pose(target=jointFrame, reference=predFrame)
if pose not in self.byPose:
logger.warning(
"The geometry model does not seem to have information about the pose of '{0}' wrt '{1}'"
.format(jointFrame.name, predFrame.name))
else:
self.byJoint[joint] = self.byPose[pose]
self.ordering = connectModel
self.frames = framesModel
self.poses = motions.PosesSpec(posesModel.name,
list(self.byPose.values()))
# The last line creates a new PosesSpec model, to make sure we store the
# model whose poses reference robot attached frames. As said before, the
# PosesSpec model given to the constructor might have poses that refer
# to the un-attached frames.
if jointAxes == None:
jointAxes = {}
for joint in connectModel.joints.values():
jointAxes[joint.name] = (0.0, 0.0, 1.0) # default is Z axis
else:
if jointAxes.keys() != connectModel.joints.keys():
logger.warning(
"The names in the joint-axes dictionary do not " +
"match the names in the connectivity model")
self.axes = jointAxes
import kgprim.core as primitives
import kgprim.motions as mot
import kgprim.ct.frommotions as mot2ct
import kgprim.ct.repr.mxrepr as mxrepr
# make a motion sequence, with two steps
rx = mot.MotionStep(mot.MotionStep.Kind.Rotation, mot.Axis.X, 0.5)
ty = mot.MotionStep(mot.MotionStep.Kind.Translation, mot.Axis.Y, 1.1)
seq = mot.MotionSequence([rx, ty], mot.MotionSequence.Mode.currentFrame)
# make a dummy pose
fR = primitives.Frame('R')
fT = primitives.Frame('T')
pose = primitives.Pose(reference=fR, target=fT)
# augment the pose with the motion spec; this object models the fact that to
# go from 'R' to 'T' one has to take the motion steps defined above
poseSpec = mot.PoseSpec(pose, seq)
# convert it to a coordinate transform model; by default, this is the transform
# from 'T' coordinates to 'R' coordinates, i.e. R_X_T
ct = mot2ct.toCoordinateTransform(poseSpec)
# use the functors in the matrix-representation module to get actual matrices
H = mxrepr.hCoordinatesNumeric( ct )
R = mxrepr.rotationMatrixNumeric( ct )
print(H)
print(R)
