public SelectionMatrix6D getSelectionMatrix() { SelectionMatrix6D selectionMatrix = new SelectionMatrix6D(); selectionMatrix.clearSelection(); if (trajectorySelectionMatrix.getLinearPart().isXSelected() || explorationSelectionMatrix.getLinearPart().isXSelected()) selectionMatrix.getLinearPart().selectXAxis(true); if (trajectorySelectionMatrix.getLinearPart().isYSelected() || explorationSelectionMatrix.getLinearPart().isYSelected()) selectionMatrix.getLinearPart().selectYAxis(true); if (trajectorySelectionMatrix.getLinearPart().isZSelected() || explorationSelectionMatrix.getLinearPart().isZSelected()) selectionMatrix.getLinearPart().selectZAxis(true); if (trajectorySelectionMatrix.getAngularPart().isXSelected() || explorationSelectionMatrix.getAngularPart().isXSelected()) selectionMatrix.getAngularPart().selectXAxis(true); if (trajectorySelectionMatrix.getAngularPart().isYSelected() || explorationSelectionMatrix.getAngularPart().isYSelected()) selectionMatrix.getAngularPart().selectYAxis(true); if (trajectorySelectionMatrix.getAngularPart().isZSelected() || explorationSelectionMatrix.getAngularPart().isZSelected()) selectionMatrix.getAngularPart().selectZAxis(true); return selectionMatrix; }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testSO3() { Random random = new Random(4219L); for (int i = 0; i < 100; i++) { SO3TrajectoryControllerCommand expected = new SO3TrajectoryControllerCommand(random); SE3TrajectoryControllerCommand intermediate = new SE3TrajectoryControllerCommand(); SO3TrajectoryControllerCommand actual = new SO3TrajectoryControllerCommand(); CommandConversionTools.convertToSE3(expected, intermediate); CommandConversionTools.convertToSO3(intermediate, actual); Assert.assertEquals(0, intermediate.getSelectionMatrix().getLinearPart().getNumberOfSelectedAxes()); Assert.assertTrue(expected.epsilonEquals(actual, Double.MIN_VALUE)); } }
rigidBodyMessage.getControlFramePositionInEndEffector().set(command.getControlFramePose().getPosition()); rigidBodyMessage.getControlFrameOrientationInEndEffector().set(command.getControlFramePose().getOrientation()); rigidBodyMessage.getLinearSelectionMatrix().set(MessageTools.createSelectionMatrix3DMessage(command.getSelectionMatrix().getLinearPart())); rigidBodyMessage.getAngularSelectionMatrix().set(MessageTools.createSelectionMatrix3DMessage(command.getSelectionMatrix().getAngularPart())); rigidBodyMessage.getLinearWeightMatrix().set(MessageTools.createWeightMatrix3DMessage(command.getWeightMatrix().getLinearPart()));
selectionMatrix6D.setSelectionFrames(angularSelectionFrame, linearSelectionFrame); message.getAngularSelectionMatrix().set(MessageTools.createSelectionMatrix3DMessage(selectionMatrix6D.getAngularPart())); message.getLinearSelectionMatrix().set(MessageTools.createSelectionMatrix3DMessage(selectionMatrix6D.getLinearPart())); spatialAccelerationCommand.getSelectionMatrix(selectionMatrixFromCommand); SelectionMatrix3D selectionMatrixLinearPart = selectionMatrix6D.getLinearPart(); assertEquals(linearXSelected, selectionMatrixLinearPart.isXSelected()); assertEquals(linearYSelected, selectionMatrixLinearPart.isYSelected());
public static void convertToEuclidean(SE3TrajectoryControllerCommand command, EuclideanTrajectoryControllerCommand commandToPack) { commandToPack.setQueueableCommandVariables(command); commandToPack.setTrajectoryFrame(command.getTrajectoryFrame()); commandToPack.getSelectionMatrix().set(command.getSelectionMatrix().getLinearPart()); commandToPack.getWeightMatrix().set(command.getWeightMatrix().getLinearPart()); commandToPack.setUseCustomControlFrame(command.useCustomControlFrame()); commandToPack.setControlFramePose(command.getControlFramePose()); commandToPack.getTrajectoryPointList().setIncludingFrame(command.getTrajectoryPointList()); } }
selectionMatrix6D.setLinearAxisSelection(linearXSelected, linearYSelected, linearZSelected); message.getAngularSelectionMatrix().set(MessageTools.createSelectionMatrix3DMessage(selectionMatrix6D.getAngularPart())); message.getLinearSelectionMatrix().set(MessageTools.createSelectionMatrix3DMessage(selectionMatrix6D.getLinearPart())); SelectionMatrix3D selectionMatrixLinearPart = selectionMatrix6D.getLinearPart(); assertEquals(linearXSelected, selectionMatrixLinearPart.isXSelected()); assertEquals(linearYSelected, selectionMatrixLinearPart.isYSelected());
public KinematicsToolboxRigidBodyMessage createMessage(double timeInTrajectory, Pose3D poseToAppend) { Pose3D desiredEndEffectorPose = appendPoseToTrajectory(timeInTrajectory, poseToAppend); KinematicsToolboxRigidBodyMessage message = MessageTools.createKinematicsToolboxRigidBodyMessage(rigidBody); message.getDesiredPositionInWorld().set(desiredEndEffectorPose.getPosition()); message.getDesiredOrientationInWorld().set(desiredEndEffectorPose.getOrientation()); message.getControlFramePositionInEndEffector().set(controlFramePose.getPosition()); message.getControlFrameOrientationInEndEffector().set(controlFramePose.getOrientation()); message.getAngularSelectionMatrix().set(MessageTools.createSelectionMatrix3DMessage(getSelectionMatrix().getAngularPart())); message.getLinearSelectionMatrix().set(MessageTools.createSelectionMatrix3DMessage(getSelectionMatrix().getLinearPart())); message.getAngularWeightMatrix().set(MessageTools.createWeightMatrix3DMessage(weight)); message.getLinearWeightMatrix().set(MessageTools.createWeightMatrix3DMessage(weight)); // Sylvain's value :: 0.5 return message; }
handMessage.getLinearSelectionMatrix().set(MessageTools.createSelectionMatrix3DMessage(selectionMatrix6D.getLinearPart())); kinematicsToolboxRigidBodyPublisher.publish(handMessage); chestMessage.getLinearSelectionMatrix().set(MessageTools.createSelectionMatrix3DMessage(chestSelectionMatrix.getLinearPart())); kinematicsToolboxRigidBodyPublisher.publish(chestMessage); pelvisMessage.getLinearSelectionMatrix().set(MessageTools.createSelectionMatrix3DMessage(pelvisSelectionMatrix.getLinearPart())); kinematicsToolboxRigidBodyPublisher.publish(pelvisMessage);
pelvisTrajectoryMessage.getSe3Trajectory().getAngularSelectionMatrix() .set(MessageTools.createSelectionMatrix3DMessage(selectionMatrix6D.getAngularPart())); pelvisTrajectoryMessage.getSe3Trajectory().getLinearSelectionMatrix().set(MessageTools.createSelectionMatrix3DMessage(selectionMatrix6D.getLinearPart()));
pelvisTrajectoryMessage.getSe3Trajectory().getAngularSelectionMatrix() .set(MessageTools.createSelectionMatrix3DMessage(selectionMatrix6D.getAngularPart())); pelvisTrajectoryMessage.getSe3Trajectory().getLinearSelectionMatrix().set(MessageTools.createSelectionMatrix3DMessage(selectionMatrix6D.getLinearPart()));
pelvisTrajectoryMessage.getSe3Trajectory().getAngularSelectionMatrix() .set(MessageTools.createSelectionMatrix3DMessage(selectionMatrix6D.getAngularPart())); pelvisTrajectoryMessage.getSe3Trajectory().getLinearSelectionMatrix().set(MessageTools.createSelectionMatrix3DMessage(selectionMatrix6D.getLinearPart()));