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)