SelectionMatrix3D selectionMatrix3D = new SelectionMatrix3D();
@Override public void onBehaviorEntered() { System.out.println("kinematics planning behavior"); if (keyFrameTimes.size() == 0) throw new RuntimeException("key frame times should be set ahead."); if (rigidBodyMessages.size() == 0) throw new RuntimeException("rigid body key frames should be set ahead."); toolboxStatePublisher.publish(MessageTools.createToolboxStateMessage(ToolboxState.WAKE_UP)); for (int i = 0; i < rigidBodyMessages.size(); i++) { KinematicsPlanningToolboxRigidBodyMessage message = rigidBodyMessages.get(i); message.getAngularWeightMatrix().set(MessageTools.createWeightMatrix3DMessage(defaultRigidBodyWeight)); message.getLinearWeightMatrix().set(MessageTools.createWeightMatrix3DMessage(defaultRigidBodyWeight)); rigidBodyMessagePublisher.publish(message); } Vector3DReadOnly translationVector = fullRobotModel.getRootBody().getBodyFixedFrame().getTransformToWorldFrame().getTranslationVector(); List<Point3DReadOnly> desiredCOMPoints = new ArrayList<Point3DReadOnly>(); for (int i = 0; i < keyFrameTimes.size(); i++) desiredCOMPoints.add(new Point3D(translationVector)); KinematicsPlanningToolboxCenterOfMassMessage comMessage = HumanoidMessageTools.createKinematicsPlanningToolboxCenterOfMassMessage(keyFrameTimes, desiredCOMPoints); SelectionMatrix3D selectionMatrix = new SelectionMatrix3D(); selectionMatrix.selectZAxis(false); comMessage.getSelectionMatrix().set(MessageTools.createSelectionMatrix3DMessage(selectionMatrix)); comMessage.getWeights().set(MessageTools.createWeightMatrix3DMessage(defaultCOMWeight)); comMessagePublisher.publish(comMessage); System.out.println("published"); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 300000) public void testSelectionForSimpleObjective() throws Exception { ReferenceFrame taskFrame = ReferenceFrame.getWorldFrame(); SelectionMatrix3D selectionMatrix = new SelectionMatrix3D(); ReferenceFrame selectionFrame; Vector3D objective; Vector3D result; for (int i = 0; i < 1000; i++) { RigidBodyTransform selectionFrameTransform = EuclidCoreRandomTools.nextRigidBodyTransform(random); selectionFrame = ReferenceFrame.constructFrameWithUnchangingTransformFromParent("SelectionFrame", ReferenceFrame.getWorldFrame(), selectionFrameTransform); objective = EuclidCoreRandomTools.nextVector3D(random); selectionMatrix.setSelectionFrame(selectionFrame); selectionMatrix.selectXAxis(random.nextBoolean()); selectionMatrix.selectYAxis(random.nextBoolean()); selectionMatrix.selectZAxis(random.nextBoolean()); result = runTest(taskFrame, selectionMatrix, objective); } if (visualize) { visualize(taskFrame, selectionFrame, objective, result); } }
SelectionMatrix3D selectionMatrix3D = new SelectionMatrix3D(); FrameMatrix3D frameMatrix3D = new FrameMatrix3D();
public static void main(String[] args) { RigidBodyTransform t1 = new RigidBodyTransform(); Vector3D rotationVector = new Vector3D(); DenseMatrix64F rotationVectorMatrix = new DenseMatrix64F(3, 1); t1.appendYawRotation(Math.PI / 8.0); t1.getRotation(rotationVector); rotationVector.get(rotationVectorMatrix); SelectionMatrix3D selectionMatrix3d = new SelectionMatrix3D(); selectionMatrix3d.selectZAxis(false); DenseMatrix64F selectionMatrix = new DenseMatrix64F(3, 3); selectionMatrix3d.getFullSelectionMatrixInFrame(null, selectionMatrix); DenseMatrix64F result = new DenseMatrix64F(3, 1); CommonOps.mult(selectionMatrix, rotationVectorMatrix, result); System.out.println(result); rotationVector.set(result); RotationMatrix rm = new RotationMatrix(rotationVector); System.out.println(rm); } }
SelectionMatrix3D selectionMatrix3D = new SelectionMatrix3D(); FrameMatrix3D frameMatrix3D = new FrameMatrix3D();
SelectionMatrix3D angularPart3D = new SelectionMatrix3D(); SelectionMatrix3D linearPart3D = new SelectionMatrix3D();
/** * Convenience method to create a {@link KinematicsToolboxCenterOfMassMessage} that can be used * to hold the current center of mass of a robot given its root body. * <p> * By default the weight of the task is set to {@value #DEFAULT_CENTER_OF_MASS_WEIGHT} which it a * rather strong weight for the center of mass. As result, the solution should be very close to * the current center of mass position. * </p> * * @param rootBody the root body of the robot for which the center of mass is to be held in * place. * @param holdX whether the x-coordinate should be maintained. * @param holdY whether the y-coordinate should be maintained. * @param holdZ whether the z-coordinate should be maintained. * @return the message ready to send to the {@code KinematicsToolbosModule}. */ public static KinematicsToolboxCenterOfMassMessage holdCenterOfMassCurrentPosition(RigidBodyBasics rootBody, boolean holdX, boolean holdY, boolean holdZ) { KinematicsToolboxCenterOfMassMessage message = new KinematicsToolboxCenterOfMassMessage(); CenterOfMassCalculator calculator = new CenterOfMassCalculator(rootBody, worldFrame); calculator.reset(); message.getDesiredPositionInWorld().set(calculator.getCenterOfMass()); message.getWeights().set(MessageTools.createWeightMatrix3DMessage(DEFAULT_CENTER_OF_MASS_WEIGHT)); SelectionMatrix3D selectionMatrix3D = new SelectionMatrix3D(); selectionMatrix3D.setAxisSelection(holdX, holdY, holdZ); message.getSelectionMatrix().set(MessageTools.createSelectionMatrix3DMessage(selectionMatrix3D)); message.setDestination(PacketDestination.KINEMATICS_TOOLBOX_MODULE.ordinal()); return message; }
SelectionMatrix3D selectionMatrix3D = new SelectionMatrix3D(); assertNull(selectionMatrix3D.getSelectionFrame()); assertTrue(selectionMatrix3D.isXSelected());
SelectionMatrix3D selectionMatrix3D = new SelectionMatrix3D(); selectionMatrix3D.selectZAxis(random.nextBoolean()); selectionMatrix3D.selectYAxis(random.nextBoolean());
SelectionMatrix3D selectionMatrix = new SelectionMatrix3D(); selectionMatrix.selectZAxis(false); message.getSelectionMatrix().set(MessageTools.createSelectionMatrix3DMessage(selectionMatrix));
endEffectorMessage.getLinearWeightMatrix().set(MessageTools.createWeightMatrix3DMessage(20.0)); SelectionMatrix3D selectionMatrix = new SelectionMatrix3D(); selectionMatrix.selectZAxis(false); comMessage.getSelectionMatrix().set(MessageTools.createSelectionMatrix3DMessage(selectionMatrix));
SelectionMatrix3D selectionMatrix = new SelectionMatrix3D(); selectionMatrix.selectZAxis(false); message.getSelectionMatrix().set(MessageTools.createSelectionMatrix3DMessage(selectionMatrix));
SelectionMatrix3D selectionMatrix3D = new SelectionMatrix3D(); selectionMatrix3D.selectZAxis(false); selectionMatrix3D.selectYAxis(false);
SelectionMatrix3D selectionMatrix = new SelectionMatrix3D(); selectionMatrix.selectZAxis(false); message.getSelectionMatrix().set(MessageTools.createSelectionMatrix3DMessage(selectionMatrix));