/** * Creates a new {@code TwistCalculator} that will compute all the twists of all the rigid-bodies * of the system to which {@code body} belongs. * * @param inertialFrame non-moving frame with respect to which the twists are computed. Typically * {@link ReferenceFrame#getWorldFrame()} is used here. * @param body a body that belongs to the system this twist calculator will be available for. */ public TwistCalculator(ReferenceFrame inertialFrame, RigidBodyBasics body) { this.inertialFrame = inertialFrame; this.rootBody = MultiBodySystemTools.getRootBody(body); this.rootTwist = new Twist(rootBody.getBodyFixedFrame(), inertialFrame, rootBody.getBodyFixedFrame()); int numberOfRigidBodies = MultiBodySystemTools.collectSubtreeSuccessors(MultiBodySystemTools.collectSubtreeJoints(rootBody)).length; while (unnassignedTwists.size() < numberOfRigidBodies) unnassignedTwists.add(new Twist()); assignedTwists = new ArrayList<>(numberOfRigidBodies); rigidBodiesWithAssignedTwist = new ArrayList<>(numberOfRigidBodies); assignedTwists.add(rootTwist); rigidBodiesWithAssignedTwist.add(rootBody); rigidBodyToAssignedTwistIndex.put(rootBody, new MutableInt(0)); }
@ContinuousIntegrationTest(estimatedDuration = 0.4) @Test(timeout = 30000) public void testHoldBodyPose() throws Exception { Pair<FloatingJointBasics, OneDoFJointBasics[]> initialFullRobotModel = createFullRobotModelAtInitialConfiguration(); snapGhostToFullRobotModel(initialFullRobotModel); RigidBodyBasics hand = ScrewTools.findRigidBodiesWithNames(MultiBodySystemTools.collectSubtreeSuccessors(initialFullRobotModel.getRight()), "handLink")[0]; commandInputManager.submitMessage(holdRigidBodyCurrentPose(hand)); RobotConfigurationData robotConfigurationData = extractRobotConfigurationData(initialFullRobotModel); toolboxController.updateRobotConfigurationData(robotConfigurationData); int numberOfIterations = 250; runKinematicsToolboxController(numberOfIterations); assertTrue(KinematicsToolboxController.class.getSimpleName() + " did not manage to initialize.", initializationSucceeded.getBooleanValue()); assertTrue("Poor solution quality: " + toolboxController.getSolution().getSolutionQuality(), toolboxController.getSolution().getSolutionQuality() < 1.0e-4); }
RigidBodyBasics hand = ScrewTools.findRigidBodiesWithNames(MultiBodySystemTools.collectSubtreeSuccessors(randomizedFullRobotModel.getRight()), "handLink")[0]; KinematicsToolboxRigidBodyMessage message = holdRigidBodyCurrentPose(hand); message.getAngularWeightMatrix().set(MessageTools.createWeightMatrix3DMessage(20.0));
RigidBodyBasics hand = ScrewTools.findRigidBodiesWithNames(MultiBodySystemTools.collectSubtreeSuccessors(randomizedFullRobotModel.getRight()), "handLink")[0]; FramePoint3D desiredPosition = new FramePoint3D(hand.getBodyFixedFrame()); desiredPosition.changeFrame(worldFrame);