/** * Sets the x-coordinate of this shape position. * * @param x the new x-coordinate for this shape. */ public final void setPositionX(double x) { shapePose.setTranslationX(x); }
/** * Sets the x and y coordinates of this shape position. * * @param point2D point holding the new x and y coordinates for this shape position. Not modified. */ public final void setPositionXY(Point2DReadOnly point2D) { shapePose.setTranslationX(point2D.getX()); shapePose.setTranslationY(point2D.getY()); }
public void getTransform(RigidBodyTransform transformToPack) { transformToPack.setRotation(orientation); transformToPack.setTranslationX(stateVector.get(positionStart + 0)); transformToPack.setTranslationY(stateVector.get(positionStart + 1)); transformToPack.setTranslationZ(stateVector.get(positionStart + 2)); }
private void notifyListenerSolutionWasFound(FootstepNode endNode) { FootstepPlan plan = new FootstepPlan(); List<FootstepNode> path = footstepGraph.getPathFromStart(bestGoalNode); path.add(goalNodes.get(bestGoalNode.getRobotSide().getOppositeSide())); for (int i = 1; i < path.size(); i++) { RobotSide robotSide = path.get(i).getRobotSide(); RigidBodyTransform footstepPose = new RigidBodyTransform(); footstepPose.setRotationYawAndZeroTranslation(path.get(i).getYaw()); footstepPose.setTranslationX(path.get(i).getX()); footstepPose.setTranslationY(path.get(i).getY()); RigidBodyTransform snapTransform = snapper.snapFootstepNode(path.get(i)).getSnapTransform(); if (!snapTransform.containsNaN()) snapTransform.transform(footstepPose); plan.addFootstep(robotSide, new FramePose3D(ReferenceFrame.getWorldFrame(), footstepPose)); } } }
@Override public FootstepPlan getPlan() { if (!footstepGraph.doesNodeExist(bestGoalNode)) return null; FootstepPlan plan = new FootstepPlan(); List<FootstepNode> path = footstepGraph.getPathFromStart(bestGoalNode); path.add(goalNodes.get(bestGoalNode.getRobotSide().getOppositeSide())); for (int i = 1; i < path.size(); i++) { RobotSide robotSide = path.get(i).getRobotSide(); RigidBodyTransform footstepPose = new RigidBodyTransform(); footstepPose.setRotationYawAndZeroTranslation(path.get(i).getYaw()); footstepPose.setTranslationX(path.get(i).getX()); footstepPose.setTranslationY(path.get(i).getY()); RigidBodyTransform snapTransform = snapper.snapFootstepNode(path.get(i)).getSnapTransform(); if (!snapTransform.containsNaN()) snapTransform.transform(footstepPose); plan.addFootstep(robotSide, new FramePose3D(ReferenceFrame.getWorldFrame(), footstepPose)); } return plan; }
@Override public FootstepPlan getPlan() { if (endNode == null || !graph.doesNodeExist(endNode)) return null; FootstepPlan plan = new FootstepPlan(); List<FootstepNode> path = graph.getPathFromStart(endNode); for (int i = 1; i < path.size(); i++) { RobotSide robotSide = path.get(i).getRobotSide(); RigidBodyTransform footstepPose = new RigidBodyTransform(); footstepPose.setRotationYawAndZeroTranslation(path.get(i).getYaw()); footstepPose.setTranslationX(path.get(i).getX()); footstepPose.setTranslationY(path.get(i).getY()); FootstepNodeSnapData snapData = snapper.snapFootstepNode(path.get(i)); RigidBodyTransform snapTransform = snapData.getSnapTransform(); snapTransform.transform(footstepPose); plan.addFootstep(robotSide, new FramePose3D(ReferenceFrame.getWorldFrame(), footstepPose)); ConvexPolygon2D foothold = snapData.getCroppedFoothold(); if (!foothold.isEmpty()) plan.getFootstep(i - 1).setFoothold(foothold); } plan.setLowLevelPlanGoal(goalPoseInWorld); return plan; }
footstepPose.setTranslationX(midFootPoint.getX()); footstepPose.setTranslationY(midFootPoint.getY()); snapTransform.transform(footstepPose);
/** {@inheritDoc} */ @Override default void getJointConfiguration(RigidBodyTransform jointTransform) { jointTransform.setRotationToZero(); jointTransform.setTranslationX(getQ() * getJointAxis().getX()); jointTransform.setTranslationY(getQ() * getJointAxis().getY()); jointTransform.setTranslationZ(getQ() * getJointAxis().getZ()); } }
private static void addNodeDataToFootstepPlan(FootstepPlan footstepPlan, FootstepNodeDataMessage nodeData) { RobotSide robotSide = RobotSide.fromByte(nodeData.getRobotSide()); RigidBodyTransform footstepPose = new RigidBodyTransform(); footstepPose.setRotationYawAndZeroTranslation(nodeData.getYawIndex() * FootstepNode.gridSizeYaw); footstepPose.setTranslationX(nodeData.getXIndex() * FootstepNode.gridSizeXY); footstepPose.setTranslationY(nodeData.getYIndex() * FootstepNode.gridSizeXY); RigidBodyTransform snapTransform = new RigidBodyTransform(); snapTransform.set(nodeData.getSnapRotation(), nodeData.getSnapTranslation()); snapTransform.transform(footstepPose); footstepPlan.addFootstep(robotSide, new FramePose3D(ReferenceFrame.getWorldFrame(), footstepPose)); }
actualTransform.setTranslationX(expectedTranslation.getX()); actualTransform.setTranslationY(expectedTranslation.getY()); actualTransform.setTranslationZ(expectedTranslation.getZ()); EuclidCoreTestTools.assertTuple3DEquals(expectedTranslation, actualTransform.getTranslationVector(), 0.0); actualTransform.setTranslationX(0.0); actualTransform.setTranslationY(0.0); actualTransform.setTranslationZ(0.0); assertFalse(actualTransform.hasTranslation()); actualTransform.setTranslationX(1.0); assertTrue(actualTransform.hasTranslation()); actualTransform.setTranslationX(0.0); assertFalse(actualTransform.hasTranslation());