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 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 base_H_ee(geometryModel, eelinkname):
'''Compute the homogeneous transformation matrix from the frame of the link
given in the argument to the robot base, for the default configuration of
the robot.'''
robot = geometryModel.connectivityModel
tree = robmodel.treeutils.TreeUtils(robot)
frames = geometryModel.framesModel
if eelinkname not in robot.links:
logger.error("Could not find link '{0}' on robot '{1}'".format(
eelinkname, robot.name))
return None
ee = robot.links[eelinkname]
H = np.identity(4)
currentLink = ee
while currentLink is not None:
parent = tree.parent(currentLink)
if parent is not None:
joint = tree.connectingJoint(currentLink, parent)
frJ = frames.jointFrames[joint]
frP = frames.linkFrames[parent]
pose = primitives.Pose(target=frJ, reference=frP)
posespec = geometryModel.byPose[pose]
parent_CT_link = motionconvert.toCoordinateTransform(posespec)
parent_H_link = mxrepr.hCoordinatesNumeric(parent_CT_link)
H = parent_H_link @ H
#print( pose, "\n")
#print( np.round(link_H_parent,5), "\n", np.round(H[:3,3], 5), "\n" )
currentLink = parent
return H
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_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 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 base_H_ee(kinematics, framename):
if framename not in kinematics.robotGeometry.framesModel.framesByName:
logger.error("Could not find frame '{0}' in model '{1}'".format(
framename, kinematics.robotGeometry.robotName))
return None
ee = kinematics.robotGeometry.framesModel.framesByName[framename]
if not kinematics.framesConnectivity.hasRelativePose(
ee, kinematics.baseFrame):
logger.error(
"Frame '{0}' and the base frame do not seem to be connected".
format(framename))
return None
poseSpec = kinematics.framesConnectivity.getPoseSpec(
ee, kinematics.baseFrame)
cotr = frommotions.toCoordinateTransform(poseSpec)
H = mxrepr.hCoordinatesSymbolic(cotr)
q = numpy.zeros(len(H.variables))
H = H.setVariablesValue(valueslist=q)
return H
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)