/** * Sets the pose, i.e. position and orientation, of this shape. * * @param pose pose holding the new position and orientation for this shape. Not modified. */ public final void setPose(Pose3DReadOnly pose) { pose.get(shapePose); }
/** * Computes the snap transform which snaps the given node to the given pose * * @param node * @param footstepPose * @return */ public static RigidBodyTransform computeSnapTransform(FootstepNode node, Pose3DReadOnly footstepPose) { RigidBodyTransform snapTransform = new RigidBodyTransform(); RigidBodyTransform stepTransform = new RigidBodyTransform(); footstepPose.get(stepTransform); FootstepNodeTools.getNodeTransform(node, snapTransform); snapTransform.preMultiplyInvertThis(stepTransform); return snapTransform; }